mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-09 22:49:53 +08:00
Merge pull request #2931 from enacuavlab/tag_tracking_speed-integration
Tag tracking speed integration
This commit is contained in:
Gautier Hattenberger
GitHub
@@ -72,17 +72,20 @@
|
||||
|
||||
<section name="IMU" prefix="IMU_">
|
||||
|
||||
<define name="GYRO_P_SIGN" value="1"/>
|
||||
<!--define name="GYRO_P_SIGN" value="1"/>
|
||||
<define name="GYRO_Q_SIGN" value="-1"/>
|
||||
<define name="GYRO_R_SIGN" value="-1"/> <!-- ugly hack for dead accel -->
|
||||
<define name="GYRO_R_SIGN" value="-1"/>
|
||||
|
||||
<define name="ACCEL_X_SIGN" value="1"/>
|
||||
<define name="ACCEL_Y_SIGN" value="-1"/>
|
||||
<define name="ACCEL_Z_SIGN" value="-1"/>
|
||||
<define name="ACCEL_Z_SIGN" value="-1"/-->
|
||||
|
||||
<define name="BODY_TO_IMU_PHI" value="0." unit="deg"/>
|
||||
<define name="BODY_TO_IMU_THETA" value="180." unit="deg"/>
|
||||
<define name="BODY_TO_IMU_PSI" value="-8." unit="deg"/>
|
||||
<!--define name="BODY_TO_IMU_PHI" value="0." unit="deg"/>
|
||||
<define name="BODY_TO_IMU_THETA" value="0." unit="deg"/>
|
||||
<define name="BODY_TO_IMU_PSI" value="0." unit="deg"/>
|
||||
<define name="BODY_TO_IMU_PSI" value="-8." unit="deg"/-->
|
||||
</section>
|
||||
|
||||
<section name="AHRS" prefix="AHRS_">
|
||||
@@ -135,7 +138,7 @@
|
||||
<define name="ALT_SHIFT_PLUS_PLUS" value="5"/>
|
||||
<define name="ALT_SHIFT_PLUS" value="1"/>
|
||||
<define name="ALT_SHIFT_MINUS" value="-1"/>
|
||||
<define name="AC_ICON" value="quadrotor_x"/>
|
||||
<define name="AC_ICON" value="rover"/>
|
||||
</section>
|
||||
|
||||
</airframe>
|
||||
|
||||
@@ -0,0 +1,182 @@
|
||||
<!DOCTYPE flight_plan SYSTEM "../flight_plan.dtd">
|
||||
|
||||
<flight_plan alt="148" ground_alt="146" lat0="43.5640917" lon0="1.4829201" wp_frame="ltp" max_dist_from_home="20" name="Drop on Tag (ENAC indoor)" security_height="0.3">
|
||||
<header>
|
||||
#ifndef SwitchServoOn
|
||||
#define SwitchServoOn(_x) {}
|
||||
#endif
|
||||
#ifndef SwitchServoOff
|
||||
#define SwitchServoOff(_x) {}
|
||||
#endif
|
||||
#define DropOpen SwitchServoOff
|
||||
#define DropClose SwitchServoOn
|
||||
|
||||
#ifdef NAV_C
|
||||
#ifndef TAG_TRACKING_COORD_TO_M
|
||||
#define TAG_TRACKING_COORD_TO_M (1.f / 1000.f)
|
||||
#endif
|
||||
|
||||
static void fp_tag_cb(uint8_t sender_id UNUSED,
|
||||
uint8_t type, char * id UNUSED,
|
||||
uint8_t nb UNUSED, int16_t * coord, uint16_t * dim UNUSED,
|
||||
struct FloatQuat quat UNUSED, char * extra UNUSED)
|
||||
{
|
||||
if (type == JEVOIS_MSG_D3) {
|
||||
tag_distance = coord[2] * TAG_TRACKING_COORD_TO_M;
|
||||
tag_valid = true;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
#define Neq(a,b) (a!=b)
|
||||
</header>
|
||||
<waypoints>
|
||||
<waypoint name="HOME" x="0.0" y="0.0"/>
|
||||
<waypoint name="STDBY" x="-0.7" y="-0.8"/>
|
||||
<waypoint name="TAG" x="-1.0" y="-1.0"/>
|
||||
<waypoint name="TARGET" x="2.0" y="1.0"/>
|
||||
<waypoint name="ROVER" x="-2.0" y="2.0"/>
|
||||
<waypoint name="TD" x="0.8" y="-1.7"/>
|
||||
<waypoint name="S1" x="2" y="3."/>
|
||||
<waypoint name="S2" x="2" y="-3."/>
|
||||
<waypoint name="S3" x="-3" y="-3."/>
|
||||
<waypoint name="S4" x="-3" y="3."/>
|
||||
<waypoint name="_N1" x="4.5" y="5.2"/>
|
||||
<waypoint name="_N2" x="4.5" y="-5.2"/>
|
||||
<waypoint name="_N3" x="-4.5" y="-5.2"/>
|
||||
<waypoint name="_N4" x="-4.5" y="5.2"/>
|
||||
</waypoints>
|
||||
<sectors>
|
||||
<sector name="Net" color="red">
|
||||
<corner name="_N1"/>
|
||||
<corner name="_N2"/>
|
||||
<corner name="_N3"/>
|
||||
<corner name="_N4"/>
|
||||
</sector>
|
||||
<sector name="Survey" color="green">
|
||||
<corner name="S1"/>
|
||||
<corner name="S2"/>
|
||||
<corner name="S3"/>
|
||||
<corner name="S4"/>
|
||||
</sector>
|
||||
</sectors>
|
||||
<variables>
|
||||
<variable init="0.1" var="fp_throttle"/>
|
||||
<variable init="1." var="drop_height" min="0.2" max="5." step="0.1"/>
|
||||
<variable init="2." var="search_height" min="0.5" max="8." step="0.1"/>
|
||||
<variable init="42." var="tag_distance"/>
|
||||
<variable init="false" type="bool" var="tag_valid"/>
|
||||
<abi_binding name="JEVOIS_MSG" handler="fp_tag_cb"/>
|
||||
</variables>
|
||||
<modules>
|
||||
<module name="nav" type="survey_rectangle_rotorcraft"/>
|
||||
<module name="tag_tracking"/>
|
||||
<module name="follow">
|
||||
<define name="FOLLOW_AC_ID" value="51"/>
|
||||
<define name="FOLLOW_WAYPOINT_ID" value="WP_ROVER"/>
|
||||
<define name="FOLLOW_OFFSET_Z" value="3"/>
|
||||
</module>
|
||||
</modules>
|
||||
<blocks>
|
||||
<block name="Wait GPS">
|
||||
<call_once fun="NavKillThrottle()"/>
|
||||
<call_once fun="jevois_stream(false)"/>
|
||||
<call_once fun="DropOpen()"/>
|
||||
<while cond="!GpsFixValid()"/>
|
||||
</block>
|
||||
<block name="Holding point" group="home">
|
||||
<call_once fun="NavKillThrottle()"/>
|
||||
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
|
||||
</block>
|
||||
<block name="Start Engine">
|
||||
<call_once fun="NavResurrect()"/>
|
||||
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
|
||||
</block>
|
||||
<block name="Takeoff" strip_button="Takeoff" strip_icon="takeoff.png" group="home">
|
||||
<exception cond="stateGetPositionEnu_f() @DEREF z @GT 1.0" deroute="Standby"/>
|
||||
<call_once fun="DropClose()"/>
|
||||
<call_once fun="NavResurrect()"/>
|
||||
<attitude pitch="0" roll="0" throttle="0.1" vmode="throttle" until="stage_time @GT 2"/>
|
||||
<call_once fun="NavSetWaypointHere(WP_STDBY)"/>
|
||||
<stay vmode="climb" climb="nav_climb_vspeed" wp="STDBY"/>
|
||||
</block>
|
||||
<block name="Standby" strip_button="Standby" strip_icon="home.png" group="home">
|
||||
<call_once fun="jevois_stream(false)"/>
|
||||
<stay wp="STDBY"/>
|
||||
</block>
|
||||
|
||||
<block name="Search Tag" strip_button="Search Target" group="tag">
|
||||
<call_once fun="jevois_stream(true)"/>
|
||||
<call_once fun="nav_survey_rectangle_rotorcraft_setup(WP_S1, WP_S3, 1, NS)"/>
|
||||
<deroute block="Search run"/>
|
||||
</block>
|
||||
<block name="Search run">
|
||||
<exception cond="tag_tracking.status == TAG_TRACKING_RUNNING" deroute="Drop Package"/>
|
||||
<exception cond="rectangle_survey_sweep_num == 1" deroute="Standby"/>
|
||||
<call fun="nav_survey_rectangle_rotorcraft_run(WP_S1, WP_S3)"/>
|
||||
</block>
|
||||
|
||||
<block name="Follow Tag" strip_button="Follow Tag" group="tag">
|
||||
<call_once fun="jevois_stream(true)"/>
|
||||
<set var="tag_tracking.motion_type" value="TAG_TRACKING_MOVING"/>
|
||||
<stay wp="TAG" height="search_height"/>
|
||||
</block>
|
||||
<block name="Follow Guided" strip_button="Follow Guided" group="tag">
|
||||
<call_once fun="jevois_stream(true)"/>
|
||||
<set var="tag_tracking.motion_type" value="TAG_TRACKING_MOVING"/>
|
||||
<go wp="TAG" height="search_height"/>
|
||||
<guided flags="GUIDED_FLAG_XY_VEL" commands="tag_tracking.speed_cmd.x, tag_tracking.speed_cmd.y, -search_height, 0." until="Neq(tag_tracking.status, TAG_TRACKING_RUNNING)" pre_call="tag_tracking_compute_speed()"/>
|
||||
<deroute block="Standby"/>
|
||||
</block>
|
||||
|
||||
<block name="Drop Package" strip_button="Drop Package" group="tag">
|
||||
<call_once fun="jevois_stream(true)"/>
|
||||
<go wp="TAG" height="search_height"/>
|
||||
<stay wp="TAG" until="(stage_time @GT 3 @AND tag_tracking.status == TAG_TRACKING_RUNNING) @OR (stage_time @GT 15)" height="search_height"/>
|
||||
<stay wp="TAG" climb="nav_descend_vspeed" vmode="climb" until="((tag_distance @LT drop_height) @AND tag_valid) @OR (GetPosHeight() @LT drop_height/2.)" post_call="tag_valid = false"/>
|
||||
<call_once fun="DropOpen()"/>
|
||||
<stay wp="TAG" until="stage_time @GT 3" alt="search_height"/>
|
||||
<set var="tag_tracking.motion_type" value="TAG_TRACKING_FIXED_POS"/>
|
||||
<set var="follow_follow_wp_status" value="MODULES_STOP"/>
|
||||
<deroute block="Land"/>
|
||||
</block>
|
||||
|
||||
<block name="Goto Rover" strip_button="Goto Rover" group="tag">
|
||||
<set var="follow_follow_wp_status" value="MODULES_START"/>
|
||||
<go wp="ROVER"/>
|
||||
<call_once fun="jevois_stream(true)"/>
|
||||
<stay wp="ROVER" until="stage_time @GT 2"/>
|
||||
<deroute block="Drop Package"/>
|
||||
</block>
|
||||
|
||||
<block name="Land Tag" strip_button="Land Tag" group="tag">
|
||||
<exception cond="!nav_is_in_flight()" deroute="Kill landed"/>
|
||||
<exception cond="stateGetPositionEnu_f()->z @LT 0.2" deroute="Ramp down"/>
|
||||
<call_once fun="jevois_stream(true)"/>
|
||||
<stay climb="nav_descend_vspeed" vmode="climb" wp="TAG"/>
|
||||
</block>
|
||||
|
||||
<block name="Land Here" strip_button="Land Here" group="home">
|
||||
<call_once fun="NavSetWaypointHere(WP_TD)"/>
|
||||
</block>
|
||||
<block name="Land" strip_button="Land" strip_icon="land-right.png" group="home">
|
||||
<call_once fun="jevois_stream(false)"/>
|
||||
<go wp="TD"/>
|
||||
</block>
|
||||
<block name="Flare">
|
||||
<!--exception cond="NavDetectGround()" deroute="Holding point"/-->
|
||||
<exception cond="!nav_is_in_flight()" deroute="Kill landed"/>
|
||||
<!--call_once fun="NavStartDetectGround()"/-->
|
||||
<stay climb="nav_descend_vspeed" vmode="climb" wp="TD"/>
|
||||
</block>
|
||||
<block name="Ramp down">
|
||||
<exception cond="guidance_v_delta_t @LT 0.05*9600." deroute="Kill landed"/>
|
||||
<set var="fp_throttle" value="guidance_v_delta_t/9600."/>
|
||||
<stay throttle="fp_throttle-0.02*stage_time" vmode="throttle" wp="TAG"/>
|
||||
</block>
|
||||
<block name="Kill landed">
|
||||
<call_once fun="NavKillThrottle()"/>
|
||||
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
|
||||
</block>
|
||||
</blocks>
|
||||
</flight_plan>
|
||||
@@ -28,14 +28,14 @@
|
||||
<!ELEMENT exceptions (exception*)>
|
||||
|
||||
<!ELEMENT blocks (block+)>
|
||||
<!ELEMENT block (exception|while|heading|attitude|manual|go|xyz|set|call|call_once|circle|deroute|stay|follow|survey_rectangle|for|return|eight|oval|home|path)*>
|
||||
<!ELEMENT block (exception|while|heading|attitude|manual|go|xyz|set|call|call_once|circle|deroute|stay|follow|survey_rectangle|for|return|eight|oval|home|path|guided)*>
|
||||
|
||||
<!ELEMENT include (arg|with)*>
|
||||
<!ELEMENT arg EMPTY>
|
||||
<!ELEMENT with EMPTY>
|
||||
|
||||
<!ELEMENT while (exception|while|heading|attitude|manual|go|xyz|set|call|call_once|circle|deroute|stay|follow|survey_rectangle|for|return|eight|oval|path)*>
|
||||
<!ELEMENT for (exception|while|heading|attitude|manual|go|xyz|set|call|call_once|circle|deroute|stay|follow|survey_rectangle|for|return|eight|oval|path)*>
|
||||
<!ELEMENT while (exception|while|heading|attitude|manual|go|xyz|set|call|call_once|circle|deroute|stay|follow|survey_rectangle|for|return|eight|oval|path|guided)*>
|
||||
<!ELEMENT for (exception|while|heading|attitude|manual|go|xyz|set|call|call_once|circle|deroute|stay|follow|survey_rectangle|for|return|eight|oval|path|guided)*>
|
||||
<!ELEMENT exception EMPTY>
|
||||
<!ELEMENT heading EMPTY>
|
||||
<!ELEMENT attitude EMPTY>
|
||||
@@ -56,6 +56,7 @@
|
||||
<!ELEMENT param EMPTY>
|
||||
<!ELEMENT return EMPTY>
|
||||
<!ELEMENT path EMPTY>
|
||||
<!ELEMENT guided EMPTY>
|
||||
|
||||
|
||||
<!ATTLIST flight_plan
|
||||
@@ -330,6 +331,15 @@ nav_type CDATA #IMPLIED
|
||||
nav_params CDATA #IMPLIED
|
||||
height CDATA #IMPLIED>
|
||||
|
||||
<!ATTLIST guided
|
||||
commands CDATA #REQUIRED
|
||||
flags CDATA #IMPLIED
|
||||
until CDATA #IMPLIED
|
||||
pre_call CDATA #IMPLIED
|
||||
post_call CDATA #IMPLIED
|
||||
nav_type CDATA #IMPLIED
|
||||
nav_params CDATA #IMPLIED>
|
||||
|
||||
<!ATTLIST deroute
|
||||
block CDATA #REQUIRED>
|
||||
|
||||
|
||||
@@ -0,0 +1,52 @@
|
||||
<!DOCTYPE flight_plan SYSTEM "flight_plan.dtd">
|
||||
|
||||
<flight_plan alt="148" ground_alt="146" lat0="43.5640917" lon0="1.4829201" max_dist_from_home="20" name="Rotorcraft Optitrack (ENAC)" security_height="0.3">
|
||||
<header>
|
||||
// Useful Combination of the flags fir the autopilot_guided_update
|
||||
#define GUIDED_FLAG_XY_VEL_BODY GUIDED_FLAG_XY_BODY|GUIDED_FLAG_XY_VEL
|
||||
#define GUIDED_FLAG_XY_VEL_BODY_YAW_RATE GUIDED_FLAG_XY_BODY|GUIDED_FLAG_XY_VEL|GUIDED_FLAG_YAW_RATE
|
||||
#define GUIDED_FLAG_XY_OFFSET_Z_VEL GUIDED_FLAG_XY_OFFSET|GUIDED_FLAG_Z_VEL
|
||||
</header>
|
||||
<waypoints>
|
||||
<waypoint name="HOME" x="0.0" y="0.0"/>
|
||||
<waypoint name="STDBY" x="0.0" y="0.0"/>
|
||||
</waypoints>
|
||||
<variables>
|
||||
<variable var="desired_heading" init="0.0f" type="float" min="0." max="10." step="0.1"/>
|
||||
<variable var="climb_descent_vel" init="0.3f" type="float" min="0." max="1" step="0.1"/>
|
||||
<variable var="nominal_alt" init="2." type="float" min="0." max="8." step="0.1"/>
|
||||
</variables>
|
||||
<blocks>
|
||||
<block name="Wait GPS">
|
||||
<call_once fun="NavKillThrottle()"/>
|
||||
<while cond="!GpsFixValid()"/>
|
||||
</block>
|
||||
<block name="Holding point">
|
||||
<call_once fun="NavKillThrottle()"/>
|
||||
<attitude pitch="0" roll="0" throttle="0" vmode="throttle" until="FALSE"/>
|
||||
</block>
|
||||
<block name="Take off">
|
||||
<set var="desired_heading" value="stateGetNedToBodyEulers_f()->psi"/>
|
||||
<call_once fun="NavResurrect()"/>
|
||||
<attitude pitch="0" roll="0" throttle="0" vmode="throttle" until="stage_time @GT 1"/>
|
||||
<guided flags="GUIDED_FLAG_XY_OFFSET_Z_VEL" commands="0.,0.,-climb_descent_vel,desired_heading" until="GetPosHeight() @GT 1."/>
|
||||
<guided commands="0.,0.,-nominal_alt,desired_heading" until="FALSE"/>
|
||||
</block>
|
||||
<block name="Square">
|
||||
<guided commands="2.,2.,-nominal_alt,desired_heading" until="stage_time @GT 5"/>
|
||||
<guided commands="-2.,2.,-nominal_alt,desired_heading" until="stage_time @GT 5"/>
|
||||
<guided commands="-2.,-2.,-nominal_alt,desired_heading" until="stage_time @GT 5"/>
|
||||
<guided commands="2.,-2.,-nominal_alt,desired_heading" until="stage_time @GT 5"/>
|
||||
<guided commands="2.,2.,-nominal_alt,desired_heading" until="stage_time @GT 5"/>
|
||||
<guided commands="0.,0.,-nominal_alt,desired_heading" until="stage_time @GT 5"/>
|
||||
</block>
|
||||
<block name="Land">
|
||||
<guided flags="GUIDED_FLAG_XY_OFFSET_Z_VEL" commands="0.,0.,climb_descent_vel,desired_heading" until="GetPosHeight() @LT 0.1"/>
|
||||
<attitude pitch="0" roll="0" throttle="0" vmode="throttle" until="stage_time @GT 1"/>
|
||||
</block>
|
||||
<block name="Kill Engines">
|
||||
<call_once fun="NavKillThrottle()"/>
|
||||
<attitude pitch="0" roll="0" throttle="0" vmode="throttle" until="FALSE"/>
|
||||
</block>
|
||||
</blocks>
|
||||
</flight_plan>
|
||||
@@ -22,6 +22,10 @@
|
||||
<define name="TIMEOUT" value="5." description="timeout for lost tracking in seconds"/>
|
||||
<define name="PREDICT_TIME" value="1." description="position prediction time when moving waypoint"/>
|
||||
<define name="MAX_OFFSET" value="2." description="maximum position offset for predicted positin compared to estimated position"/>
|
||||
<define name="MAX_SPEED" value="4." description="maximum horiontal speed for the tracking control"/>
|
||||
<define name="MAX_VZ" value="2." description="maximum vertical speed for the tracking control"/>
|
||||
<define name="KP" value="0.5" description="horizontal gain on position error"/>
|
||||
<define name="KPZ" value="0.2" description="vertical gain on position error"/>
|
||||
</section>
|
||||
</doc>
|
||||
<settings target="ap|nps">
|
||||
@@ -29,6 +33,8 @@
|
||||
<dl_settings NAME="Tag Tracking">
|
||||
<dl_setting var="tag_tracking.motion_type" min="0" step="1" max="1" shortname="motion" values="FIXED|MOVING" module="modules/computer_vision/tag_tracking"/>
|
||||
<dl_setting var="tag_tracking.predict_time" min="0." step="0.1" max="5." shortname="pred_time"/>
|
||||
<dl_setting var="tag_tracking.kp" min="0." step="0.01" max="5." shortname="kp"/>
|
||||
<dl_setting var="tag_tracking.kpz" min="0." step="0.01" max="5." shortname="kpz"/>
|
||||
</dl_settings>
|
||||
</dl_settings>
|
||||
</settings>
|
||||
|
||||
@@ -36,9 +36,9 @@
|
||||
bool autopilot_guided_goto_ned(float x, float y, float z, float heading)
|
||||
{
|
||||
if (autopilot_get_mode() == AP_MODE_GUIDED) {
|
||||
guidance_h_set_guided_pos(x, y);
|
||||
guidance_h_set_guided_heading(heading);
|
||||
guidance_v_set_guided_z(z);
|
||||
guidance_h_set_pos(x, y);
|
||||
guidance_h_set_heading(heading);
|
||||
guidance_v_set_z(z);
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
@@ -72,9 +72,9 @@ bool autopilot_guided_goto_body_relative(float dx, float dy, float dz, float dya
|
||||
bool autopilot_guided_move_ned(float vx, float vy, float vz, float heading)
|
||||
{
|
||||
if (autopilot_get_mode() == AP_MODE_GUIDED) {
|
||||
guidance_h_set_guided_vel(vx, vy);
|
||||
guidance_h_set_guided_heading(heading);
|
||||
guidance_v_set_guided_vz(vz);
|
||||
guidance_h_set_vel(vx, vy);
|
||||
guidance_h_set_heading(heading);
|
||||
guidance_v_set_vz(vz);
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
@@ -95,22 +95,17 @@ bool autopilot_guided_move_ned(float vx, float vy, float vz, float heading)
|
||||
*/
|
||||
void autopilot_guided_update(uint8_t flags, float x, float y, float z, float yaw)
|
||||
{
|
||||
/* only update setpoints when in guided mode */
|
||||
if (autopilot_get_mode() != AP_MODE_GUIDED) {
|
||||
return;
|
||||
}
|
||||
|
||||
// handle x,y
|
||||
struct FloatVect2 setpoint = {.x = x, .y = y};
|
||||
if (bit_is_set(flags, 5)) { // velocity setpoint
|
||||
if (bit_is_set(flags, 1)) { // set velocity in body frame
|
||||
guidance_h_set_guided_body_vel(setpoint.x, setpoint.y);
|
||||
guidance_h_set_body_vel(setpoint.x, setpoint.y);
|
||||
} else {
|
||||
guidance_h_set_guided_vel(setpoint.x, setpoint.y);
|
||||
guidance_h_set_vel(setpoint.x, setpoint.y);
|
||||
}
|
||||
} else { // position setpoint
|
||||
if (!bit_is_set(flags, 0) && !bit_is_set(flags, 1)) { // set absolute position setpoint
|
||||
guidance_h_set_guided_pos(setpoint.x, setpoint.y);
|
||||
guidance_h_set_pos(setpoint.x, setpoint.y);
|
||||
} else {
|
||||
if (stateIsLocalCoordinateValid()) {
|
||||
if (bit_is_set(flags, 1)) { // set position as offset in body frame
|
||||
@@ -122,40 +117,42 @@ void autopilot_guided_update(uint8_t flags, float x, float y, float z, float yaw
|
||||
setpoint.x += stateGetPositionNed_f()->x;
|
||||
setpoint.y += stateGetPositionNed_f()->y;
|
||||
}
|
||||
guidance_h_set_guided_pos(setpoint.x, setpoint.y);
|
||||
guidance_h_set_pos(setpoint.x, setpoint.y);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
//handle z
|
||||
if (bit_is_set(flags, 6)) { // speed set-point
|
||||
guidance_v_set_guided_vz(z);
|
||||
guidance_v_set_vz(z);
|
||||
} else { // position set-point
|
||||
if (bit_is_set(flags, 2)) { // set position as offset in NED frame
|
||||
if (stateIsLocalCoordinateValid()) {
|
||||
z += stateGetPositionNed_f()->z;
|
||||
guidance_v_set_guided_z(z);
|
||||
guidance_v_set_z(z);
|
||||
}
|
||||
} else {
|
||||
guidance_v_set_guided_z(z);
|
||||
guidance_v_set_z(z);
|
||||
}
|
||||
}
|
||||
|
||||
//handle yaw
|
||||
if (bit_is_set(flags, 7)) { // speed set-point
|
||||
guidance_h_set_guided_heading_rate(yaw);
|
||||
guidance_h_set_heading_rate(yaw);
|
||||
} else { // position set-point
|
||||
if (bit_is_set(flags, 3)) { // set yaw as offset
|
||||
yaw += stateGetNedToBodyEulers_f()->psi; // will be wrapped to [-pi,pi] later
|
||||
}
|
||||
guidance_h_set_guided_heading(yaw);
|
||||
guidance_h_set_heading(yaw);
|
||||
}
|
||||
}
|
||||
|
||||
/** Parse GUIDED_SETPOINT_NED messages from datalink
|
||||
*/
|
||||
void autopilot_guided_parse_GUIDED(uint8_t *buf) {
|
||||
if (DL_GUIDED_SETPOINT_NED_ac_id(buf) != AC_ID) { return; }
|
||||
if (DL_GUIDED_SETPOINT_NED_ac_id(buf) != AC_ID || autopilot_get_mode() != AP_MODE_GUIDED) {
|
||||
return;
|
||||
}
|
||||
|
||||
autopilot_guided_update(
|
||||
DL_GUIDED_SETPOINT_NED_flags(buf),
|
||||
|
||||
@@ -103,5 +103,13 @@ extern void autopilot_guided_parse_GUIDED(uint8_t *buf);
|
||||
#define GUIDED_FLAG_Z_VEL (1<<6)
|
||||
#define GUIDED_FLAG_YAW_RATE (1<<7)
|
||||
|
||||
/** Macro for the flight plan insctructions
|
||||
*/
|
||||
#define NavGuided(_flags, _x, _y, _z, _yaw) { \
|
||||
horizontal_mode = HORIZONTAL_MODE_GUIDED; \
|
||||
vertical_mode = VERTICAL_MODE_GUIDED; \
|
||||
autopilot_guided_update(_flags, _x, _y, _z, _yaw); \
|
||||
}
|
||||
|
||||
#endif /* AUTOPILOT_GUIDED_H */
|
||||
|
||||
|
||||
@@ -556,35 +556,28 @@ void guidance_h_hover_enter(void)
|
||||
guidance_h.sp.speed.x = 0;
|
||||
guidance_h.sp.speed.y = 0;
|
||||
|
||||
/* disable horizontal velocity setpoints,
|
||||
* might still be activated in guidance_h_read_rc if GUIDANCE_H_USE_SPEED_REF
|
||||
*/
|
||||
ClearBit(guidance_h.sp.mask, 5);
|
||||
ClearBit(guidance_h.sp.mask, 7);
|
||||
|
||||
/* set horizontal setpoint to current position */
|
||||
VECT2_COPY(guidance_h.sp.pos, *stateGetPositionNed_i());
|
||||
|
||||
guidance_h_set_pos(
|
||||
stateGetPositionNed_f()->x,
|
||||
stateGetPositionNed_f()->y);
|
||||
/* reset guidance reference */
|
||||
reset_guidance_reference_from_current_position();
|
||||
|
||||
/* set guidance to current heading and position */
|
||||
guidance_h.rc_sp.psi = stateGetNedToBodyEulers_f()->psi;
|
||||
guidance_h.sp.heading = guidance_h.rc_sp.psi;
|
||||
guidance_h_set_heading(stateGetNedToBodyEulers_f()->psi);
|
||||
}
|
||||
|
||||
void guidance_h_nav_enter(void)
|
||||
{
|
||||
ClearBit(guidance_h.sp.mask, 5);
|
||||
ClearBit(guidance_h.sp.mask, 7);
|
||||
|
||||
/* horizontal position setpoint from navigation/flightplan */
|
||||
INT32_VECT2_NED_OF_ENU(guidance_h.sp.pos, navigation_carrot);
|
||||
|
||||
guidance_h_set_pos(
|
||||
POS_FLOAT_OF_BFP(navigation_carrot.y),
|
||||
POS_FLOAT_OF_BFP(navigation_carrot.x));
|
||||
reset_guidance_reference_from_current_position();
|
||||
|
||||
/* set nav_heading to current heading */
|
||||
nav_heading = stateGetNedToBodyEulers_i()->psi;
|
||||
guidance_h.sp.heading = stateGetNedToBodyEulers_f()->psi;
|
||||
guidance_h_set_heading(stateGetNedToBodyEulers_f()->psi);
|
||||
}
|
||||
|
||||
void guidance_h_from_nav(bool in_flight)
|
||||
@@ -609,20 +602,23 @@ void guidance_h_from_nav(bool in_flight)
|
||||
//make sure the heading is right before leaving horizontal_mode attitude
|
||||
guidance_hybrid_reset_heading(&sp_cmd_i);
|
||||
#endif
|
||||
} else if (horizontal_mode == HORIZONTAL_MODE_GUIDED) {
|
||||
guidance_h_guided_run(in_flight);
|
||||
} else {
|
||||
|
||||
#if HYBRID_NAVIGATION
|
||||
INT32_VECT2_NED_OF_ENU(guidance_h.sp.pos, navigation_target);
|
||||
guidance_hybrid_run();
|
||||
#else
|
||||
INT32_VECT2_NED_OF_ENU(guidance_h.sp.pos, navigation_carrot);
|
||||
|
||||
// set guidance in NED
|
||||
guidance_h_set_pos(
|
||||
POS_FLOAT_OF_BFP(navigation_carrot.y),
|
||||
POS_FLOAT_OF_BFP(navigation_carrot.x));
|
||||
guidance_h_update_reference();
|
||||
|
||||
#if GUIDANCE_HEADING_IS_FREE
|
||||
/* set psi command */
|
||||
guidance_h.sp.heading = ANGLE_FLOAT_OF_BFP(nav_heading);
|
||||
FLOAT_ANGLE_NORMALIZE(guidance_h.sp.heading);
|
||||
guidance_h_set_heading(ANGLE_FLOAT_OF_BFP(nav_heading));
|
||||
#endif
|
||||
|
||||
#if GUIDANCE_INDI
|
||||
@@ -717,55 +713,39 @@ void guidance_h_guided_run(bool in_flight)
|
||||
stabilization_attitude_run(in_flight);
|
||||
}
|
||||
|
||||
bool guidance_h_set_guided_pos(float x, float y)
|
||||
void guidance_h_set_pos(float x, float y)
|
||||
{
|
||||
if (guidance_h.mode == GUIDANCE_H_MODE_GUIDED) {
|
||||
ClearBit(guidance_h.sp.mask, 5);
|
||||
guidance_h.sp.pos.x = POS_BFP_OF_REAL(x);
|
||||
guidance_h.sp.pos.y = POS_BFP_OF_REAL(y);
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
ClearBit(guidance_h.sp.mask, 5);
|
||||
guidance_h.sp.pos.x = POS_BFP_OF_REAL(x);
|
||||
guidance_h.sp.pos.y = POS_BFP_OF_REAL(y);
|
||||
}
|
||||
|
||||
bool guidance_h_set_guided_heading(float heading)
|
||||
void guidance_h_set_heading(float heading)
|
||||
{
|
||||
if (guidance_h.mode == GUIDANCE_H_MODE_GUIDED) {
|
||||
ClearBit(guidance_h.sp.mask, 7);
|
||||
guidance_h.sp.heading = heading;
|
||||
FLOAT_ANGLE_NORMALIZE(guidance_h.sp.heading);
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
ClearBit(guidance_h.sp.mask, 7);
|
||||
guidance_h.sp.heading = heading;
|
||||
FLOAT_ANGLE_NORMALIZE(guidance_h.sp.heading);
|
||||
}
|
||||
|
||||
bool guidance_h_set_guided_body_vel(float vx, float vy)
|
||||
void guidance_h_set_body_vel(float vx, float vy)
|
||||
{
|
||||
float psi = stateGetNedToBodyEulers_f()->psi;
|
||||
float newvx = cosf(-psi) * vx + sinf(-psi) * vy;
|
||||
float newvy = -sinf(-psi) * vx + cosf(-psi) * vy;
|
||||
return guidance_h_set_guided_vel(newvx, newvy);
|
||||
guidance_h_set_vel(newvx, newvy);
|
||||
}
|
||||
|
||||
bool guidance_h_set_guided_vel(float vx, float vy)
|
||||
void guidance_h_set_vel(float vx, float vy)
|
||||
{
|
||||
if (guidance_h.mode == GUIDANCE_H_MODE_GUIDED) {
|
||||
SetBit(guidance_h.sp.mask, 5);
|
||||
guidance_h.sp.speed.x = SPEED_BFP_OF_REAL(vx);
|
||||
guidance_h.sp.speed.y = SPEED_BFP_OF_REAL(vy);
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
SetBit(guidance_h.sp.mask, 5);
|
||||
guidance_h.sp.speed.x = SPEED_BFP_OF_REAL(vx);
|
||||
guidance_h.sp.speed.y = SPEED_BFP_OF_REAL(vy);
|
||||
}
|
||||
|
||||
bool guidance_h_set_guided_heading_rate(float rate)
|
||||
void guidance_h_set_heading_rate(float rate)
|
||||
{
|
||||
if (guidance_h.mode == GUIDANCE_H_MODE_GUIDED) {
|
||||
SetBit(guidance_h.sp.mask, 7);
|
||||
guidance_h.sp.heading_rate = rate;
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
SetBit(guidance_h.sp.mask, 7);
|
||||
guidance_h.sp.heading_rate = rate;
|
||||
}
|
||||
|
||||
const struct Int32Vect2 *guidance_h_get_pos_err(void)
|
||||
|
||||
@@ -128,38 +128,33 @@ extern void guidance_h_set_igain(uint32_t igain);
|
||||
*/
|
||||
extern void guidance_h_guided_run(bool in_flight);
|
||||
|
||||
/** Set horizontal position setpoint in GUIDED mode.
|
||||
/** Set horizontal position setpoint.
|
||||
* @param x North position (local NED frame) in meters.
|
||||
* @param y East position (local NED frame) in meters.
|
||||
* @return TRUE if setpoints were set (currently in GUIDANCE_H_MODE_GUIDED)
|
||||
*/
|
||||
extern bool guidance_h_set_guided_pos(float x, float y);
|
||||
extern void guidance_h_set_pos(float x, float y);
|
||||
|
||||
/** Set heading setpoint in GUIDED mode.
|
||||
/** Set heading setpoint.
|
||||
* @param heading Setpoint in radians.
|
||||
* @return TRUE if setpoint was set (currently in GUIDANCE_H_MODE_GUIDED)
|
||||
*/
|
||||
extern bool guidance_h_set_guided_heading(float heading);
|
||||
extern void guidance_h_set_heading(float heading);
|
||||
|
||||
/** Set body relative horizontal velocity setpoint in GUIDED mode.
|
||||
/** Set body relative horizontal velocity setpoint.
|
||||
* @param vx forward velocity (body frame) in meters/sec.
|
||||
* @param vy right velocity (body frame) in meters/sec.
|
||||
* @return TRUE if setpoints were set (currently in GUIDANCE_H_MODE_GUIDED)
|
||||
*/
|
||||
extern bool guidance_h_set_guided_body_vel(float vx, float vy);
|
||||
extern void guidance_h_set_body_vel(float vx, float vy);
|
||||
|
||||
/** Set horizontal velocity setpoint in GUIDED mode.
|
||||
/** Set horizontal velocity setpoint.
|
||||
* @param vx North velocity (local NED frame) in meters/sec.
|
||||
* @param vy East velocity (local NED frame) in meters/sec.
|
||||
* @return TRUE if setpoints were set (currently in GUIDANCE_H_MODE_GUIDED)
|
||||
*/
|
||||
extern bool guidance_h_set_guided_vel(float vx, float vy);
|
||||
extern void guidance_h_set_vel(float vx, float vy);
|
||||
|
||||
/** Set heading rate setpoint in GUIDED mode.
|
||||
/** Set heading rate setpoint.
|
||||
* @param rate Heading rate in radians.
|
||||
* @return TRUE if setpoints were set (currently in GUIDANCE_H_MODE_GUIDED)
|
||||
*/
|
||||
extern bool guidance_h_set_guided_heading_rate(float rate);
|
||||
extern void guidance_h_set_heading_rate(float rate);
|
||||
|
||||
/** Gets the position error
|
||||
* @param none.
|
||||
|
||||
@@ -488,6 +488,8 @@ void guidance_v_from_nav(bool in_flight)
|
||||
GuidanceVSetRef(guidance_v_z_sp, guidance_v_zd_sp, 0);
|
||||
guidance_v_z_sum_err = 0;
|
||||
guidance_v_delta_t = nav_throttle;
|
||||
} else if (vertical_mode == VERTICAL_MODE_GUIDED) {
|
||||
guidance_v_guided_run(in_flight);
|
||||
}
|
||||
#if HYBRID_NAVIGATION
|
||||
guidance_hybrid_vertical();
|
||||
@@ -504,12 +506,8 @@ void guidance_v_from_nav(bool in_flight)
|
||||
|
||||
void guidance_v_guided_enter(void)
|
||||
{
|
||||
/* disable vertical velocity setpoints */
|
||||
guidance_v_guided_mode = GUIDANCE_V_GUIDED_MODE_ZHOLD;
|
||||
|
||||
/* set current altitude as setpoint and reset speed setpoint */
|
||||
guidance_v_z_sp = stateGetPositionNed_i()->z;
|
||||
guidance_v_zd_sp = 0;
|
||||
/* set current altitude as setpoint */
|
||||
guidance_v_set_z(stateGetPositionNed_f()->z);
|
||||
|
||||
/* reset guidance reference */
|
||||
guidance_v_z_sum_err = 0;
|
||||
@@ -548,50 +546,33 @@ void guidance_v_guided_run(bool in_flight)
|
||||
stabilization_cmd[COMMAND_THRUST] = guidance_v_delta_t;
|
||||
}
|
||||
|
||||
bool guidance_v_set_guided_z(float z)
|
||||
void guidance_v_set_z(float z)
|
||||
{
|
||||
if (guidance_v_mode == GUIDANCE_V_MODE_GUIDED) {
|
||||
/* disable vertical velocity setpoints */
|
||||
guidance_v_guided_mode = GUIDANCE_V_GUIDED_MODE_ZHOLD;
|
||||
|
||||
/* set altitude setpoint */
|
||||
guidance_v_z_sp = POS_BFP_OF_REAL(z);
|
||||
|
||||
/* reset speed setting */
|
||||
guidance_v_zd_sp = 0;
|
||||
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
/* disable vertical velocity setpoints */
|
||||
guidance_v_guided_mode = GUIDANCE_V_GUIDED_MODE_ZHOLD;
|
||||
/* set altitude setpoint */
|
||||
guidance_v_z_sp = POS_BFP_OF_REAL(z);
|
||||
/* reset speed setting */
|
||||
guidance_v_zd_sp = 0;
|
||||
}
|
||||
|
||||
bool guidance_v_set_guided_vz(float vz)
|
||||
void guidance_v_set_vz(float vz)
|
||||
{
|
||||
if (guidance_v_mode == GUIDANCE_V_MODE_GUIDED) {
|
||||
/* enable vertical velocity setpoints */
|
||||
guidance_v_guided_mode = GUIDANCE_V_GUIDED_MODE_CLIMB;
|
||||
|
||||
/* set speed setting */
|
||||
guidance_v_zd_sp = SPEED_BFP_OF_REAL(vz);
|
||||
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
/* enable vertical velocity setpoints */
|
||||
guidance_v_guided_mode = GUIDANCE_V_GUIDED_MODE_CLIMB;
|
||||
/* set speed setting */
|
||||
guidance_v_zd_sp = SPEED_BFP_OF_REAL(vz);
|
||||
}
|
||||
|
||||
bool guidance_v_set_guided_th(float th)
|
||||
void guidance_v_set_th(float th)
|
||||
{
|
||||
if (guidance_v_mode == GUIDANCE_V_MODE_GUIDED) {
|
||||
/* enable vertical velocity setpoints */
|
||||
guidance_v_guided_mode = GUIDANCE_V_GUIDED_MODE_THROTTLE;
|
||||
/* enable vertical velocity setpoints */
|
||||
guidance_v_guided_mode = GUIDANCE_V_GUIDED_MODE_THROTTLE;
|
||||
|
||||
/* reset guidance reference */
|
||||
GuidanceVSetRef(stateGetPositionNed_i()->z, stateGetSpeedNed_i()->z, 0);
|
||||
guidance_v_th_sp = (int32_t)(MAX_PPRZ * th);
|
||||
Bound(guidance_v_th_sp, 0, MAX_PPRZ);
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
/* reset guidance reference */
|
||||
GuidanceVSetRef(stateGetPositionNed_i()->z, stateGetSpeedNed_i()->z, 0);
|
||||
guidance_v_th_sp = (int32_t)(MAX_PPRZ * th);
|
||||
Bound(guidance_v_th_sp, 0, MAX_PPRZ);
|
||||
}
|
||||
|
||||
|
||||
|
||||
@@ -133,19 +133,20 @@ extern void guidance_v_guided_enter(void);
|
||||
*/
|
||||
extern void guidance_v_guided_run(bool in_flight);
|
||||
|
||||
/** Set z setpoint in GUIDED mode.
|
||||
/** Set z position setpoint.
|
||||
* @param z Setpoint (down is positive) in meters.
|
||||
* @return TRUE if setpoint was set (currently in GUIDANCE_V_MODE_GUIDED)
|
||||
*/
|
||||
extern bool guidance_v_set_guided_z(float z);
|
||||
extern void guidance_v_set_z(float z);
|
||||
|
||||
/** Set z velocity setpoint in GUIDED mode.
|
||||
/** Set z velocity setpoint.
|
||||
* @param vz Setpoint (down is positive) in meters/second.
|
||||
* @return TRUE if setpoint was set (currently in GUIDANCE_V_MODE_GUIDED)
|
||||
*/
|
||||
extern bool guidance_v_set_guided_vz(float vz);
|
||||
extern void guidance_v_set_vz(float vz);
|
||||
|
||||
extern bool guidance_v_set_guided_th(float th);
|
||||
/** Set throttle setpoint.
|
||||
* @param th Throttle setpoint between 0. and 1.
|
||||
*/
|
||||
extern void guidance_v_set_th(float th);
|
||||
|
||||
#define guidance_v_SetKi(_val) { \
|
||||
guidance_v_ki = _val; \
|
||||
|
||||
@@ -62,6 +62,7 @@ extern int32_t nav_circle_radius, nav_circle_qdr, nav_circle_radians;
|
||||
#define HORIZONTAL_MODE_CIRCLE 2
|
||||
#define HORIZONTAL_MODE_ATTITUDE 3
|
||||
#define HORIZONTAL_MODE_MANUAL 4
|
||||
#define HORIZONTAL_MODE_GUIDED 5
|
||||
extern int32_t nav_roll, nav_pitch; ///< with #INT32_ANGLE_FRAC
|
||||
extern int32_t nav_heading; ///< with #INT32_ANGLE_FRAC
|
||||
extern int32_t nav_cmd_roll, nav_cmd_pitch, nav_cmd_yaw;
|
||||
@@ -80,6 +81,7 @@ extern float flight_altitude;
|
||||
#define VERTICAL_MODE_MANUAL 0
|
||||
#define VERTICAL_MODE_CLIMB 1
|
||||
#define VERTICAL_MODE_ALT 2
|
||||
#define VERTICAL_MODE_GUIDED 3
|
||||
|
||||
extern float dist2_to_home; ///< squared distance to home waypoint
|
||||
extern bool too_far_from_home;
|
||||
|
||||
@@ -127,13 +127,29 @@ float speed_circle = 0.03;
|
||||
#endif
|
||||
|
||||
#ifndef TAG_TRACKING_PREDICT_TIME
|
||||
#define TAG_TRACKING_PREDICT_TIME 1.5f
|
||||
#define TAG_TRACKING_PREDICT_TIME 1.f
|
||||
#endif
|
||||
|
||||
#ifndef TAG_TRACKING_MAX_OFFSET
|
||||
#define TAG_TRACKING_MAX_OFFSET 2.0f
|
||||
#endif
|
||||
|
||||
#ifndef TAG_TRACKING_KP
|
||||
#define TAG_TRACKING_KP 0.5f
|
||||
#endif
|
||||
|
||||
#ifndef TAG_TRACKING_KPZ
|
||||
#define TAG_TRACKING_KPZ 0.2f
|
||||
#endif
|
||||
|
||||
#ifndef TAG_TRACKING_MAX_SPEED
|
||||
#define TAG_TRACKING_MAX_SPEED 4.f
|
||||
#endif
|
||||
|
||||
#ifndef TAG_TRACKING_MAX_VZ
|
||||
#define TAG_TRACKING_MAX_VZ 2.f
|
||||
#endif
|
||||
|
||||
// generated in modules.h
|
||||
static const float tag_track_dt = TAG_TRACKING_PROPAGATE_PERIOD;
|
||||
|
||||
@@ -233,7 +249,7 @@ void tag_tracking_parse_target_pos(uint8_t *buf)
|
||||
}
|
||||
|
||||
// Update and display tracking WP
|
||||
static void update_wp(bool report)
|
||||
static void update_wp(bool report UNUSED)
|
||||
{
|
||||
#ifdef TAG_TRACKING_WP
|
||||
struct FloatVect3 target_pos_enu, target_pos_pred;
|
||||
@@ -263,6 +279,7 @@ void tag_tracking_init()
|
||||
FLOAT_VECT3_ZERO(tag_track_private.meas);
|
||||
FLOAT_VECT3_ZERO(tag_tracking.pos);
|
||||
FLOAT_VECT3_ZERO(tag_tracking.speed);
|
||||
FLOAT_VECT3_ZERO(tag_tracking.speed_cmd);
|
||||
struct FloatEulers euler = {
|
||||
TAG_TRACKING_BODY_TO_CAM_PHI,
|
||||
TAG_TRACKING_BODY_TO_CAM_THETA,
|
||||
@@ -273,6 +290,8 @@ void tag_tracking_init()
|
||||
TAG_TRACKING_CAM_POS_X,
|
||||
TAG_TRACKING_CAM_POS_Y,
|
||||
TAG_TRACKING_CAM_POS_Z);
|
||||
tag_tracking.kp = TAG_TRACKING_KP;
|
||||
tag_tracking.kpz = TAG_TRACKING_KPZ;
|
||||
|
||||
// Bind to ABI message
|
||||
AbiBindMsgJEVOIS_MSG(TAG_TRACKING_ID, &tag_track_ev, tag_track_cb);
|
||||
@@ -365,6 +384,28 @@ void tag_tracking_report()
|
||||
}
|
||||
}
|
||||
|
||||
/** Control function
|
||||
*
|
||||
* calling this function only updates the command vector
|
||||
* it can be applied to the guidance control using the guided mode
|
||||
* or from the flight plan with 'guided' instruction
|
||||
*/
|
||||
void tag_tracking_compute_speed(void)
|
||||
{
|
||||
if (tag_tracking.status == TAG_TRACKING_RUNNING) {
|
||||
// compute speed command as estimated tag speed + gain * position error
|
||||
struct NedCoor_f pos = *stateGetPositionNed_f();
|
||||
tag_tracking.speed_cmd.x = tag_tracking.speed.x + tag_tracking.kp * (tag_tracking.pos.x - pos.x);
|
||||
tag_tracking.speed_cmd.y = tag_tracking.speed.y + tag_tracking.kp * (tag_tracking.pos.y - pos.y);
|
||||
tag_tracking.speed_cmd.z = tag_tracking.speed.z + tag_tracking.kpz * (tag_tracking.pos.z - pos.z);
|
||||
VECT2_STRIM(tag_tracking.speed_cmd, -TAG_TRACKING_MAX_SPEED, TAG_TRACKING_MAX_SPEED); // trim max horizontal speed
|
||||
BoundAbs(tag_tracking.speed_cmd.z, TAG_TRACKING_MAX_VZ); // tim max vertical speed
|
||||
}
|
||||
else {
|
||||
// filter is not runing, set speed command to zero
|
||||
FLOAT_VECT3_ZERO(tag_tracking.speed_cmd);
|
||||
}
|
||||
}
|
||||
|
||||
// Simulate detection using a WP coordinate
|
||||
#if defined SITL && defined TAG_TRACKING_SIM_WP
|
||||
|
||||
@@ -31,6 +31,7 @@
|
||||
|
||||
#include "std.h"
|
||||
#include "math/pprz_algebra_float.h"
|
||||
#include "math/pprz_geodetic_float.h"
|
||||
|
||||
// Searching status
|
||||
#define TAG_TRACKING_SEARCHING 0
|
||||
@@ -49,6 +50,9 @@ struct tag_tracking_public {
|
||||
uint8_t status; ///< tracking status flag
|
||||
uint8_t motion_type; ///< type of tag motion
|
||||
float predict_time; ///< prediction time for WP tag
|
||||
struct NedCoor_f speed_cmd; ///< speed command to track the tag position
|
||||
float kp; ///< horizontal tracking command gain
|
||||
float kpz; ///< vertical tracking command gain
|
||||
};
|
||||
|
||||
extern struct tag_tracking_public tag_tracking;
|
||||
@@ -58,6 +62,7 @@ extern void tag_tracking_propagate(void);
|
||||
extern void tag_tracking_propagate_start(void);
|
||||
extern void tag_tracking_report(void);
|
||||
extern void tag_tracking_parse_target_pos(uint8_t *buf);
|
||||
extern void tag_tracking_compute_speed(void);
|
||||
|
||||
#endif // TAG_TRACKING_H
|
||||
|
||||
|
||||
@@ -150,13 +150,13 @@ void orange_avoider_guided_periodic(void)
|
||||
} else if (obstacle_free_confidence == 0){
|
||||
navigation_state = OBSTACLE_FOUND;
|
||||
} else {
|
||||
guidance_h_set_guided_body_vel(speed_sp, 0);
|
||||
guidance_h_set_body_vel(speed_sp, 0);
|
||||
}
|
||||
|
||||
break;
|
||||
case OBSTACLE_FOUND:
|
||||
// stop
|
||||
guidance_h_set_guided_body_vel(0, 0);
|
||||
guidance_h_set_body_vel(0, 0);
|
||||
|
||||
// randomly select new search direction
|
||||
chooseRandomIncrementAvoidance();
|
||||
@@ -165,20 +165,20 @@ void orange_avoider_guided_periodic(void)
|
||||
|
||||
break;
|
||||
case SEARCH_FOR_SAFE_HEADING:
|
||||
guidance_h_set_guided_heading_rate(avoidance_heading_direction * oag_heading_rate);
|
||||
guidance_h_set_heading_rate(avoidance_heading_direction * oag_heading_rate);
|
||||
|
||||
// make sure we have a couple of good readings before declaring the way safe
|
||||
if (obstacle_free_confidence >= 2){
|
||||
guidance_h_set_guided_heading(stateGetNedToBodyEulers_f()->psi);
|
||||
guidance_h_set_heading(stateGetNedToBodyEulers_f()->psi);
|
||||
navigation_state = SAFE;
|
||||
}
|
||||
break;
|
||||
case OUT_OF_BOUNDS:
|
||||
// stop
|
||||
guidance_h_set_guided_body_vel(0, 0);
|
||||
guidance_h_set_body_vel(0, 0);
|
||||
|
||||
// start turn back into arena
|
||||
guidance_h_set_guided_heading_rate(avoidance_heading_direction * RadOfDeg(15));
|
||||
guidance_h_set_heading_rate(avoidance_heading_direction * RadOfDeg(15));
|
||||
|
||||
navigation_state = REENTER_ARENA;
|
||||
|
||||
@@ -187,7 +187,7 @@ void orange_avoider_guided_periodic(void)
|
||||
// force floor center to opposite side of turn to head back into arena
|
||||
if (floor_count >= floor_count_threshold && avoidance_heading_direction * floor_centroid_frac >= 0.f){
|
||||
// return to heading mode
|
||||
guidance_h_set_guided_heading(stateGetNedToBodyEulers_f()->psi);
|
||||
guidance_h_set_heading(stateGetNedToBodyEulers_f()->psi);
|
||||
|
||||
// reset safe counter
|
||||
obstacle_free_confidence = 0;
|
||||
|
||||
@@ -50,7 +50,7 @@
|
||||
#include "math/pprz_algebra.h"// Needed for vector operations (simple subtraction)
|
||||
#include "math/pprz_geodetic_float.h"// Needed for NedCoor_f
|
||||
#include "generated/flight_plan.h" // Needed for WP (waypoint) functions and WP IDs (as defined in "ralphthesis2020_stereo_cyberzoo.xml")
|
||||
#include "firmwares/rotorcraft/autopilot_guided.h" // Needed for guidance functions such as "autopilot_guided_goto_ned" and "guidance_h_set_guided_heading"
|
||||
#include "firmwares/rotorcraft/autopilot_guided.h" // Needed for guidance functions such as "autopilot_guided_goto_ned" and "guidance_h_set_heading"
|
||||
#include <math.h> // needed for basic math functions
|
||||
#include "autopilot.h" // Needed to set states (GUIDED vs NAV)
|
||||
#include <time.h> // Needed to measure time
|
||||
@@ -2357,7 +2357,7 @@ void wedgebug_periodic()
|
||||
// Else, adjust heading and check confidence that heading is reached
|
||||
else { // This happens continuously, as long as the state is active. It stops when the flag has been set below
|
||||
// Set desired heading
|
||||
guidance_h_set_guided_heading(heading_towards_setpoint_WNED(
|
||||
guidance_h_set_heading(heading_towards_setpoint_WNED(
|
||||
&VPBESTEDGECOORDINATESwned));// heading_towards_waypoint(WP_GOAL));
|
||||
|
||||
// If heading appears to be reached increase confidence
|
||||
@@ -2388,8 +2388,8 @@ void wedgebug_periodic()
|
||||
else { // This happens continuously, as long as the state is active. It stops when the flag has been set below.
|
||||
|
||||
// Sets setpoint to edge position and orientates robot towards it as well
|
||||
guidance_h_set_guided_pos(VPBESTEDGECOORDINATESwned.x, VPBESTEDGECOORDINATESwned.y);
|
||||
guidance_v_set_guided_z(VPBESTEDGECOORDINATESwned.z);
|
||||
guidance_h_set_pos(VPBESTEDGECOORDINATESwned.x, VPBESTEDGECOORDINATESwned.y);
|
||||
guidance_v_set_z(VPBESTEDGECOORDINATESwned.z);
|
||||
|
||||
// If edge appears to be reached increase confidence
|
||||
if (is_setpoint_reached(&VPBESTEDGECOORDINATESwned, &VRwned,
|
||||
@@ -2425,19 +2425,19 @@ void wedgebug_periodic()
|
||||
printf("POSITION_EDGE = %d\n", POSITION_EDGE);
|
||||
|
||||
// This ensures that the robot stay on the edge at all times
|
||||
guidance_h_set_guided_pos(VPBESTEDGECOORDINATESwned.x, VPBESTEDGECOORDINATESwned.y);
|
||||
guidance_v_set_guided_z(VPBESTEDGECOORDINATESwned.z);
|
||||
guidance_h_set_pos(VPBESTEDGECOORDINATESwned.x, VPBESTEDGECOORDINATESwned.y);
|
||||
guidance_v_set_z(VPBESTEDGECOORDINATESwned.z);
|
||||
|
||||
// 1. Orientates robot towards the final goal
|
||||
// If robot faces goal, robot stays on current edge
|
||||
if (is_heading_reached_flag) {
|
||||
printf("Heading is reached\n");
|
||||
guidance_h_set_guided_heading(heading_towards_waypoint(WP_GOAL));
|
||||
guidance_h_set_heading(heading_towards_waypoint(WP_GOAL));
|
||||
}
|
||||
// Else, adjust heading and check confidence that heading is reached
|
||||
else { // This happens continuously, as long as the state is active. It stops when the flag has been set.
|
||||
// Set desired heading
|
||||
guidance_h_set_guided_heading(heading_towards_waypoint(WP_GOAL));
|
||||
guidance_h_set_heading(heading_towards_waypoint(WP_GOAL));
|
||||
|
||||
// If heading appears to be reached increase confidence
|
||||
if (is_heading_reached(heading_towards_waypoint(WP_GOAL), heading, threshold_distance_to_angle)) {
|
||||
@@ -2523,7 +2523,7 @@ void wedgebug_periodic()
|
||||
|
||||
// 1. The robot is tasked to stay at the holding point for the entire duration of this state
|
||||
// Making drone hover, so that it does not drift from its current position
|
||||
guidance_h_set_guided_pos(VHOLDINGPOINTwned.x, VHOLDINGPOINTwned.y); guidance_v_set_guided_z(VHOLDINGPOINTwned.z);
|
||||
guidance_h_set_pos(VHOLDINGPOINTwned.x, VHOLDINGPOINTwned.y); guidance_v_set_z(VHOLDINGPOINTwned.z);
|
||||
|
||||
// 2. Checking if edges are located
|
||||
// Running function to detect and save edge
|
||||
@@ -2594,7 +2594,7 @@ void wedgebug_periodic()
|
||||
// Else, adjust angle to to face more left and check confidence that left heading has been reached
|
||||
else { // This happens continuously, as long as the state is active. It stops when the flag has been set below
|
||||
// Set heading to maximum left heading
|
||||
guidance_h_set_guided_heading(initial_heading.heading_max_left);
|
||||
guidance_h_set_heading(initial_heading.heading_max_left);
|
||||
|
||||
// If heading appears to be reached increase confidence
|
||||
if (is_heading_reached(initial_heading.heading_max_left, heading, threshold_distance_to_angle)) {
|
||||
@@ -2620,7 +2620,7 @@ void wedgebug_periodic()
|
||||
// Else, adjust angle to to face more right and check confidence that right heading has been reached
|
||||
else {
|
||||
// Set heading to maximum left heading
|
||||
guidance_h_set_guided_heading(initial_heading.heading_max_right);
|
||||
guidance_h_set_heading(initial_heading.heading_max_right);
|
||||
|
||||
// If heading appears to be reached increase confidence
|
||||
if (is_heading_reached(initial_heading.heading_max_right, heading, threshold_distance_to_angle)) {
|
||||
|
||||
@@ -30,6 +30,10 @@
|
||||
#include <stdbool.h>
|
||||
#include <math.h>
|
||||
|
||||
#ifdef SITL
|
||||
#include <stdio.h> // for debuging in simulation
|
||||
#endif
|
||||
|
||||
/* some convenience macros to print debug/config messages at compile time */
|
||||
#include "message_pragmas.h"
|
||||
|
||||
|
||||
@@ -336,7 +336,7 @@ let rec index_stage = fun x ->
|
||||
incr stage; (* To count the loop stage *)
|
||||
Xml.Element (Xml.tag x, Xml.attribs x@["no", soi n], l)
|
||||
| "return" | "goto" | "deroute" | "exit_block" | "follow" | "call" | "call_once" | "home"
|
||||
| "heading" | "attitude" | "manual" | "go" | "stay" | "xyz" | "set" | "circle" ->
|
||||
| "heading" | "attitude" | "manual" | "go" | "stay" | "xyz" | "set" | "circle" | "guided" ->
|
||||
incr stage;
|
||||
Xml.Element (Xml.tag x, Xml.attribs x@["no", soi !stage], Xml.children x)
|
||||
| "survey_rectangle" | "eight" | "oval"->
|
||||
@@ -552,6 +552,17 @@ let rec print_stage = fun out index_of_waypoints x ->
|
||||
stage_until out x;
|
||||
fp_post_call out x;
|
||||
lprintf out "break;\n"
|
||||
| "guided" ->
|
||||
stage out;
|
||||
fp_pre_call out x;
|
||||
let cmds = ExtXml.attrib x "commands" in
|
||||
let flags = ExtXml.attrib_or_default x "flags" "0" in
|
||||
let t = ExtXml.attrib_or_default x "nav_type" "Nav" in
|
||||
let p = try ", " ^ (Xml.attrib x "nav_params") with _ -> "" in
|
||||
lprintf out "%sGuided(%s, %s%s);\n" t flags cmds p;
|
||||
stage_until out x;
|
||||
fp_post_call out x;
|
||||
lprintf out "break;\n"
|
||||
| "eight" ->
|
||||
stage out;
|
||||
lprintf out "nav_eight_init();\n";
|
||||
|
||||
Reference in New Issue
Block a user