mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-09 22:49:53 +08:00
Collection of changes after IMAV2024 (#3370)
- update airframe and add competition flight plans - fix flight plan DTD - mag params for Bristol - add a set_expo function (might be weak) for digital cam driver - fix tag tracking for simulation, add getter functions - add option to skip initial circle in nav survey - fix jevois driver parsing
This commit is contained in:
Gautier Hattenberger
committed by
GitHub
GitHub
parent
7ac367b77c
commit
51e43fb67d
@@ -1,18 +1,17 @@
|
||||
<!DOCTYPE airframe SYSTEM "../../airframe.dtd">
|
||||
|
||||
<airframe name="Jumper Neo">
|
||||
<airframe name="Hawkeye">
|
||||
<description>
|
||||
Hawkeye plane with NanoPi Air and IDS cam
|
||||
Designed for IMAV2018
|
||||
Used at IMAV2019 and IMAV2024
|
||||
</description>
|
||||
|
||||
<firmware name="fixedwing">
|
||||
<configure name="PERIODIC_FREQUENCY" value="125"/>
|
||||
|
||||
<target name="ap" board="apogee_1.0_chibios">
|
||||
<module name="radio_control" type="sbus">
|
||||
<define name="RADIO_KILL_SWITCH" value="RADIO_GAIN1"/>
|
||||
</module>
|
||||
<module name="radio_control" type="sbus"/>
|
||||
<!--module name="ins" type="float_invariant">
|
||||
<define name="USE_MAGNETOMETER"/>
|
||||
<configure name="AHRS_PROPAGATE_FREQUENCY" value="125"/>
|
||||
@@ -22,12 +21,14 @@
|
||||
</module-->
|
||||
<module name="ahrs" type="float_dcm"/>
|
||||
<module name="ins" type="alt_float"/>
|
||||
<define name="DC_SHOT_EXTRA_DL" value="TRUE"/>
|
||||
</target>
|
||||
<target name="sim" board="pc">
|
||||
<module name="radio_control" type="ppm"/>
|
||||
<module name="ahrs" type="float_dcm"/>
|
||||
<module name="ins" type="alt_float"/>
|
||||
<module name="uart"/>
|
||||
<define name="UART6_DEV" value="/tmp/ttya"/>
|
||||
<define name="POWER_SWITCH_GPIO" value="GPIOA,GPIO0"/>
|
||||
</target>
|
||||
|
||||
<!-- Communication -->
|
||||
@@ -45,21 +46,23 @@
|
||||
<module name="gps" type="ublox">
|
||||
<configure name="GPS_BAUD" value="B115200"/>
|
||||
</module>
|
||||
<module name="mag_hmc58xx.xml">
|
||||
<!--module name="mag_hmc58xx.xml">
|
||||
<define name="MODULE_HMC58XX_UPDATE_AHRS"/>
|
||||
<configure name="MAG_HMC58XX_I2C_DEV" value="i2c2"/>
|
||||
</module>
|
||||
</module-->
|
||||
|
||||
<module name="extra_dl">
|
||||
<configure name="EXTRA_DL_PORT" value="UART6"/>
|
||||
<configure name="EXTRA_DL_BAUD" value="B115200"/>
|
||||
</module>
|
||||
<module name="digital_cam_gpio">
|
||||
<define name="DC_SHUTTER_GPIO" value="GPIOB,GPIO15"/> <!-- Apogee AUX4 -->
|
||||
<define name="DC_AUTOSHOOT_DISTANCE_INIT" value="4"/>
|
||||
</module>
|
||||
<module name="digital_cam" type="pprzlink"/>
|
||||
|
||||
<!--module name="current_sensor">
|
||||
<configure name="ADC_CURRENT_SENSOR" value="ADC_1"/>
|
||||
</module-->
|
||||
|
||||
<module name="takeoff_detect"/>
|
||||
<module name="flight_recorder"/>
|
||||
</firmware>
|
||||
|
||||
<!-- commands section -->
|
||||
@@ -144,6 +147,7 @@
|
||||
|
||||
<section name="BAT">
|
||||
<define name="CATASTROPHIC_BAT_LEVEL" value="9.3" unit="V"/>
|
||||
<!--define name="MilliAmpereOfAdc(_adc)" value="(9.82587*(_adc-421))"/-->
|
||||
</section>
|
||||
|
||||
<section name="MISC">
|
||||
|
||||
@@ -26,6 +26,7 @@
|
||||
<target name="nps" board="pc">
|
||||
<module name="fdm" type="jsbsim"/>
|
||||
<module name="radio_control" type="ppm"/>
|
||||
<define name="UART4_DEV" value="/tmp/ttya"/>
|
||||
</target>
|
||||
|
||||
<module name="telemetry" type="xbee_api"/>
|
||||
@@ -40,6 +41,7 @@
|
||||
<module name="gps" type="ublox">
|
||||
<configure name="GPS_BAUD" value="B115200"/>
|
||||
<define name="USE_GPS_UBX_RTCM" value="TRUE"/>
|
||||
<define name="GPS_FIX_TIMEOUT" value="0.5"/>
|
||||
</module>
|
||||
|
||||
<module name="stabilization" type="indi">
|
||||
@@ -67,8 +69,20 @@
|
||||
</module>
|
||||
<module name="agl_dist"/-->
|
||||
|
||||
<module name="digital_cam" type="pprzlink"/>
|
||||
<module name="power_switch"/>
|
||||
<module name="current_sensor">
|
||||
<configure name="ADC_CURRENT_SENSOR" value="ADC_8"/>
|
||||
</module>
|
||||
|
||||
<module name="extra_dl">
|
||||
<configure name="EXTRA_DL_PORT" value="uart4"/>
|
||||
<configure name="EXTRA_DL_BAUD" value="B115200"/>
|
||||
</module>
|
||||
|
||||
<module name="tag_tracking">
|
||||
<configure name="JEVOIS_UART" value="UART3" description="UART on which Jevois camera is connected"/>
|
||||
<!--define name="JEVOIS_SEND_ALT"/--> <!-- enable for color rectangle tracking -->
|
||||
</module>
|
||||
|
||||
<module name="flight_recorder"/>
|
||||
@@ -83,7 +97,7 @@
|
||||
</servos>
|
||||
|
||||
<servos driver="Pwm">
|
||||
<servo name="SWITCH" no="1" min="1000" neutral="1000" max="2000"/>
|
||||
<servo name="SWITCH" no="1" min="1150" neutral="1150" max="1800"/>
|
||||
</servos>
|
||||
|
||||
<commands>
|
||||
@@ -118,12 +132,12 @@
|
||||
<define name="MAG_X_SIGN" value="-1"/>
|
||||
<define name="MAG_Y_SIGN" value="-1"/>
|
||||
<define name="MAG_Z_SIGN" value="1"/>
|
||||
<define name="MAG_X_NEUTRAL" value="415"/>
|
||||
<define name="MAG_Y_NEUTRAL" value="-2365"/>
|
||||
<define name="MAG_Z_NEUTRAL" value="1626"/>
|
||||
<define name="MAG_X_SENS" value="0.6639205812316621" integer="16"/>
|
||||
<define name="MAG_Y_SENS" value="0.6470134039115051" integer="16"/>
|
||||
<define name="MAG_Z_SENS" value="0.6456573321937272" integer="16"/>
|
||||
<define name="MAG_X_NEUTRAL" value="437"/>
|
||||
<define name="MAG_Y_NEUTRAL" value="-2357"/>
|
||||
<define name="MAG_Z_NEUTRAL" value="1978"/>
|
||||
<define name="MAG_X_SENS" value="0.6420696170457475" integer="16"/>
|
||||
<define name="MAG_Y_SENS" value="0.6426945667319279" integer="16"/>
|
||||
<define name="MAG_Z_SENS" value="0.6220200205771864" integer="16"/>
|
||||
|
||||
<define name="BODY_TO_IMU_PHI" value="0." unit="deg"/>
|
||||
<define name="BODY_TO_IMU_THETA" value="0." unit="deg"/>
|
||||
@@ -211,7 +225,15 @@
|
||||
</section>
|
||||
|
||||
<section name="BAT">
|
||||
<define name="CRITIC_BAT_LEVEL" value="9.3" unit="V"/>
|
||||
<define name="MilliAmpereOfAdc(_adc)" value="(20.76*_adc-9970)"/>
|
||||
<!-- 3 Cells -->
|
||||
<!--define name="CRITIC_BAT_LEVEL" value="9.3" unit="V"/-->
|
||||
<!-- 4 Cells -->
|
||||
<define name="CATASTROPHIC_BAT_LEVEL" value="12.3" unit="V"/>
|
||||
<define name="CRITIC_BAT_LEVEL" value="13." unit="V"/>
|
||||
<define name="LOW_BAT_LEVEL" value="13.5" unit="V"/>
|
||||
<define name="MAX_BAT_LEVEL" value="17.0" unit="V"/>
|
||||
<define name="BAT_NB_CELLS" value="4"/>
|
||||
</section>
|
||||
|
||||
<section name="AUTOPILOT">
|
||||
@@ -236,16 +258,10 @@
|
||||
<section name="TAG_TRACKING" prefix="TAG_TRACKING_">
|
||||
<define name="BODY_TO_CAM_PSI" value="0"/>
|
||||
<define name="CAM_POS_Y" value="-0.12"/>
|
||||
<define name="WPS" type="array">
|
||||
<field type="struct">
|
||||
<field name="wp_id" value="WP_TAG"/>
|
||||
<field name="tag_id" value="0"/>
|
||||
</field>
|
||||
<field type="struct">
|
||||
<field name="wp_id" value="WP_TRUCK"/>
|
||||
<field name="tag_id" value="4"/>
|
||||
</field>
|
||||
</define>
|
||||
</section>
|
||||
|
||||
<section name="MISC">
|
||||
<define name="POWER_SWITCH_GPIO" value="GPIOA,GPIO6"/>
|
||||
</section>
|
||||
|
||||
<section name="GCS">
|
||||
|
||||
@@ -0,0 +1,214 @@
|
||||
<?xml version="1.0"?>
|
||||
<!DOCTYPE flight_plan SYSTEM "../flight_plan.dtd">
|
||||
<flight_plan alt="40" ground_alt="5" home_mode_height="30" lat0="51.4045109" lon0="-2.8193286" max_dist_from_home="600" name="IMAV2024 Carto fixed-wing" qfu="250." security_height="10">
|
||||
<header>
|
||||
#include "modules/datalink/datalink.h"
|
||||
static inline bool delay_test_rc(bool test UNUSED, int delay UNUSED) {
|
||||
#if SITL
|
||||
return false;
|
||||
#else
|
||||
static int nb = 0;
|
||||
if (test) {
|
||||
nb++;
|
||||
if (nb == delay) {
|
||||
nb = 0;
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
} else {
|
||||
nb = 0;
|
||||
return false;
|
||||
}
|
||||
#endif
|
||||
}
|
||||
static inline bool delay_test_gf(bool test UNUSED, int delay UNUSED) {
|
||||
#if SITL
|
||||
return false;
|
||||
#else
|
||||
static int nb = 0;
|
||||
if (test) {
|
||||
nb++;
|
||||
if (nb == delay) {
|
||||
nb = 0;
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
} else {
|
||||
nb = 0;
|
||||
return false;
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
#define WaypointDist(_wp1, _wp2) (sqrtf(DistanceSquare(WaypointX(_wp1),WaypointY(_wp1),WaypointX(_wp2),WaypointY(_wp2))))
|
||||
|
||||
#if DIGITAL_CAM
|
||||
#define LINE_START_FUNCTION if (mapping_shot) { dc_Survey(mapping_shot_distance); } else { dc_autoshoot = DC_AUTOSHOOT_STOP; }
|
||||
#define LINE_STOP_FUNCTION dc_autoshoot = DC_AUTOSHOOT_STOP;
|
||||
#endif
|
||||
|
||||
#define NavQdrCloseToAxis(_from, _to) NavQdrCloseTo(DegOfRad(-atan2f(WaypointY(_to) - WaypointY(_from), WaypointX(_to) - WaypointX(_from))))
|
||||
</header>
|
||||
<waypoints>
|
||||
<waypoint name="HOME" x="18.462" y="-72.154"/>
|
||||
<waypoint name="STDBY" x="38.643" y="-48.229"/>
|
||||
<waypoint alt="5" name="TD" x="15.73" y="-22.048"/>
|
||||
<waypoint alt="25" name="AF" x="131.918" y="51.292"/>
|
||||
<waypoint name="_BASELEG" x="168.8" y="-13.8"/>
|
||||
<waypoint name="CLIMB" x="-64.705" y="-94.836"/>
|
||||
<!--waypoint name="P1" x="164.51" y="27.772"/>
|
||||
<waypoint name="P2" x="-41.797" y="-139.096"/>
|
||||
<waypoint name="_C1" x="151.573" y="31.161"/>
|
||||
<waypoint name="_C2" x="-55.663" y="-114.939"/-->
|
||||
<waypoint name="MAP1" lat="51.40397348780787" lon="-2.820792534133579"/>
|
||||
<waypoint name="MAP2" lat="51.40324827491988" lon="-2.822579075697425"/>
|
||||
<waypoint name="MAP3" lat="51.40216290330571" lon="-2.821736580096323"/>
|
||||
<waypoint name="MAP4" lat="51.40318633735384" lon="-2.820434907050242"/>
|
||||
<waypoint name="ZBC" x="-136.955" y="-155.073"/>
|
||||
<waypoint name="ZBDIR" x="-169.598" y="-175.953"/>
|
||||
<waypoint name="C1" lat="51.40337101177714" lon="-2.81946343972307"/>
|
||||
<waypoint name="C2" lat="51.40269828686684" lon="-2.81854499646789"/>
|
||||
<waypoint name="C3" lat="51.40412401003561" lon="-2.81582776937354"/>
|
||||
<waypoint name="C4" lat="51.40480870184149" lon="-2.81670290546800"/>
|
||||
<waypoint name="_KILL1" lat="51.4037999994502" lon="-2.81235628238941"/>
|
||||
<waypoint name="_KILL2" lat="51.4068594662325" lon="-2.81500423477735"/>
|
||||
<waypoint name="_KILL3" lat="51.4057914135146" lon="-2.81767832087734"/>
|
||||
<waypoint name="_KILL4" lat="51.4050872678795" lon="-2.81840937698165"/>
|
||||
<waypoint name="_KILL5" lat="51.4044429026898" lon="-2.81999822497422"/>
|
||||
<waypoint name="_KILL6" lat="51.4045354063380" lon="-2.82099070071843"/>
|
||||
<waypoint name="_KILL7" lat="51.4029174531834" lon="-2.82498963302300"/>
|
||||
<waypoint name="_KILL8" lat="51.3999463540515" lon="-2.82207948203042"/>
|
||||
<waypoint name="_FLY1" lat="51.40424870902749" lon="-2.820119499603871"/>
|
||||
<!--waypoint name="_FLY2" lat="51.40364806450683" lon="-2.821602369304292"/-->
|
||||
<waypoint name="_FLY3" lat="51.40277234701238" lon="-2.823731664510897"/>
|
||||
<waypoint name="_FLY4" lat="51.40074322069155" lon="-2.821774308525683"/>
|
||||
<waypoint name="_FLY5" lat="51.40400414773781" lon="-2.813566012884135"/>
|
||||
<waypoint name="_FLY6" lat="51.40610220954286" lon="-2.815401874714203"/>
|
||||
<!--waypoint name="_FLY7" lat="51.40434319438164" lon="-2.819871389267096"/-->
|
||||
</waypoints>
|
||||
<sectors>
|
||||
<sector color="red" name="Kill">
|
||||
<corner name="_KILL1"/>
|
||||
<corner name="_KILL2"/>
|
||||
<corner name="_KILL3"/>
|
||||
<corner name="_KILL4"/>
|
||||
<corner name="_KILL5"/>
|
||||
<corner name="_KILL6"/>
|
||||
<corner name="_KILL7"/>
|
||||
<corner name="_KILL8"/>
|
||||
</sector>
|
||||
<sector color="green" name="Flight_Area">
|
||||
<corner name="_FLY1"/>
|
||||
<corner name="_FLY3"/>
|
||||
<corner name="_FLY4"/>
|
||||
<corner name="_FLY5"/>
|
||||
<corner name="_FLY6"/>
|
||||
</sector>
|
||||
<sector color="yellow" name="Survey">
|
||||
<corner name="MAP1"/>
|
||||
<corner name="MAP2"/>
|
||||
<corner name="MAP3"/>
|
||||
<corner name="MAP4"/>
|
||||
</sector>
|
||||
<sector color="blue" name="Circuit">
|
||||
<corner name="C1"/>
|
||||
<corner name="C2"/>
|
||||
<corner name="C3"/>
|
||||
<corner name="C4"/>
|
||||
</sector>
|
||||
</sectors>
|
||||
<variables>
|
||||
<variable init="30." var="mapping_height" min="20." max="120." step="1."/>
|
||||
<variable init="25." var="mapping_sweep" min="5." max="20." step="0.1"/>
|
||||
<variable init="120." var="mapping_length" min="50." max="600." step="10."/>
|
||||
<variable init="3." var="mapping_lines" min="3." max="15." step="0.1"/>
|
||||
<variable init="0" var="mapping_shot" min="0" max="1" step="1"/>
|
||||
<variable init="20." var="mapping_shot_distance" min="5." max="60." step="0.1"/>
|
||||
</variables>
|
||||
<modules>
|
||||
<module name="nav" type="survey_zamboni"/>
|
||||
<module name="power_switch"/>
|
||||
</modules>
|
||||
<exceptions>
|
||||
<exception cond="(delay_test_rc(RCLost(),20)
|
||||
@AND (nav_block @GT IndexOfBlock('Holding point'))
|
||||
@AND (nav_block @LEQ IndexOfBlock('flare'))
|
||||
@AND (autopilot.launch == true))" deroute="EmergencyLanding"/>
|
||||
<exception cond="(datalink_time @GT 10
|
||||
@AND (nav_block @GT IndexOfBlock('Holding point'))
|
||||
@AND (nav_block @LEQ IndexOfBlock('flare'))
|
||||
@AND (autopilot.launch == true))" deroute="EmergencyLanding"/>
|
||||
<exception cond="(delay_test_gf(!InsideKill(GetPosX(), GetPosY()),20)
|
||||
@AND (nav_block @GT IndexOfBlock('Takeoff'))
|
||||
@AND (nav_block @LEQ IndexOfBlock('Land Right AF-TD'))
|
||||
@AND (autopilot.launch == true))" deroute="EmergencyLanding"/>
|
||||
</exceptions>
|
||||
<blocks>
|
||||
<block name="Wait GPS">
|
||||
<set value="1" var="autopilot.kill_throttle"/>
|
||||
<call_once fun="power_switch_set(true)"/>
|
||||
<while cond="!GpsFixValid()"/>
|
||||
</block>
|
||||
<block name="Geo init">
|
||||
<while cond="LessThan(NavBlockTime(), 10)"/>
|
||||
</block>
|
||||
<block name="Holding point">
|
||||
<set value="1" var="autopilot.kill_throttle"/>
|
||||
<attitude roll="0" throttle="0" vmode="throttle"/>
|
||||
</block>
|
||||
<block group="home" key="t" name="Takeoff" strip_button="Takeoff (wp CLIMB)" strip_icon="takeoff.png">
|
||||
<exception cond="GetPosAlt() @GT GetAltRef()+20" deroute="Endurance"/>
|
||||
<set value="0" var="autopilot.kill_throttle"/>
|
||||
<set value="0" var="autopilot.flight_time"/>
|
||||
<go from="HOME" pitch="30" throttle="0.8" vmode="throttle" wp="CLIMB"/>
|
||||
<deroute block="Endurance"/>
|
||||
</block>
|
||||
<block group="home" key="Ctrl+a" name="Standby" strip_button="Standby" strip_icon="home.png">
|
||||
<circle radius="nav_radius" wp="STDBY"/>
|
||||
</block>
|
||||
<block name="Endurance" strip_button="Endurance" group="mission">
|
||||
<set var="mapping_shot" value="0"/>
|
||||
<oval p1="C2" p2="C3" radius="-WaypointDist(WP_C1,WP_C2)/2." until="nav_oval_count==3"/>
|
||||
<deroute block="ZamboniSurvey"/>
|
||||
</block>
|
||||
<!--block name="Align Zamboni">
|
||||
<circle radius="nav_radius" until="NavQdrCloseToAxis(WP_STDBY,WP_ZBC) @AND fabsf(GetPosAlt() - WaypointAlt(WP_ZBC)) @LT 10." wp="STDBY"/>
|
||||
</block-->
|
||||
<block group="mission" name="ZamboniSurvey" strip_button="Zamboni">
|
||||
<set var="mapping_shot" value="1"/>
|
||||
<call_once fun="pprzlink_cam_ctrl_set_expo(PPRZLINK_CAM_AUTO_EXPO)"/>
|
||||
<call_once fun="nav_survey_zamboni_setup(WP_ZBC, WP_ZBDIR, mapping_length, -mapping_sweep, mapping_lines, GetAltRef()+mapping_height)"/>
|
||||
<call fun="nav_survey_zamboni_run()"/>
|
||||
<deroute block="Land Left AF-TD"/>
|
||||
</block>
|
||||
<block group="land" name="Land Right AF-TD" strip_button="Land right (wp AF-TD)" strip_icon="land-right.png">
|
||||
<set value="DEFAULT_CIRCLE_RADIUS" var="nav_radius"/>
|
||||
<deroute block="land"/>
|
||||
</block>
|
||||
<block group="land" name="Land Left AF-TD" strip_button="Land left (wp AF-TD)" strip_icon="land-left.png">
|
||||
<set value="-DEFAULT_CIRCLE_RADIUS" var="nav_radius"/>
|
||||
<deroute block="land"/>
|
||||
</block>
|
||||
<block name="land">
|
||||
<call_once fun="nav_compute_baseleg(WP_AF, WP_TD, WP__BASELEG, nav_radius)"/>
|
||||
<circle radius="nav_radius" until="NavCircleCount() @GT 0.5" wp="_BASELEG"/>
|
||||
<circle radius="nav_radius" until="And(NavQdrCloseTo(DegOfRad(baseleg_out_qdr)-(nav_radius/fabs(nav_radius))*10), 10 @GT fabs(GetPosAlt() - WaypointAlt(WP__BASELEG)))" wp="_BASELEG"/>
|
||||
</block>
|
||||
<block name="final">
|
||||
<exception cond="GetAltRef() + 3 @GT GetPosAlt()" deroute="flare"/>
|
||||
<go from="AF" hmode="route" vmode="glide" wp="TD"/>
|
||||
</block>
|
||||
<block name="flare">
|
||||
<go exceeding_time="10" from="AF" hmode="route" throttle="0.0" vmode="throttle" wp="TD"/>
|
||||
<attitude roll="0.0" throttle="0.0" until="FALSE" vmode="throttle"/>
|
||||
</block>
|
||||
<block name="EmergencyLanding">
|
||||
<go throttle="0." vmode="throttle" wp="HOME"/>
|
||||
<attitude roll="0" throttle="0" vmode="throttle"/>
|
||||
</block>
|
||||
<block name="SetExpo">
|
||||
<call_once fun="pprzlink_cam_ctrl_set_expo(PPRZLINK_CAM_AUTO_EXPO)"/>
|
||||
<return/>
|
||||
</block>
|
||||
</blocks>
|
||||
</flight_plan>
|
||||
@@ -0,0 +1,362 @@
|
||||
<?xml version="1.0"?>
|
||||
<!DOCTYPE flight_plan SYSTEM "../flight_plan.dtd">
|
||||
<flight_plan alt="35" ground_alt="5" home_mode_height="30" lat0="51.4045109" lon0="-2.8193286" max_dist_from_home="600" name="IMAV2024 Rotorcraft" qfu="250." security_height="10">
|
||||
<header>
|
||||
#define FP_NONE 0
|
||||
#define FP_ENDURANCE 1
|
||||
#define FP_MAPPING 2
|
||||
#define FP_DROP 4
|
||||
#define FP_IMAGES 5
|
||||
|
||||
#ifndef SwitchServoOn
|
||||
#define SwitchServoOn(_x) {}
|
||||
#endif
|
||||
#ifndef SwitchServoOff
|
||||
#define SwitchServoOff(_x) {}
|
||||
#endif
|
||||
#define DropOpen SwitchServoOn
|
||||
#define DropClose SwitchServoOff
|
||||
|
||||
#define TagID 800
|
||||
#define SetRectID jevois_send_string("set_rect_id 800")
|
||||
|
||||
#if DIGITAL_CAM
|
||||
#define LINE_START_FUNCTION dc_Survey(mapping_shot_distance);
|
||||
#define LINE_STOP_FUNCTION dc_autoshoot = DC_AUTOSHOOT_STOP;
|
||||
#endif
|
||||
|
||||
static inline bool delay_test_rc(bool test, int delay) {
|
||||
static int nb = 0;
|
||||
if (test) {
|
||||
nb++;
|
||||
if (nb == delay) {
|
||||
nb = 0;
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
} else {
|
||||
nb = 0;
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
static inline void send_image_mark(uint8_t id) {
|
||||
struct LlaCoor_f lla = *stateGetPositionLla_f();
|
||||
float lat_deg = DegOfRad(lla.lat);
|
||||
float lon_deg = DegOfRad(lla.lon);
|
||||
DOWNLINK_SEND_MARK(DefaultChannel, DefaultDevice, Ptr(id), Ptr(lat_deg), Ptr(lon_deg));
|
||||
}
|
||||
</header>
|
||||
<waypoints>
|
||||
<waypoint name="HOME" lat="51.4041653" lon="-2.8191115"/>
|
||||
<waypoint name="STDBY" lat="51.4043" lon="-2.81948"/>
|
||||
<waypoint name="LANDPAD" lat="51.4045" lon="-2.81923"/>
|
||||
<waypoint name="TAG" x="47.13" y="-3.824"/>
|
||||
<waypoint name="_TAG_SIM" x="57.13" y="-3.824"/>
|
||||
<waypoint name="_HERE" x="100.545" y="24.169"/>
|
||||
<waypoint name="MAP1" lat="51.40397348780787" lon="-2.820792534133579"/>
|
||||
<waypoint name="MAP2" lat="51.40324827491988" lon="-2.822579075697425"/>
|
||||
<waypoint name="MAP3" lat="51.4023" lon="-2.82183"/>
|
||||
<waypoint name="MAP4" lat="51.40318633735384" lon="-2.820434907050242"/>
|
||||
<waypoint name="DROP" lat="51.4045039" lon="-2.8189669"/>
|
||||
<waypoint name="START_IMAGES" lat="51.4041454" lon="-2.8197853"/>
|
||||
<waypoint name="D1" lat="51.4037624" lon="-2.8200670"/>
|
||||
<waypoint name="D2" lat="51.4035808" lon="-2.8202947"/>
|
||||
<waypoint name="D3" lat="51.4037931" lon="-2.8204535"/>
|
||||
<waypoint name="D4" lat="51.4039687" lon="-2.8204949"/>
|
||||
<waypoint name="C1" lat="51.40337101177714" lon="-2.81946343972307"/>
|
||||
<waypoint name="C2" lat="51.40269828686684" lon="-2.81854499646789"/>
|
||||
<waypoint name="C3" lat="51.40412401003561" lon="-2.81582776937354"/>
|
||||
<waypoint name="C4" lat="51.40480870184149" lon="-2.81670290546800"/>
|
||||
<waypoint name="_KILL1" lat="51.4037999994502" lon="-2.81235628238941"/>
|
||||
<waypoint name="_KILL2" lat="51.4068594662325" lon="-2.81500423477735"/>
|
||||
<waypoint name="_KILL3" lat="51.4057914135146" lon="-2.81767832087734"/>
|
||||
<waypoint name="_KILL4" lat="51.4050872678795" lon="-2.81840937698165"/>
|
||||
<waypoint name="_KILL5" lat="51.4044429026898" lon="-2.81999822497422"/>
|
||||
<waypoint name="_KILL6" lat="51.4045354063380" lon="-2.82099070071843"/>
|
||||
<waypoint name="_KILL7" lat="51.4029174531834" lon="-2.82498963302300"/>
|
||||
<waypoint name="_KILL8" lat="51.3999463540515" lon="-2.82207948203042"/>
|
||||
<waypoint name="_FLY1" lat="51.40424870902749" lon="-2.820119499603871"/>
|
||||
<!--waypoint name="_FLY2" lat="51.40364806450683" lon="-2.821602369304292"/-->
|
||||
<waypoint name="_FLY3" lat="51.40277234701238" lon="-2.823731664510897"/>
|
||||
<waypoint name="_FLY4" lat="51.40074322069155" lon="-2.821774308525683"/>
|
||||
<waypoint name="_FLY5" lat="51.40400414773781" lon="-2.813566012884135"/>
|
||||
<waypoint name="_FLY6" lat="51.40610220954286" lon="-2.815401874714203"/>
|
||||
<!--waypoint name="_FLY7" lat="51.40434319438164" lon="-2.819871389267096"/-->
|
||||
<waypoint name="_P1" lat="51.40362957556762" lon="-2.819863662028597"/>
|
||||
<waypoint name="_P2" lat="51.40429280586027" lon="-2.820004683319591"/>
|
||||
<waypoint name="_P3" lat="51.40397717175711" lon="-2.820793630630163"/>
|
||||
<waypoint name="_P4" lat="51.40359227287529" lon="-2.820623028420364"/>
|
||||
<waypoint name="_P5" lat="51.40318748775402" lon="-2.820433716921217"/>
|
||||
</waypoints>
|
||||
<sectors>
|
||||
<sector color="red" name="Kill">
|
||||
<corner name="_KILL1"/>
|
||||
<corner name="_KILL2"/>
|
||||
<corner name="_KILL3"/>
|
||||
<corner name="_KILL4"/>
|
||||
<corner name="_KILL5"/>
|
||||
<corner name="_KILL6"/>
|
||||
<corner name="_KILL7"/>
|
||||
<corner name="_KILL8"/>
|
||||
</sector>
|
||||
<sector color="green" name="Flight_Area">
|
||||
<corner name="_FLY1"/>
|
||||
<!--corner name="_FLY2"/-->
|
||||
<corner name="_FLY3"/>
|
||||
<corner name="_FLY4"/>
|
||||
<corner name="_FLY5"/>
|
||||
<corner name="_FLY6"/>
|
||||
<!--corner name="_FLY7"/-->
|
||||
</sector>
|
||||
<sector color="yellow" name="Survey">
|
||||
<corner name="MAP1"/>
|
||||
<corner name="MAP2"/>
|
||||
<corner name="MAP3"/>
|
||||
<corner name="MAP4"/>
|
||||
</sector>
|
||||
<sector color="purple" name="Photos">
|
||||
<corner name="_P1"/>
|
||||
<corner name="_P2"/>
|
||||
<corner name="_P3"/>
|
||||
<corner name="_P4"/>
|
||||
<corner name="_P5"/>
|
||||
</sector>
|
||||
<sector color="blue" name="Circuit">
|
||||
<corner name="C1"/>
|
||||
<corner name="C2"/>
|
||||
<corner name="C3"/>
|
||||
<corner name="C4"/>
|
||||
</sector>
|
||||
</sectors>
|
||||
<variables>
|
||||
<variable init="0" type="int" var="mission_nb"/>
|
||||
<variable init="20." var="goto_height" min="5." max="120." step="0.1"/>
|
||||
<variable init="7." var="goto_speed" min="0.5" max="10." step="0.1"/>
|
||||
<variable init="5." var="takeoff_height" min="0.5" max="15." step="0.1"/>
|
||||
<variable init="10." var="land_height" min="0.5" max="15." step="0.1"/>
|
||||
<variable init="4." var="land_speed" min="0.5" max="10." step="0.1"/>
|
||||
<variable init="4.5" var="tag_height" min="0.5" max="15." step="0.1"/>
|
||||
<variable init="10." var="images_height" min="0.5" max="15." step="0.1"/>
|
||||
<variable init="4." var="images_speed" min="0.5" max="10." step="0.1"/>
|
||||
<variable init="30." var="mapping_height" min="5." max="120." step="0.1"/>
|
||||
<variable init="25." var="mapping_sweep" min="5." max="20." step="0.1"/>
|
||||
<variable init="7." var="mapping_speed" min="0.5" max="10." step="0.1"/>
|
||||
<variable init="-1." var="mapping_radius" min="-1." max="100." step="1.0"/>
|
||||
<variable init="20." var="mapping_shot_distance" min="5." max="60." step="0.1"/>
|
||||
<variable init="false" type="bool" var="stay_on_wp" min="0" max="1" step="1"/>
|
||||
</variables>
|
||||
<modules>
|
||||
<module name="nav" type="survey_hybrid">
|
||||
<define name="SURVEY_HYBRID_MAX_SWEEP_BACK" value="1"/>
|
||||
<define name="SURVEY_HYBRID_APPROACHING_TIME" value="0"/>
|
||||
<define name="SURVEY_HYBRID_ENTRY_DISTANCE" value="survey_private.sweep_distance"/>
|
||||
<define name="SURVEY_HYBRID_ENTRY_CIRCLE" value="FALSE"/>
|
||||
</module>
|
||||
<module name="tag_tracking">
|
||||
<define name="TAG_TRACKING_SIM_WP" value="WP__TAG_SIM"/>
|
||||
</module>
|
||||
</modules>
|
||||
<exceptions>
|
||||
<exception cond="(!InsideFlight_Area(GetPosX(), GetPosY()) @OR (GetPosAlt() @GT GetAltRef() + 100))
|
||||
@AND (nav_block @GT IndexOfBlock('Holding point'))
|
||||
@AND (nav_block @LT IndexOfBlock('Kill landed'))" deroute="Standby"/>
|
||||
<exception cond="(!InsideKill(GetPosX(), GetPosY()) @OR (GetPosAlt() @GT GetAltRef() + 120))
|
||||
@AND (nav_block @GT IndexOfBlock('Holding point'))
|
||||
@AND (nav_block @LT IndexOfBlock('Kill landed'))" deroute="Kill landed"/>
|
||||
<exception cond="(nav_block @GT IndexOfBlock('Holding point') @AND nav_block @LT IndexOfBlock('Kill landed'))
|
||||
@AND (GpsIsLost() @AND delay_test_rc(RadioControlIsLost(),20)
|
||||
@AND (datalink_time @GT 5))" deroute="Kill landed"/>
|
||||
<exception cond="(nav_block @GT IndexOfBlock('Holding point') @AND nav_block @LT IndexOfBlock('Kill landed'))
|
||||
@AND (delay_test_rc(RadioControlIsLost(),20)
|
||||
@AND (datalink_time @GT 15))" deroute="Kill landed"/>
|
||||
</exceptions>
|
||||
<blocks>
|
||||
<block name="Wait GPS">
|
||||
<call_once fun="NavKillThrottle()"/>
|
||||
<call_once fun="jevois_stream(false)"/>
|
||||
<call_once fun="power_switch_set(false)"/>
|
||||
<call_once fun="tag_tracking_tag_tracking_report_status = MODULES_STOP"/>
|
||||
<while cond="!GpsFixValid()"/>
|
||||
</block>
|
||||
<block name="Geo init">
|
||||
<while cond="LessThan(NavBlockTime(), 10)"/>
|
||||
</block>
|
||||
<block name="Holding point" strip_button="H. 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="GetPosHeight() @GT takeoff_height @AND mission_nb == FP_NONE" deroute="Standby"/>
|
||||
<exception cond="GetPosHeight() @GT takeoff_height @AND mission_nb == FP_ENDURANCE" deroute="Goto Endurance"/>
|
||||
<exception cond="GetPosHeight() @GT takeoff_height @AND mission_nb == FP_MAPPING" deroute="Goto Mapping"/>
|
||||
<exception cond="GetPosHeight() @GT takeoff_height @AND mission_nb == FP_DROP" deroute="Goto Drop"/>
|
||||
<exception cond="GetPosHeight() @GT takeoff_height @AND mission_nb == FP_IMAGES" deroute="Goto Images"/>
|
||||
<call_once fun="ins_reset_vertical_pos()"/>
|
||||
<call_once fun="NavResurrect()"/>
|
||||
<attitude pitch="0" roll="0" throttle="0.1" until="stage_time @GT 2" vmode="throttle"/>
|
||||
<call_once fun="NavSetWaypointHere(WP_LANDPAD)"/>
|
||||
<stay climb="nav.climb_vspeed" vmode="climb" wp="LANDPAD"/>
|
||||
</block>
|
||||
<block name="Standby" strip_button="Standby" strip_icon="home.png" group="home">
|
||||
<set var="mission_nb" value="FP_NONE"/>
|
||||
<call_once fun="guidance_h_SetMaxSpeed(land_speed)"/>
|
||||
<stay wp="STDBY"/>
|
||||
</block>
|
||||
|
||||
<block group="mission" name="Start Endurance" strip_button="Endurance">
|
||||
<set var="mission_nb" value="FP_ENDURANCE"/>
|
||||
<deroute block="Takeoff"/>
|
||||
</block>
|
||||
<block group="mission" name="Start Mapping" strip_button="Mapping">
|
||||
<set var="mission_nb" value="FP_MAPPING"/>
|
||||
<deroute block="Takeoff"/>
|
||||
</block>
|
||||
<block group="mission" name="Start Drop" strip_button="Drop">
|
||||
<set var="mission_nb" value="FP_DROP"/>
|
||||
<deroute block="Takeoff"/>
|
||||
</block>
|
||||
<block group="mission" name="Start Images" strip_button="Images">
|
||||
<set var="mission_nb" value="FP_IMAGES"/>
|
||||
<deroute block="Takeoff"/>
|
||||
</block>
|
||||
|
||||
<block name="Goto Endurance">
|
||||
<set var="mission_nb" value="FP_NONE"/>
|
||||
<call_once fun="NavSetWaypointHere(WP__HERE)"/>
|
||||
<call_once fun="jevois_stream(false)"/>
|
||||
<call_once fun="power_switch_set(false)"/>
|
||||
<call_once fun="guidance_h_SetMaxSpeed(mapping_speed)"/>
|
||||
<stay wp="_HERE" until="stage_time @GT 3" height="mapping_height"/>
|
||||
<go wp="C1" from="_HERE" hmode="route" height="mapping_height"/>
|
||||
<deroute block="Run Endurance"/>
|
||||
</block>
|
||||
<block name="Run Endurance">
|
||||
<call_once fun="guidance_h_SetMaxSpeed(mapping_speed)"/>
|
||||
<for var="i" from="1" to="3">
|
||||
<path wpts="C1,C2,C3,C4,C1" height="mapping_height"/>
|
||||
</for>
|
||||
<deroute block="Goto Mapping"/>
|
||||
</block>
|
||||
|
||||
<block name="Goto Mapping">
|
||||
<set var="mission_nb" value="FP_NONE"/>
|
||||
<!--call_once fun="NavSetWaypointHere(WP__HERE)"/-->
|
||||
<call_once fun="jevois_stream(false)"/>
|
||||
<call_once fun="power_switch_set(false)"/>
|
||||
<call_once fun="guidance_h_SetMaxSpeed(mapping_speed)"/>
|
||||
<!--stay wp="_HERE" until="stage_time @GT 3" height="mapping_height"/>
|
||||
<go wp="MAP1" from="_HERE" hmode="route" height="mapping_height"/-->
|
||||
<call_once fun="pprzlink_cam_ctrl_set_expo(PPRZLINK_CAM_AUTO_EXPO)"/>
|
||||
<call_once fun="nav_survey_hybrid_setup_towards(WP_MAP1, WP_MAP2, 4, mapping_sweep, mapping_radius, mapping_height)"/>
|
||||
<deroute block="Run Mapping"/>
|
||||
</block>
|
||||
<block name="Run Mapping">
|
||||
<call_once fun="guidance_h_SetMaxSpeed(mapping_speed)"/>
|
||||
<call fun="nav_survey_hybrid_run()"/>
|
||||
<deroute block="Landing"/>
|
||||
</block>
|
||||
|
||||
<block name="Goto Drop">
|
||||
<set var="mission_nb" value="FP_NONE"/>
|
||||
<call_once fun="NavSetWaypointHere(WP__HERE)"/>
|
||||
<call_once fun="jevois_stream(false)"/>
|
||||
<call_once fun="power_switch_set(true)"/>
|
||||
<call_once fun="tag_tracking_set_tracker_id(TagID, WP_TAG)"/>
|
||||
<call_once fun="SetRectID"/>
|
||||
<call_once fun="guidance_h_SetMaxSpeed(land_speed)"/>
|
||||
<stay wp="_HERE" until="stage_time @GT 3" height="images_height"/>
|
||||
<go wp="DROP" from="_HERE" hmode="route" height="images_height"/>
|
||||
<call_once fun="guidance_h_SetMaxSpeed(land_speed)"/>
|
||||
<call_once fun="waypoint_position_copy(WP_TAG, WP_DROP)"/>
|
||||
<call_once fun="jevois_stream(true)"/>
|
||||
<stay wp="DROP" until="(tag_tracking_get_status(TAG_TRACKING_ANY) == TAG_TRACKING_RUNNING) @OR (stage_time @GT 15)" height="tag_height"/>
|
||||
<deroute block="Drop Camera"/>
|
||||
</block>
|
||||
<block name="Drop Camera">
|
||||
<stay wp="TAG" until="(stage_time @GT 2 @AND tag_tracking_get_status(TAG_TRACKING_ANY) == TAG_TRACKING_RUNNING) @OR (stage_time @GT 5)" height="tag_height"/>
|
||||
<stay wp="TAG" climb="-0.5" vmode="climb" until="!nav_is_in_flight() @OR (stage_time @GT 30)"/>
|
||||
<call_once fun="NavKillThrottle()"/>
|
||||
<call_once fun="DropOpen()"/>
|
||||
<attitude pitch="0" roll="0" throttle="0" until="stage_time @GT 2" vmode="throttle"/>
|
||||
<call_once fun="NavResurrect()"/>
|
||||
<attitude pitch="0" roll="0" throttle="0" until="stage_time @GT 2" vmode="throttle"/>
|
||||
<stay climb="nav.climb_vspeed" vmode="climb" wp="TAG" until="GetPosHeight() @GT takeoff_height"/>
|
||||
<stay wp="TAG" until="stage_time @GT 3" height="images_height"/>
|
||||
<deroute block="Goto Images"/>
|
||||
</block>
|
||||
|
||||
<block name="Goto Images">
|
||||
<set var="mission_nb" value="FP_NONE"/>
|
||||
<call_once fun="NavSetWaypointHere(WP__HERE)"/>
|
||||
<call_once fun="jevois_stream(false)"/>
|
||||
<call_once fun="power_switch_set(true)"/>
|
||||
<call_once fun="guidance_h_SetMaxSpeed(goto_speed)"/>
|
||||
<stay wp="_HERE" until="stage_time @GT 3" alt="images_height"/>
|
||||
<go from="_HERE" hmode="route" wp="START_IMAGES" alt="images_height"/>
|
||||
<deroute block="Waiting WP"/>
|
||||
</block>
|
||||
<block name="Waiting WP">
|
||||
<call_once fun="guidance_h_SetMaxSpeed(images_speed)"/>
|
||||
<stay wp="START_IMAGES" alt="images_height"/>
|
||||
</block>
|
||||
<block name="Image 1" strip_button="Image 1" group="images">
|
||||
<call_once fun="NavSetWaypointHere(WP__HERE)"/>
|
||||
<go from="_HERE" hmode="route" wp="D1" alt="images_height"/>
|
||||
<call_once fun="send_image_mark(1)"/>
|
||||
<stay wp="D1" alt="images_height" until="stage_time @GT 3 @AND !stay_on_wp"/>
|
||||
</block>
|
||||
<block name="Image 2" strip_button="Image 2" group="images">
|
||||
<call_once fun="NavSetWaypointHere(WP__HERE)"/>
|
||||
<go from="_HERE" hmode="route" wp="D2" alt="images_height"/>
|
||||
<call_once fun="send_image_mark(2)"/>
|
||||
<stay wp="D2" alt="images_height" until="stage_time @GT 3 @AND !stay_on_wp"/>
|
||||
</block>
|
||||
<block name="Image 3" strip_button="Image 3" group="images">
|
||||
<call_once fun="NavSetWaypointHere(WP__HERE)"/>
|
||||
<go from="_HERE" hmode="route" wp="D3" alt="images_height"/>
|
||||
<call_once fun="send_image_mark(3)"/>
|
||||
<stay wp="D3" alt="images_height" until="stage_time @GT 3 @AND !stay_on_wp"/>
|
||||
</block>
|
||||
<block name="Image 4" strip_button="Image 4" group="images">
|
||||
<call_once fun="NavSetWaypointHere(WP__HERE)"/>
|
||||
<go from="_HERE" hmode="route" wp="D4" alt="images_height"/>
|
||||
<call_once fun="send_image_mark(4)"/>
|
||||
<stay wp="D4" alt="images_height" until="stage_time @GT 3 @AND !stay_on_wp"/>
|
||||
<deroute block="Landing"/>
|
||||
</block>
|
||||
|
||||
<block name="Land here" strip_button="Land here" group="land">
|
||||
<call_once fun="NavSetWaypointHere(WP_LANDPAD)"/>
|
||||
<deroute block="Flare"/>
|
||||
</block>
|
||||
<block name="Landing" strip_button="Land on pad" group="land">
|
||||
<call_once fun="jevois_stream(false)"/>
|
||||
<call_once fun="dc_send_command(DC_OFF)"/>
|
||||
<call_once fun="NavSetWaypointHere(WP__HERE)"/>
|
||||
<call_once fun="guidance_h_SetMaxSpeed(goto_speed)"/>
|
||||
<go wp="STDBY" from="_HERE" hmode="route" height="land_height"/>
|
||||
<call_once fun="guidance_h_SetMaxSpeed(land_speed)"/>
|
||||
<go wp="LANDPAD" from="STDBY" hmode="route" height="land_height"/>
|
||||
<deroute block="Flare"/>
|
||||
</block>
|
||||
<block name="Flare">
|
||||
<exception cond="!nav_is_in_flight()" deroute="Kill landed"/>
|
||||
<call_once fun="guidance_h_SetMaxSpeed(land_speed)"/>
|
||||
<call_once fun="jevois_stream(true)"/>
|
||||
<stay climb="2*nav.descend_vspeed" vmode="climb" wp="LANDPAD" until="GetPosHeight() @LT 10"/>
|
||||
<stay climb="nav.descend_vspeed" vmode="climb" wp="LANDPAD"/>
|
||||
</block>
|
||||
<block name="Kill landed">
|
||||
<call_once fun="power_switch_set(false)"/>
|
||||
<call_once fun="NavKillThrottle()"/>
|
||||
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
|
||||
</block>
|
||||
|
||||
</blocks>
|
||||
|
||||
</flight_plan>
|
||||
@@ -214,6 +214,7 @@ wpts CDATA #REQUIRED
|
||||
vmode CDATA #IMPLIED
|
||||
pitch CDATA #IMPLIED
|
||||
alt CDATA #IMPLIED
|
||||
height CDATA #IMPLIED
|
||||
approaching_time CDATA #IMPLIED
|
||||
exceeding_time CDATA #IMPLIED
|
||||
throttle CDATA #IMPLIED
|
||||
|
||||
@@ -0,0 +1,19 @@
|
||||
<!DOCTYPE airframe SYSTEM "../airframes/airframe.dtd">
|
||||
|
||||
<airframe>
|
||||
<description>
|
||||
Bristol, UK
|
||||
Model Used: WMM-2020 More information
|
||||
Latitude: 51.4045109° N
|
||||
Longitude: 2.8193286° W
|
||||
Elevation: 0.0 m Mean Sea Level
|
||||
Date Declination (+E|-W) Inclination (+D|-U) Horizontal Intensity North Comp (+N|-S) East Comp (+E|-W) Vertical Comp (+D|-U) Total Field
|
||||
2024-09-15 0.0454° 66.3050° 19,675.8 nT 19,675.8 nT 15.6 nT 44,833.4 nT 48,960.9 nT
|
||||
</description>
|
||||
<section name="MAG_MODEL" prefix="INS_">
|
||||
<define name="H_X" value="0.401868"/>
|
||||
<define name="H_Y" value="0.000318622"/>
|
||||
<define name="H_Z" value="0.915698"/>
|
||||
</section>
|
||||
</airframe>
|
||||
|
||||
@@ -20,7 +20,7 @@
|
||||
<settings>
|
||||
<dl_settings name="control">
|
||||
<dl_settings name="dc">
|
||||
|
||||
<dl_setting max="20" min="0" step="0.1" module="digital_cam/dc" handler="set_expo" var="dc_exposure" shortname="exposure"/>
|
||||
<dl_setting max="255" min="0" step="1" module="digital_cam/dc" var="0" handler="send_command" shortname="Shutter">
|
||||
<strip_button name="Photo" icon="digital-camera.png" value="32" group="maindc"/>
|
||||
<strip_button name="Power" icon="off.png" value="111" group="maindc"/>
|
||||
|
||||
@@ -61,6 +61,10 @@ static void tag_motion_sim(void);
|
||||
#define TAG_MOTION_RANGE_X 4.f
|
||||
#define TAG_MOTION_RANGE_Y 4.f
|
||||
|
||||
#ifndef TAG_TRACKING_SIM_ID
|
||||
#define TAG_TRACKING_SIM_ID "U1"
|
||||
#endif
|
||||
|
||||
static uint8_t tag_motion_sim_type = TAG_MOTION_NONE;
|
||||
static struct FloatVect3 tag_motion_speed = { TAG_MOTION_SPEED_X, TAG_MOTION_SPEED_Y, 0.f };
|
||||
|
||||
@@ -179,8 +183,8 @@ struct tag_info {
|
||||
};
|
||||
|
||||
struct wp_tracking {
|
||||
uint8_t wp_id;
|
||||
int16_t tag_id;
|
||||
uint8_t wp_id;
|
||||
};
|
||||
|
||||
|
||||
@@ -215,7 +219,7 @@ struct tag_tracking_public* tag_tracking_get(int16_t tag_id) {
|
||||
}
|
||||
|
||||
// tag_id == TAG_TRACKING_ANY, returns the first running tag.
|
||||
if(tag_id == TAG_TRACKING_ANY && tag_infos[i].tag_tracking.status == TAG_TRACKING_RUNNING) {
|
||||
if (tag_id == TAG_TRACKING_ANY && tag_infos[i].tag_tracking.status == TAG_TRACKING_RUNNING) {
|
||||
return &tag_infos[i].tag_tracking;
|
||||
}
|
||||
}
|
||||
@@ -225,6 +229,16 @@ struct tag_tracking_public* tag_tracking_get(int16_t tag_id) {
|
||||
return &dummy;
|
||||
}
|
||||
|
||||
uint8_t tag_tracking_get_status(int16_t tag_id)
|
||||
{
|
||||
return tag_tracking_get(tag_id)->status;
|
||||
}
|
||||
|
||||
uint8_t tag_tracking_get_motion_type(int16_t tag_id)
|
||||
{
|
||||
return tag_tracking_get(tag_id)->motion_type;
|
||||
}
|
||||
|
||||
float tag_tracking_get_heading(int16_t tag_id) {
|
||||
struct tag_tracking_public* tag = tag_tracking_get(tag_id);
|
||||
struct FloatEulers e;
|
||||
@@ -278,13 +292,13 @@ static void tag_track_cb(uint8_t sender_id UNUSED,
|
||||
{
|
||||
if (type == JEVOIS_MSG_D3) {
|
||||
int16_t tag_id = (int16_t)jevois_extract_nb(id);
|
||||
for(int i=0; i<TAG_TRACKING_NB_MAX; i++) {
|
||||
for (int i=0; i<TAG_TRACKING_NB_MAX; i++) {
|
||||
// free slot, store tag ID
|
||||
if(tag_infos[i].tag_track_private.id == TAG_UNUSED_ID) {
|
||||
if (tag_infos[i].tag_track_private.id == TAG_UNUSED_ID) {
|
||||
tag_infos[i].tag_track_private.id = tag_id;
|
||||
}
|
||||
|
||||
if(tag_infos[i].tag_track_private.id == tag_id) {
|
||||
if (tag_infos[i].tag_track_private.id == tag_id) {
|
||||
// store data from Jevois detection
|
||||
tag_infos[i].tag_track_private.meas.x = coord[0] * TAG_TRACKING_COORD_TO_M;
|
||||
tag_infos[i].tag_track_private.meas.y = coord[1] * TAG_TRACKING_COORD_TO_M;
|
||||
@@ -324,9 +338,7 @@ void tag_tracking_parse_target_pos(uint8_t *buf)
|
||||
// Update and display tracking WP
|
||||
static void update_wp(struct tag_info* tag_info UNUSED, bool report UNUSED)
|
||||
{
|
||||
#ifdef TAG_TRACKING_WPS
|
||||
|
||||
if(tag_info->wp_id == 0) {
|
||||
if (tag_info->wp_id == 0) {
|
||||
// not associated with any WP
|
||||
return;
|
||||
}
|
||||
@@ -348,8 +360,6 @@ static void update_wp(struct tag_info* tag_info UNUSED, bool report UNUSED)
|
||||
} else {
|
||||
waypoint_set_enu_i(tag_info->wp_id, &pos_i);
|
||||
}
|
||||
|
||||
#endif
|
||||
}
|
||||
|
||||
// Init function
|
||||
@@ -498,7 +508,7 @@ void tag_tracking_report()
|
||||
*/
|
||||
void tag_tracking_compute_speed(void)
|
||||
{
|
||||
for(int i=0; i<TAG_TRACKING_NB_MAX; i++) {
|
||||
for (int i = 0; i < TAG_TRACKING_NB_MAX; i++) {
|
||||
if (tag_infos[i].tag_tracking.status == TAG_TRACKING_RUNNING) {
|
||||
// compute speed command as estimated tag speed + gain * position error
|
||||
struct NedCoor_f pos = *stateGetPositionNed_f();
|
||||
@@ -515,6 +525,18 @@ void tag_tracking_compute_speed(void)
|
||||
}
|
||||
}
|
||||
|
||||
bool tag_tracking_set_tracker_id(int16_t tag_id, uint8_t wp_id)
|
||||
{
|
||||
for (int i = 0; i < TAG_TRACKING_NB_MAX; i++) {
|
||||
if (tag_infos[i].tag_track_private.id == TAG_UNUSED_ID || tag_infos[i].tag_track_private.id == tag_id) {
|
||||
tag_infos[i].tag_track_private.id = tag_id;
|
||||
tag_infos[i].wp_id = wp_id;
|
||||
return true;
|
||||
}
|
||||
}
|
||||
return false; // fail to set tracker id
|
||||
}
|
||||
|
||||
// Simulate detection using a WP coordinate
|
||||
#if defined SITL && defined TAG_TRACKING_SIM_WP
|
||||
static void tag_tracking_sim(void)
|
||||
@@ -551,7 +573,7 @@ static void tag_tracking_sim(void)
|
||||
uint16_t dim[3] = { 100, 100, 0 };
|
||||
struct FloatQuat quat; // TODO
|
||||
float_quat_identity(&quat);
|
||||
AbiSendMsgJEVOIS_MSG(42, JEVOIS_MSG_D3, "1", 3, coord, dim, quat, "");
|
||||
AbiSendMsgJEVOIS_MSG(42, JEVOIS_MSG_D3, TAG_TRACKING_SIM_ID, 3, coord, dim, quat, "");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -78,13 +78,16 @@ void tag_tracking_set_kp(float kp);
|
||||
void tag_tracking_set_kpz(float kpz);
|
||||
|
||||
extern struct tag_tracking_public* tag_tracking_get(int16_t tag_id);
|
||||
extern uint8_t tag_tracking_get_status(int16_t tag_id);
|
||||
extern uint8_t tag_tracking_get_motion_type(int16_t tag_id);
|
||||
extern void tag_tracking_init(void);
|
||||
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);
|
||||
float tag_tracking_get_heading(int16_t tag_id);
|
||||
extern bool tag_tracking_set_tracker_id(int16_t tag_id, uint8_t wp_id);
|
||||
extern float tag_tracking_get_heading(int16_t tag_id);
|
||||
|
||||
#endif // TAG_TRACKING_H
|
||||
|
||||
|
||||
@@ -69,6 +69,8 @@ uint16_t dc_gps_count = 0;
|
||||
uint8_t dc_cam_tracing = 1;
|
||||
float dc_cam_angle = 0;
|
||||
|
||||
float dc_exposure;
|
||||
|
||||
float dc_circle_interval = 0;
|
||||
float dc_circle_start_angle = 0;
|
||||
float dc_circle_last_block = 0;
|
||||
@@ -155,6 +157,7 @@ void dc_send_shot_position(void)
|
||||
|
||||
void dc_init(void)
|
||||
{
|
||||
dc_exposure = 0.f;
|
||||
dc_autoshoot = DC_AUTOSHOOT_STOP;
|
||||
dc_autoshoot_period = DC_AUTOSHOOT_PERIOD;
|
||||
dc_distance_interval = DC_AUTOSHOOT_DISTANCE_INTERVAL;
|
||||
@@ -195,6 +198,8 @@ void dc_send_command_common(uint8_t cmd __attribute__((unused)))
|
||||
#endif
|
||||
}
|
||||
|
||||
// weak function for set_expo, to be implemented by drivers if available
|
||||
void WEAK dc_set_expo(float expo UNUSED) {}
|
||||
|
||||
/* shoot on distance */
|
||||
uint8_t dc_distance(float interval)
|
||||
|
||||
@@ -48,6 +48,8 @@ extern uint16_t dc_photo_nr;
|
||||
/** number of images taken since the last change of dc_mode */
|
||||
extern uint16_t dc_gps_count;
|
||||
|
||||
/** camera exposure */
|
||||
extern float dc_exposure;
|
||||
|
||||
/*
|
||||
* Variables for PERIODIC mode.
|
||||
@@ -123,6 +125,9 @@ extern void dc_send_command(uint8_t cmd);
|
||||
/** Command sending function */
|
||||
extern void dc_send_command_common(uint8_t cmd);
|
||||
|
||||
/** Set camera exposure */
|
||||
extern void dc_set_expo(float expo);
|
||||
|
||||
/** Auotmatic Digital Camera Photo Triggering modes */
|
||||
typedef enum {
|
||||
DC_AUTOSHOOT_STOP = 0,
|
||||
|
||||
@@ -29,11 +29,9 @@
|
||||
// Include Standard Camera Control Interface
|
||||
#include "dc.h"
|
||||
|
||||
float digital_cam_exposure;
|
||||
|
||||
void pprzlink_cam_ctrl_init(void)
|
||||
{
|
||||
digital_cam_exposure = 0.f; // auto expo
|
||||
dc_exposure = PPRZLINK_CAM_AUTO_EXPO;
|
||||
}
|
||||
|
||||
void pprzlink_cam_ctrl_periodic(void)
|
||||
@@ -57,9 +55,9 @@ void dc_send_command(uint8_t cmd)
|
||||
#include "modules/datalink/downlink.h"
|
||||
#include "modules/datalink/extra_pprz_dl.h"
|
||||
|
||||
void pprzlink_cam_ctrl_set_expo(float expo)
|
||||
void dc_set_expo(float expo)
|
||||
{
|
||||
digital_cam_exposure = expo;
|
||||
dc_exposure = expo;
|
||||
uint8_t tab[2];
|
||||
tab[0] = 'e';
|
||||
tab[1] = (uint8_t)(expo * 10.f);
|
||||
@@ -72,7 +70,7 @@ void dc_expo_cb(uint8_t* buf) {
|
||||
|
||||
// feedback from camera
|
||||
if (DL_PAYLOAD_COMMAND_command_length(buf) == 2 && DL_PAYLOAD_COMMAND_command(buf)[0] == 'e') {
|
||||
digital_cam_exposure = DL_PAYLOAD_COMMAND_command(buf)[1] / 10.0;
|
||||
dc_exposure = DL_PAYLOAD_COMMAND_command(buf)[1] / 10.0;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -32,7 +32,6 @@ extern void pprzlink_cam_ctrl_init(void);
|
||||
extern void pprzlink_cam_ctrl_periodic(void);
|
||||
|
||||
/** Set expo setting */
|
||||
extern float digital_cam_exposure;
|
||||
extern void pprzlink_cam_ctrl_set_expo(float expo);
|
||||
extern void dc_expo_cb(uint8_t* buf);
|
||||
#define PPRZLINK_CAM_AUTO_EXPO 0.f
|
||||
|
||||
@@ -60,4 +60,7 @@ void nav_goto_block(uint8_t block_id);
|
||||
/** Time in s since the entrance in the current block */
|
||||
#define NavBlockTime() (block_time)
|
||||
|
||||
/** macro to use pointers in flight plans XML */
|
||||
#define Ptr(x) (&(x))
|
||||
|
||||
#endif /* COMMON_FLIGHT_PLAN_H */
|
||||
|
||||
@@ -69,6 +69,11 @@
|
||||
#define SURVEY_HYBRID_ENTRY_DISTANCE (survey_private.sweep_distance / 2.f)
|
||||
#endif
|
||||
|
||||
// make a circle at entry point if radius is not 0
|
||||
#ifndef SURVEY_HYBRID_ENTRY_CIRCLE
|
||||
#define SURVEY_HYBRID_ENTRY_CIRCLE TRUE
|
||||
#endif
|
||||
|
||||
struct Line {float m; float b; float x;};
|
||||
enum SurveyStatus { Init, Entry, Sweep, Turn };
|
||||
|
||||
@@ -417,7 +422,7 @@ bool nav_survey_hybrid_run(void)
|
||||
RotateAndTranslateToWorld(&C, 0.f, &survey_private.smallest_corner);
|
||||
RotateAndTranslateToWorld(&C, survey_private.orientation, &zero);
|
||||
|
||||
if (survey_private.circle_turns) {
|
||||
if (survey_private.circle_turns && SURVEY_HYBRID_ENTRY_CIRCLE) {
|
||||
// align segment at entry point with a circle
|
||||
survey_private.circle.x = C.x - (cosf(survey_private.orientation + M_PI_2) * survey_private.radius);
|
||||
survey_private.circle.y = C.y - (sinf(survey_private.orientation + M_PI_2) * survey_private.radius);
|
||||
|
||||
@@ -376,7 +376,10 @@ static void jevois_parse(struct jevois_t *jv, char c)
|
||||
if (c == '\n') {
|
||||
jevois_handle_msg(jv);
|
||||
jv->state = JV_TYPE;
|
||||
} else {
|
||||
} else if( c=='\r') {
|
||||
// do nothing, just to consume '\r'
|
||||
}
|
||||
else {
|
||||
jv->state = JV_SYNC;
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user