Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-30 03:27:33 +08:00
Code Issues Actions 29 Packages Projects Releases Wiki Activity

Merge pull request #1473 from rmeertens/pocketavoidfix

Browse Source 
Follow me based on stereo camera
This commit is contained in:
Gautier Hattenberger
2015-12-12 17:56:26 +01:00
parent f45ece5345 6676015708
commit fa487f5bda
14 changed files with 511 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/TUDELFT/tudelft_rm_conf.xml
+13 -2
View File
@@ -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"
/>
<aircraft
name="ardrone2_follow_me"
ac_id="2"
airframe="airframes/TUDELFT/tudelft_selfie.xml"
radio="radios/dummy.xml"
telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/rotorcraft_oa_avoid.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="modules/obstacle_avoidance.xml"
settings_modules=""
gui_color="red"
/>
<aircraft
@@ -16,7 +27,7 @@
airframe="airframes/BR/ladybird_kit_bart_bluegiga_optitrack.xml"
radio="radios/cockpitSX.xml"
telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/rotorcraft_optitrack_stereoavoid.xml"
flight_plan="flight_plans/rotorcraft_optitrack.xml"
settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_att_int.xml settings/control/stabilization_rate.xml"
settings_modules=""
gui_color="blue"
conf/TUDELFT/tudelft_rm_control_panel.xml
+14 -1
View File
@@ -130,6 +130,19 @@
<arg flag="-ac" constant="@AIRCRAFT"/>
</program>
</session>
<session name="Ardrone">
<program name="Data Link">
<arg flag="-udp"/>
</program>
<program name="Server"/>
<program name="GCS"/>
<program name="Joystick" command="sw/ground_segment/joystick/input2ivy">
<arg flag="-ac" constant="@AIRCRAFT"/>
<arg flag="hobbyking.xml"/>
</program>
</session>
<session name="Flight XBee @57600">
<program name="Data Link">
<arg flag="-d" constant="/dev/paparazzi/xbee"/>
@@ -210,4 +223,4 @@
</program>
</session>
</section>
</control_panel>
</control_panel>
conf/airframes/BR/ladybird_kit_bart_bluegiga.xml
-2
View File
@@ -195,8 +195,6 @@
<modules main_freq="512">
<load name="gps_ubx_ucenter.xml"/>
<load name="send_imu_mag_current.xml"/>
<load name="stereocam_nav_line_avoid.xml"/>
<load name="mission_rotorcraft.xml"/>
</modules>
<firmware name="rotorcraft">
conf/airframes/BR/ladybird_kit_bart_bluegiga_optitrack.xml
+2 -8
View File
@@ -79,7 +79,7 @@
<define name="MODE_STARTUP" value="AP_MODE_NAV"/>
<define name="MODE_MANUAL" value="AP_MODE_ATTITUDE_DIRECT"/>
<define name="MODE_AUTO1" value="AP_MODE_ATTITUDE_Z_HOLD"/>
<define name="MODE_AUTO2" value="AP_MODE_NAV"/>
<define name="MODE_AUTO2" value="AP_MODE_HOVER_Z_HOLD"/>
<define name="USE_THROTTLE_FOR_MOTOR_ARMING" value="TRUE"/>
<define name="NO_RC_THRUST_LIMIT" value="TRUE"/>
@@ -210,12 +210,11 @@
<modules main_freq="512">
<load name="send_imu_mag_current.xml"/>
<load name="stereocam_nav_line_avoid.xml"/>
<load name="mission_rotorcraft.xml"/>
<load name="stereocam.xml">
<configure name="STEREO_UART" value="UART1"/>
<configure name="STEREO_BAUD" value="B115200"/>
</load>
<load name="stereocam_forward_velocity.xml"/>
</modules>
<firmware name="rotorcraft">
@@ -227,11 +226,6 @@
<define name="USE_PERSISTENT_SETTINGS" value="TRUE"/>
</target>
<target name="nps" board="pc">
<subsystem name="fdm" type="jsbsim"/>
<subsystem name="radio_control" type="ppm"/>
</target>
<subsystem name="motor_mixing"/>
<subsystem name="actuators" type="pwm">
<!-- This airframe is using the servo PWM outputs directly to drive mosfets, which in turn directly drive brushed motors.
conf/airframes/TUDELFT/tudelft_ardrone2_oa_clint_roland.xml
+4 -2
View File
@@ -18,6 +18,8 @@
<subsystem name="motor_mixing"/>
<subsystem name="actuators" type="ardrone2"/>
<subsystem name="imu" type="ardrone2"/>
<subsystem name="gps" type="datalink"/>
<subsystem name="stabilization" type="int_quat"/>
<subsystem name="ahrs" type="int_cmpl_quat"/>
<subsystem name="ins" type="extended"/>
@@ -27,9 +29,9 @@
<load name="bat_voltage_ardrone2.xml"/>
<load name="stereocam.xml">
<configure name="STEREO_UART" value="UART1"/>
<configure name="STEREO_BAUD" value="B1000000"/>
<configure name="STEREO_BAUD" value="B115200"/>
</load>
<load name="obstacle_avoidance.xml"/>
<load name="stereocam_forward_velocity.xml"/>
</modules>
<commands>
conf/airframes/TUDELFT/tudelft_selfie.xml
+243
View File
@@ -0,0 +1,243 @@
<!DOCTYPE airframe SYSTEM "../airframe.dtd">
<airframe name="ardrone2_oa_avoid">
<firmware name="rotorcraft">
<target name="ap" board="ardrone2">
<configure name="USE_BARO_BOARD" value="TRUE"/>
</target>
<define name="FAILSAFE_DESCENT_SPEED" value="0.5"/>
<define name="USE_SONAR" value="FALSE"/>
<define name="USE_GPS_ALT" value="1"/>
<configure name="USE_MAGNETOMETER" value="TRUE"/>
<!-- Subsystem section -->
<subsystem name="telemetry" type="transparent_udp"/>
<subsystem name="radio_control" type="datalink"/>
<subsystem name="motor_mixing"/>
<subsystem name="actuators" type="ardrone2"/>
<subsystem name="imu" type="ardrone2"/>
<subsystem name="gps" type="datalink"/>
<subsystem name="stabilization" type="int_quat"/>
<subsystem name="ahrs" type="int_cmpl_quat"/>
<subsystem name="ins" type="extended"/>
</firmware>
<modules main_freq="512">
<load name="bat_voltage_ardrone2.xml"/>
<load name="stereocam.xml">
<configure name="STEREO_UART" value="UART1"/>
<configure name="STEREO_BAUD" value="B115200"/>
</load>
<load name="stereocam_follow_me.xml"/>
</modules>
<commands>
<axis name="PITCH" failsafe_value="0"/>
<axis name="ROLL" failsafe_value="0"/>
<axis name="YAW" failsafe_value="0"/>
<axis name="THRUST" failsafe_value="3000"/>
</commands>
<servos driver="Default">
<servo name="TOP_LEFT" no="0" min="0" neutral="1" max="500"/>
<servo name="TOP_RIGHT" no="1" min="0" neutral="1" max="500"/>
<servo name="BOTTOM_RIGHT" no="2" min="0" neutral="1" max="500"/>
<servo name="BOTTOM_LEFT" no="3" min="0" neutral="1" max="500"/>
</servos>
<section name="MIXING" prefix="MOTOR_MIXING_">
<define name="TRIM_ROLL" value="0"/>
<define name="TRIM_PITCH" value="0"/>
<define name="TRIM_YAW" value="0"/>
<!-- Time cross layout (X), with order NW (CW), NE (CCW), SE (CW), SW (CCW) -->
<define name="TYPE" value="QUAD_X"/>
</section>
<command_laws>
<call fun="motor_mixing_run(autopilot_motors_on,FALSE,values)"/>
<set servo="TOP_LEFT" value="motor_mixing.commands[MOTOR_FRONT_LEFT]"/>
<set servo="TOP_RIGHT" value="motor_mixing.commands[MOTOR_FRONT_RIGHT]"/>
<set servo="BOTTOM_RIGHT" value="motor_mixing.commands[MOTOR_BACK_RIGHT]"/>
<set servo="BOTTOM_LEFT" value="motor_mixing.commands[MOTOR_BACK_LEFT]"/>
</command_laws>
<section name="IMU" prefix="IMU_">
<!-- Accelero -->
<define name="ACCEL_X_NEUTRAL" value="2048"/>
<define name="ACCEL_Y_NEUTRAL" value="2048"/>
<define name="ACCEL_Z_NEUTRAL" value="2048"/>
<!-- Magneto calibration -->
<define name="MAG_X_NEUTRAL" value="0"/>
<define name="MAG_Y_NEUTRAL" value="0"/>
<define name="MAG_Z_NEUTRAL" value="-180"/>
<define name="MAG_X_SENS" value="16." integer="16"/>
<define name="MAG_Y_SENS" value="16." integer="16"/>
<define name="MAG_Z_SENS" value="16." integer="16"/>
<!-- Magneto current calibration -->
<define name="MAG_X_CURRENT_COEF" value="0.0"/>
<define name="MAG_Y_CURRENT_COEF" value="0.0"/>
<define name="MAG_Z_CURRENT_COEF" value="0.0"/>
<define name="BODY_TO_IMU_PHI" value="-2.3" unit="deg"/>
<define name="BODY_TO_IMU_THETA" value="1.7" unit="deg"/>
<define name="BODY_TO_IMU_PSI" value="0." unit="deg"/>
</section>
<!-- local magnetic field -->
<!-- http://wiki.paparazziuav.org/wiki/Subsystem/ahrs#Local_Magnetic_Field -->
<section name="AHRS" prefix="AHRS_">
<!-- Delft -->
<define name="H_X" value="0.3892503"/>
<define name="H_Y" value="0.0017972"/>
<define name="H_Z" value="0.9211303"/>
</section>
<section name="INS" prefix="INS_">
</section>
<section name="STABILIZATION_RATE" prefix="STABILIZATION_RATE_">
<!-- setpoints -->
<define name="SP_MAX_P" value="10000"/>
<define name="SP_MAX_Q" value="10000"/>
<define name="SP_MAX_R" value="10000"/>
<define name="DEADBAND_P" value="20"/>
<define name="DEADBAND_Q" value="20"/>
<define name="DEADBAND_R" value="200"/>
<!-- feedback -->
<define name="GAIN_P" value="400"/>
<define name="GAIN_Q" value="400"/>
<define name="GAIN_R" value="350"/>
<define name="IGAIN_P" value="75"/>
<define name="IGAIN_Q" value="75"/>
<define name="IGAIN_R" value="50"/>
</section>
<section name="STABILIZATION_ATTITUDE_INDI" prefix="STABILIZATION_INDI_">
<!-- control effectiveness -->
<define name="G1_P" value="0.028551973"/>
<define name="G1_Q" value="0.023775417"/>
<define name="G1_R" value="0.00173069"/>
<define name="G2_R" value="0.086460732"/>
<!-- reference acceleration for attitude control -->
<define name="REF_ERR_P" value="380.0"/>
<define name="REF_ERR_Q" value="380.0"/>
<define name="REF_ERR_R" value="70.0"/>
<define name="REF_RATE_P" value="21.6"/>
<define name="REF_RATE_Q" value="21.6"/>
<define name="REF_RATE_R" value="11.0"/>
<!-- second order filter parameters -->
<define name="FILT_OMEGA" value="20.0"/>
<define name="FILT_ZETA" value="0.55"/>
<define name="FILT_OMEGA_R" value="20.0"/>
<define name="FILT_ZETA_R" value="0.55"/>
<!-- first order actuator dynamics -->
<define name="ACT_DYN_P" value="0.06"/>
<define name="ACT_DYN_Q" value="0.06"/>
<define name="ACT_DYN_R" value="0.06"/>
<!-- Adaptive Learning Rate -->
<define name="USE_ADAPTIVE" value="FALSE"/>
<define name="ADAPTIVE_MU" value="0.0001"/>
</section>
<section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
<!-- setpoints -->
<define name="SP_MAX_PHI" value="45" unit="deg"/>
<define name="SP_MAX_THETA" value="45" unit="deg"/>
<define name="SP_MAX_R" value="180" unit="deg/s"/>
<define name="DEADBAND_A" value="0"/>
<define name="DEADBAND_E" value="0"/>
<define name="DEADBAND_R" value="250"/>
<!-- reference -->
<define name="REF_OMEGA_P" value="450" unit="deg/s"/>
<define name="REF_ZETA_P" value="0.9"/>
<define name="REF_MAX_P" value="600." unit="deg/s"/>
<define name="REF_MAX_PDOT" value="RadOfDeg(8000.)"/>
<define name="REF_OMEGA_Q" value="450" unit="deg/s"/>
<define name="REF_ZETA_Q" value="0.9"/>
<define name="REF_MAX_Q" value="600." unit="deg/s"/>
<define name="REF_MAX_QDOT" value="RadOfDeg(8000.)"/>
<define name="REF_OMEGA_R" value="450" unit="deg/s"/>
<define name="REF_ZETA_R" value="0.9"/>
<define name="REF_MAX_R" value="600." unit="deg/s"/>
<define name="REF_MAX_RDOT" value="RadOfDeg(8000.)"/>
<!-- feedback -->
<define name="PHI_PGAIN" value="850"/>
<define name="PHI_DGAIN" value="425"/>
<define name="PHI_IGAIN" value="0"/>
<define name="THETA_PGAIN" value="850"/>
<define name="THETA_DGAIN" value="425"/>
<define name="THETA_IGAIN" value="0"/>
<define name="PSI_PGAIN" value="1000"/>
<define name="PSI_DGAIN" value="700"/>
<define name="PSI_IGAIN" value="0"/>
<!-- feedforward -->
<define name="PHI_DDGAIN" value="0"/>
<define name="THETA_DDGAIN" value="0"/>
<define name="PSI_DDGAIN" value="100"/>
</section>
<section name="GUIDANCE_V" prefix="GUIDANCE_V_">
<define name="HOVER_KP" value="583"/>
<define name="HOVER_KD" value="82"/>
<define name="HOVER_KI" value="13"/>
<define name="NOMINAL_HOVER_THROTTLE" value="0.645"/>
<define name="ADAPT_THROTTLE_ENABLED" value="false"/>
</section>
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<!-- Good weather -->
<define name="MAX_BANK" value="20" unit="deg"/>
<!-- Bad weather -->
<!-- define name="MAX_BANK" value="32" unit="deg"/ -->
<define name="PGAIN" value="363"/>
<define name="DGAIN" value="237"/>
<define name="IGAIN" value="30"/>
<define name="VGAIN" value="0"/>
<define name="AGAIN" value="0"/>
</section>
<section name="SIMULATOR" prefix="NPS_">
<define name="ACTUATOR_NAMES" value="{&quot;nw_motor&quot;, &quot;ne_motor&quot;, &quot;se_motor&quot;, &quot;sw_motor&quot;}"/>
<define name="JSBSIM_MODEL" value="&quot;simple_ardrone2&quot;"/>
<define name="JSBSIM_INIT" value="&quot;reset00&quot;"/>
<define name="SENSORS_PARAMS" value="&quot;nps_sensors_params_ardrone2.h&quot;"/>
</section>
<section name="AUTOPILOT">
<define name="MODE_STARTUP" value="AP_MODE_NAV"/>
<define name="MODE_MANUAL" value="AP_MODE_ATTITUDE_DIRECT"/>
<define name="MODE_AUTO1" value="AP_MODE_HOVER_Z_HOLD"/>
<define name="MODE_AUTO2" value="AP_MODE_NAV"/>
<define name="ARRIVED_AT_WAYPOINT" value="0.15"/>
<define name="AUTOPILOT_IN_FLIGHT_MIN_THRUST" value="1500"/>
<define name="NO_RC_THRUST_LIMIT" value="TRUE"/>
</section>
<section name="BAT">
<define name="MILLIAMP_AT_FULL_THROTTLE" value="8700"/>
<define name="CATASTROPHIC_BAT_LEVEL" value="9.3" unit="V"/>
<define name="CRITIC_BAT_LEVEL" value="9.9" unit="V"/>
<define name="LOW_BAT_LEVEL" value="10.1" unit="V"/>
<define name="MAX_BAT_LEVEL" value="12.4" unit="V"/>
</section>
</airframe>
conf/flight_plans/TUDELFT/rotorcraft_selfie.xml
+32
View File
@@ -0,0 +1,32 @@
<!DOCTYPE flight_plan SYSTEM "../flight_plan.dtd">
<flight_plan alt="2" ground_alt="0" lat0="43 33 50.83" lon0="1 28 52.61" max_dist_from_home="150" name="Rotorcraft Basic (Enac)" security_height="2">
<header>
#include "autopilot.h"
</header>
<waypoints>
<waypoint name="HOME" x="0.0" y="0.0"/>
<waypoint name="CLIMB" x="0.0" y="5.0"/>
<waypoint name="STDBY" x="-2.0" y="-5.0"/>
<waypoint name="p1" x="3.6" y="-13.9"/>
<waypoint name="p2" x="27.5" y="-48.2"/>
<waypoint name="p3" x="16.7" y="-19.6"/>
<waypoint name="p4" x="13.7" y="-40.7"/>
<waypoint name="CAM" x="-20" y="-50" height="2."/>
<waypoint name="TD" x="5.6" y="-10.9"/>
</waypoints>
<blocks>
<block name="Wait GPS">
<call fun="NavKillThrottle()"/>
<while cond="!GpsFixValid()"/>
</block>
<block name="Geo init">
<while cond="LessThan(NavBlockTime(), 10)"/>
<call fun="NavSetGroundReferenceHere()"/>
</block>
<block name="avoid">
<attitude pitch="(ref_pitch)" roll="(ref_roll)" alt="2.5" vmode="alt" until="FALSE"/>
</block>
</blocks>
</flight_plan>
conf/flight_plans/rotorcraft_optitrack_stereoavoid.xml
+2 -2
View File
@@ -48,7 +48,7 @@
<attitude pitch="0" roll="0" throttle="0" until="stage_time > 1" vmode="throttle"/>
</block>
<block name="Takeoff">
<exception cond="stateGetPositionEnu_f()->z > 1.5" deroute="Standby"/>
<exception cond="stateGetPositionEnu_f()->z > 0.5" deroute="Standby"/>
<call fun="NavSetWaypointHere(WP_CLIMB)"/>
<stay climb="nav_climb_vspeed" vmode="climb" wp="CLIMB"/>
</block>
@@ -63,7 +63,7 @@
<exception cond="electrical.bat_low" deroute="land here"/>
<call fun="nav_set_heading_towards_waypoint(WP_GOAL)"/>
<call fun="NavSetWaypointHere(WP_route_START)"/>
<stay until="stage_time > 4" wp="route_START"/>
<stay until="stage_time > 3" wp="route_START"/>
</block>
<block name="route_to_GOAL">
<exception cond="GpsIsLost()" deroute="attitude_land"/>
conf/messages.xml
+8 -1
View File
@@ -887,7 +887,14 @@
<field name="servo" type="uint16[]"/>
</message>
<!--112 is free -->
<message name="STEREO_VELOCITY" id="112">
<field name="disparity" type="uint8"/>
<field name="distance" type="float"/>
<field name="velocity" type="float"/>
<field name="velocitySideways" type="float"/>
<field name="velocityUpDown" type="float"/>
</message>
<message name="MLX_SERIAL" id="113">
<field name="serial0" type="uint32"/>
conf/modules/stereocam_follow_me.xml
+18
View File
@@ -0,0 +1,18 @@
<!DOCTYPE module SYSTEM "module.dtd">
<module name="stereocam_follow_me" dir="stereocam">
<doc>
<description> 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.
</description>
</doc>
<header>
<file name="stereocam_follow_me/follow_me.h"/>
</header>
<periodic fun="follow_me_periodic()" freq="512"/>
<makefile>
<file name="stereocam_follow_me/follow_me.c" />
</makefile>
</module>
conf/telemetry/default_rotorcraft.xml
+1
View File
@@ -26,6 +26,7 @@
<message name="AIR_DATA" period="1.3"/>
<message name="SURVEY" period="2.5"/>
<message name="OPTIC_FLOW_EST" period="0.25"/>
<message name="STEREO_IMG" period="0.25"/>
<message name="VECTORNAV_INFO" period="0.5"/>
</mode>
sw/airborne/modules/stereocam/stereocam_follow_me/follow_me.c
+145
View File
@@ -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;
}
}
}
sw/airborne/modules/stereocam/stereocam_follow_me/follow_me.h
+23
View File
@@ -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
sw/airborne/subsystems/abi_sender_ids.h
+6
View File
@@ -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 */