diff --git a/conf/TUDELFT/tudelft_rm_conf.xml b/conf/TUDELFT/tudelft_rm_conf.xml index 6d2f46b2f6..444c6cf9d0 100644 --- a/conf/TUDELFT/tudelft_rm_conf.xml +++ b/conf/TUDELFT/tudelft_rm_conf.xml @@ -5,9 +5,20 @@ airframe="airframes/TUDELFT/tudelft_ardrone2_oa_clint_roland.xml" radio="radios/dummy.xml" telemetry="telemetry/default_rotorcraft.xml" + flight_plan="flight_plans/rotorcraft_optitrack.xml" + settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml [settings/control/stabilization_att_indi.xml] settings/control/stabilization_rate.xml settings/control/stabilization_att_int_quat.xml" + settings_modules="" + gui_color="red" + /> + + + + + + + + + + + + + + @@ -210,4 +223,4 @@ - \ No newline at end of file + diff --git a/conf/airframes/BR/ladybird_kit_bart_bluegiga.xml b/conf/airframes/BR/ladybird_kit_bart_bluegiga.xml index 0ccf73558a..629f5812b8 100644 --- a/conf/airframes/BR/ladybird_kit_bart_bluegiga.xml +++ b/conf/airframes/BR/ladybird_kit_bart_bluegiga.xml @@ -195,8 +195,6 @@ - - diff --git a/conf/airframes/BR/ladybird_kit_bart_bluegiga_optitrack.xml b/conf/airframes/BR/ladybird_kit_bart_bluegiga_optitrack.xml index e2306c2ace..312fe4ad0d 100644 --- a/conf/airframes/BR/ladybird_kit_bart_bluegiga_optitrack.xml +++ b/conf/airframes/BR/ladybird_kit_bart_bluegiga_optitrack.xml @@ -79,7 +79,7 @@ - + @@ -210,12 +210,11 @@ - - + @@ -227,11 +226,6 @@ - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ + + + + + +
+ + + + + + + + + +
+ + + + + + + + + + + + + + + + + + + + + +
+ + + +
+ + + + +
+ +
+
+ +
+ + + + + + + + + + + + + + + + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + + +
+ + +
+ + + + + + + + + + +
+ +
+ + + + +
+ +
+ + + + + + + +
+ +
+ + + + + +
+ diff --git a/conf/flight_plans/TUDELFT/rotorcraft_selfie.xml b/conf/flight_plans/TUDELFT/rotorcraft_selfie.xml new file mode 100644 index 0000000000..e1a9040cdc --- /dev/null +++ b/conf/flight_plans/TUDELFT/rotorcraft_selfie.xml @@ -0,0 +1,32 @@ + + + +
+#include "autopilot.h" +
+ + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/conf/flight_plans/rotorcraft_optitrack_stereoavoid.xml b/conf/flight_plans/rotorcraft_optitrack_stereoavoid.xml index 2e49cca424..ee1e82c0f8 100644 --- a/conf/flight_plans/rotorcraft_optitrack_stereoavoid.xml +++ b/conf/flight_plans/rotorcraft_optitrack_stereoavoid.xml @@ -48,7 +48,7 @@ - + @@ -63,7 +63,7 @@ - + diff --git a/conf/messages.xml b/conf/messages.xml index 26e11c916e..7756ace64b 100644 --- a/conf/messages.xml +++ b/conf/messages.xml @@ -887,7 +887,14 @@ - + + + + + + + + diff --git a/conf/modules/stereocam_follow_me.xml b/conf/modules/stereocam_follow_me.xml new file mode 100644 index 0000000000..feb92eae24 --- /dev/null +++ b/conf/modules/stereocam_follow_me.xml @@ -0,0 +1,18 @@ + + + + + Follows a person using the reference given by the stereocam. + This module does so by changing the yaw angle and roll angle alternatively. + This way the drone does not drift away, and keeps looking at the person it tries to follow. + + +
+ +
+ + + + + + diff --git a/conf/telemetry/default_rotorcraft.xml b/conf/telemetry/default_rotorcraft.xml index b358163be4..a06e2b97b9 100644 --- a/conf/telemetry/default_rotorcraft.xml +++ b/conf/telemetry/default_rotorcraft.xml @@ -26,6 +26,7 @@ + diff --git a/sw/airborne/modules/stereocam/stereocam_follow_me/follow_me.c b/sw/airborne/modules/stereocam/stereocam_follow_me/follow_me.c new file mode 100644 index 0000000000..8efa9e398b --- /dev/null +++ b/sw/airborne/modules/stereocam/stereocam_follow_me/follow_me.c @@ -0,0 +1,145 @@ +/* + * Copyright (C) Roland + * + * This file is part of paparazzi + * + */ +/** + * @file modules/stereocam/stereocam_follow_me/stereocam_follow_me.c + * @author Roland + * Follows a person using the reference given by the stereocam. + * This module does so by changing the yaw angle and roll angle alternatively. + * This way the drone does not drift away, and keeps looking at the person it tries to follow. + */ + +#include "modules/stereocam/stereocam_follow_me/follow_me.h" +#include "modules/stereocam/stereocam.h" +#include "state.h" +#include "navigation.h" +#include "subsystems/datalink/telemetry.h" +#include "generated/flight_plan.h" + +#ifndef STEREOCAM_FOLLOW_ME_USE_OPTITRACK +#define STEREOCAM_FOLLOW_ME_USE_OPTITRACK FALSE +#endif + + +#define HEADING_CHANGE_PER_MEASUREMENT 260 +#define CENTER_IMAGE_HOR 65 +#define MAXIMUM_ALTITUDE_FOLLOWING 3.0 +#define MINIMUM_ALTITUDE_FOLLOWING 1.0 +float ref_pitch = 0.0; +float ref_roll = 0.0; +float selfie_alt = 1.0; +int breaksPoints = 0; +int isRollPhase = 0; +int isYawPhase = 0; +int phaseCounter = 0; +float heightGain = 0.3; +int amountOfRollPhaseTime = 15; +int amountOfYawPhaseTime = 15; +uint8_t distanceToObject; +uint8_t heightObject; + +void changeRollYawPhase(int *phaseCounterArg, int *isRollPhaseArg, int *isYawPhaseArg); +void changeRollYawPhase(int *phaseCounterArg, int *isRollPhaseArg, int *isYawPhaseArg) +{ + (*phaseCounterArg)++; + + if (*isRollPhaseArg) { + if (*phaseCounterArg > amountOfRollPhaseTime) { + *phaseCounterArg = 0; + *isRollPhaseArg = 0; + *isYawPhaseArg = 1; + } + } else { + if (*phaseCounterArg > amountOfYawPhaseTime) { + *phaseCounterArg = 0; + *isRollPhaseArg = 1; + *isYawPhaseArg = 0; + } + } +} + +void increase_nav_heading(int32_t *heading, int32_t increment); +void increase_nav_heading(int32_t *heading, int32_t increment) +{ + *heading = *heading + increment; +} +void follow_me_periodic() +{ + if (stereocam_data.fresh) { + stereocam_data.fresh = 0; + if (stereocam_data.data[0] == 0 || stereocam_data.data[1] == 85 || stereocam_data.data[2] == 0) { + return; + } + + // If we don't use GPS we alternate a phase where we roll and where we yaw. + // This way we don't drift sideways AND we don't lose the user out of our sight + changeRollYawPhase(&phaseCounter, &isRollPhase, &isYawPhase); + uint8_t headingToFollow = stereocam_data.data[0]; + heightObject = stereocam_data.data[1]; + distanceToObject = stereocam_data.data[2]; + + + // Change our heading if the user is starting to get out of sight. + float heading_change = 0.0; + int headingChangePerIt = HEADING_CHANGE_PER_MEASUREMENT; + if (abs(headingToFollow - CENTER_IMAGE_HOR) > 10) { + if (headingToFollow > CENTER_IMAGE_HOR) { + heading_change = 0.25; + if (isYawPhase || STEREOCAM_FOLLOW_ME_USE_OPTITRACK) { + increase_nav_heading(&nav_heading, headingChangePerIt); + } + } else if (headingToFollow < CENTER_IMAGE_HOR) { + heading_change = -0.25; + if (isYawPhase || STEREOCAM_FOLLOW_ME_USE_OPTITRACK) { + increase_nav_heading(&nav_heading, -1 * headingChangePerIt); + } + } else { + heading_change = 0.0; + } + + } else { + heading_change = 0.0; + } + + // If we have our roll phase we take the value of the change we need to have in heading and use it to go sideways + ref_roll = 0.0; + if (isRollPhase) { + ref_roll = 30 * heading_change; + } + + + // If we are in flight we want to adjust our height based on the place where we see our object + if (nav_is_in_flight()) { + if (heightObject > 50) { + selfie_alt -= heightGain; + } else if (heightObject < 20) { + selfie_alt += heightGain; + } + } + + // Bound the altitude to normal values + if (selfie_alt > MAXIMUM_ALTITUDE_FOLLOWING) { + selfie_alt = MAXIMUM_ALTITUDE_FOLLOWING; + } + if (selfie_alt < MINIMUM_ALTITUDE_FOLLOWING) { + selfie_alt = MINIMUM_ALTITUDE_FOLLOWING; + } + + // If using GPS we set the location of the waypoint to our desired altitude +#if STEREOCAM_FOLLOW_ME_USE_OPTITRACK + waypoint_set_alt(WP_STDBY, selfie_alt); +#endif + + // Set a pitch if the person we follow is too close + if (distanceToObject < 35) { + ref_pitch = 13.0; + } else if (distanceToObject < 60) { + ref_pitch = 5.0; + } else { + ref_pitch = -2.0; + } + } +} diff --git a/sw/airborne/modules/stereocam/stereocam_follow_me/follow_me.h b/sw/airborne/modules/stereocam/stereocam_follow_me/follow_me.h new file mode 100644 index 0000000000..be6293f73b --- /dev/null +++ b/sw/airborne/modules/stereocam/stereocam_follow_me/follow_me.h @@ -0,0 +1,23 @@ +/* + * Copyright (C) Roland + * + * This file is part of paparazzi + * + */ +/** + * @file modules/stereocam/stereocam_follow_me/stereocam_follow_me.h + * @author Roland + * Follows a person using the reference given by the stereocam. + * This module does so by changing the yaw angle and roll angle alternatively. + * This way the drone does not drift away, and keeps looking at the person it tries to follow. + */ + +#ifndef FOLLOW_ME_H +#define FOLLOW_ME_H +extern float ref_pitch; +extern float ref_roll; +extern float selfie_alt; +extern void follow_me_periodic(void); + +#endif + diff --git a/sw/airborne/subsystems/abi_sender_ids.h b/sw/airborne/subsystems/abi_sender_ids.h index 0750518d7b..1b67b3c6fa 100644 --- a/sw/airborne/subsystems/abi_sender_ids.h +++ b/sw/airborne/subsystems/abi_sender_ids.h @@ -230,5 +230,11 @@ #define PX4FLOW_VELOCITY_ID 17 #endif +#ifndef STEREO_VELOCITY_ID +#define STEREO_VELOCITY_ID 18 +#endif + + + #endif /* ABI_SENDER_IDS_H */