mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-29 02:38:07 +08:00
Merge pull request #2813 from enacuavlab/nephelae_meteo_mission-integration
[nav] add adaptive navigation patterns
This commit is contained in:
Hector Garcia de Marina
GitHub
@@ -204,6 +204,13 @@
|
||||
<field name="throttle" type="int16_t">Throttle input in pprz_t [0;9600] or [-9600;9600] (for vertical speed control for instance)</field>
|
||||
</message>
|
||||
|
||||
<message name="PAYLOAD_DATA" id="29">
|
||||
<field name="stamp" type="uint32_t" unit="us">Timestamp in micro-seconds</field>
|
||||
<field name="data_type" type="int32_t">A data type id that can be used to identify the data format</field>
|
||||
<field name="size" type="uint32_t">Data size or number of element, this may depend on the sender and the type of data</field>
|
||||
<field name="data" type="uint8_t *">Pointer to an array of bytes, or any structure depending on the sender and the type of data</field>
|
||||
</message>
|
||||
|
||||
</msg_class>
|
||||
|
||||
</protocol>
|
||||
|
||||
@@ -0,0 +1,116 @@
|
||||
<!DOCTYPE flight_plan SYSTEM "flight_plan.dtd">
|
||||
|
||||
<flight_plan alt="260" ground_alt="185" lat0="43.46223" lon0="1.27289" max_dist_from_home="15000" name="Cloud Sim" security_height="25">
|
||||
<header/>
|
||||
<waypoints>
|
||||
<waypoint name="HOME" x="0" y="0"/>
|
||||
<waypoint name="STDBY" x="49.5" y="100.1"/>
|
||||
<waypoint name="CLOUD" x="154.4" y="156.5"/>
|
||||
<waypoint name="C1" x="206.9" y="464.7"/>
|
||||
<waypoint name="C2" x="469.1" y="379.4"/>
|
||||
<waypoint name="C3" x="485.7" y="166.1"/>
|
||||
<waypoint name="C4" x="201.2" y="113.1"/>
|
||||
<waypoint name="C5" x="28.3" y="361.1"/>
|
||||
<waypoint name="O1" x="-202.3" y="366.7"/>
|
||||
<waypoint name="O2" x="-134.9" y="221.7"/>
|
||||
<waypoint name="TARGET" x="120." y="120."/>
|
||||
<waypoint alt="215.0" name="AF" x="177.4" y="45.1"/>
|
||||
<waypoint alt="185.0" name="TD" x="28.8" y="57.0"/>
|
||||
<waypoint name="_BASELEG" x="168.8" y="-13.8"/>
|
||||
<waypoint name="CLIMB" x="-114.5" y="162.3"/>
|
||||
</waypoints>
|
||||
<sectors>
|
||||
<sector name="Cloud" color="white">
|
||||
<corner name="C1"/>
|
||||
<corner name="C2"/>
|
||||
<corner name="C3"/>
|
||||
<corner name="C4"/>
|
||||
<corner name="C5"/>
|
||||
</sector>
|
||||
</sectors>
|
||||
<variables>
|
||||
<variable init="80." max="500." min="80." step="1.0" type="float" var="fp_radius"/>
|
||||
<variable init="0." max="10." min="-10." step="0.1" type="float" var="fp_vx"/>
|
||||
<variable init="0." max="10." min="-10." step="0.1" type="float" var="fp_vy"/>
|
||||
<variable init="0." max="10." min="-10." step="0.1" type="float" var="fp_vz"/>
|
||||
</variables>
|
||||
<modules>
|
||||
<module name="cloud_sim">
|
||||
<define name="CLOUD_SIM_MODE" value="CLOUD_SIM_POLYGON"/>
|
||||
</module>
|
||||
<module name="nav" type="lace"/>
|
||||
<module name="nav" type="rosette"/>
|
||||
<module name="nav" type="trinity"/>
|
||||
<module name="nav" type="spiral_3D"/>
|
||||
<module name="mission" type="fw"/>
|
||||
</modules>
|
||||
<blocks>
|
||||
<block name="Wait GPS">
|
||||
<set value="1" var="autopilot.kill_throttle"/>
|
||||
<while cond="!GpsFixValid()"/>
|
||||
</block>
|
||||
<block name="Geo init">
|
||||
<while cond="LessThan(NavBlockTime(), 10)"/>
|
||||
<call_once fun="NavSetGroundReferenceHere()"/>
|
||||
</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() > GetAltRef()+25" deroute="Standby"/>
|
||||
<set value="0" var="autopilot.kill_throttle"/>
|
||||
<set value="0" var="autopilot.flight_time"/>
|
||||
<go from="HOME" pitch="15" throttle="1.0" vmode="throttle" wp="CLIMB"/>
|
||||
</block>
|
||||
<block group="home" key="Ctrl+a" name="Standby" strip_button="Standby" strip_icon="home.png">
|
||||
<circle radius="nav_radius" wp="STDBY"/>
|
||||
</block>
|
||||
<block group="adaptive" name="Trinity" strip_button="Trinity">
|
||||
<call_once fun="nav_trinity_setup(WaypointX(WP_TARGET), WaypointY(WP_TARGET), WaypointAlt(WP_TARGET)-GetAltRef(), 1, fp_radius, fp_vx, fp_vy, fp_vz)"/>
|
||||
<call fun="nav_trinity_run()"/>
|
||||
<deroute block="Standby"/>
|
||||
</block>
|
||||
<block group="adaptive" name="Lace" strip_button="Lace">
|
||||
<call_once fun="nav_lace_setup(WaypointX(WP_TARGET), WaypointY(WP_TARGET), WaypointAlt(WP_TARGET)-GetAltRef(), 1, fp_radius, fp_vx, fp_vy, fp_vz)"/>
|
||||
<call fun="nav_lace_run()"/>
|
||||
<deroute block="Standby"/>
|
||||
</block>
|
||||
<block group="adaptive" name="Rosette" strip_button="Rosette">
|
||||
<call_once fun="nav_rosette_setup(WaypointX(WP_TARGET), WaypointY(WP_TARGET), WaypointAlt(WP_TARGET)-GetAltRef(), 1, fp_radius, fp_vx, fp_vy, fp_vz)"/>
|
||||
<call fun="nav_rosette_run()"/>
|
||||
<deroute block="Standby"/>
|
||||
</block>
|
||||
<block group="adaptive" name="Mission" strip_button="Mission">
|
||||
<call fun="mission_run()"/>
|
||||
<deroute block="Standby"/>
|
||||
</block>
|
||||
<block group="std_pattern" name="Oval 1-2" strip_button="Oval" strip_icon="oval.png">
|
||||
<oval p1="O1" p2="O2" radius="nav_radius"/>
|
||||
</block>
|
||||
<block group="std_pattern" name="Survey 1-2" strip_button="Survey" strip_icon="survey.png">
|
||||
<survey_rectangle grid="150" wp1="O1" wp2="O2"/>
|
||||
</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() > 0.5" wp="_BASELEG"/>
|
||||
<circle radius="nav_radius" until="NavQdrCloseTo(DegOfRad(baseleg_out_qdr)-(nav_radius/fabs(nav_radius))*10) @AND (fabs(GetPosAlt() - WaypointAlt(WP__BASELEG)) @LT 10)" wp="_BASELEG"/>
|
||||
</block>
|
||||
<block name="final">
|
||||
<exception cond="GetAltRef() + 10 > GetPosAlt()" deroute="flare"/>
|
||||
<go from="AF" hmode="route" vmode="glide" wp="TD"/>
|
||||
</block>
|
||||
<block name="flare">
|
||||
<go approaching_time="0" from="AF" hmode="route" throttle="0.0" vmode="throttle" wp="TD"/>
|
||||
<attitude roll="0.0" throttle="0.0" until="FALSE" vmode="throttle"/>
|
||||
</block>
|
||||
</blocks>
|
||||
</flight_plan>
|
||||
@@ -0,0 +1,37 @@
|
||||
<!DOCTYPE module SYSTEM "module.dtd">
|
||||
|
||||
<module name="cloud_sensor" dir="meteo">
|
||||
<doc>
|
||||
<description>
|
||||
Get data from Cloud Sensor
|
||||
- compute Liquid Water Content (LWC) value from PAYLOAD_FLOAT data
|
||||
- get already computed LWC from PAYLOAD_COMMAND data
|
||||
</description>
|
||||
</doc>
|
||||
<settings>
|
||||
<dl_settings NAME="CloudSensor">
|
||||
<dl_settings NAME="Cloud">
|
||||
<dl_setting max="1000." min="0." step="0.1" var="cloud_sensor_calib_alpha" shortname="alpha" module="meteo/cloud_sensor"/>
|
||||
<dl_setting max="60000." min="0." step="10." var="cloud_sensor_calib_beta" shortname="beta" module="meteo/cloud_sensor"/>
|
||||
<dl_setting max="1000." min="1." step="0.1" var="cloud_sensor_channel_scale" shortname="scale" module="meteo/cloud_sensor"/>
|
||||
<dl_setting MAX="2" MIN="0" STEP="1" VAR="cloud_sensor_compute_coef" shortname="coef type" module="meteo/cloud_sensor" values="NONE|SINGLE|ANGSTROM"/>
|
||||
<dl_setting max="1000." min="0." step="1." var="cloud_sensor_threshold" shortname="threshold" module="meteo/cloud_sensor"/>
|
||||
<dl_setting max="1000." min="0." step="1." var="cloud_sensor_hysteresis" shortname="hysteresis" module="meteo/cloud_sensor"/>
|
||||
<dl_setting max="20000." min="0." step="0.1" var="cloud_sensor_background" shortname="background" module="meteo/cloud_sensor"/>
|
||||
<dl_setting MAX="1" MIN="1" STEP="1" VAR="cloud_sensor_compute_background" shortname="compute background" module="meteo/cloud_sensor"/>
|
||||
<dl_setting MAX="20." MIN="0." STEP="0.1" VAR="cloud_sensor_tau" shortname="tau" module="meteo/cloud_sensor" handler="update_tau"/>
|
||||
</dl_settings>
|
||||
</dl_settings>
|
||||
</settings>
|
||||
<header>
|
||||
<file name="cloud_sensor.h"/>
|
||||
</header>
|
||||
<init fun="cloud_sensor_init()"/>
|
||||
<periodic fun="cloud_sensor_report()" freq="2." autorun="TRUE"/>
|
||||
<datalink message="PAYLOAD_COMMAND" fun="LWC_callback(buf)"/>
|
||||
<datalink message="PAYLOAD_FLOAT" fun="cloud_sensor_callback(buf)"/>
|
||||
<makefile>
|
||||
<file name="cloud_sensor.c"/>
|
||||
</makefile>
|
||||
</module>
|
||||
|
||||
@@ -0,0 +1,26 @@
|
||||
<!DOCTYPE module SYSTEM "module.dtd">
|
||||
<module name="cloud_sim" dir="meteo">
|
||||
<doc>
|
||||
<description>Basic cloud simulation for testing adaptive navigation patterns</description>
|
||||
</doc>
|
||||
<settings>
|
||||
<dl_settings>
|
||||
<dl_settings name="Cloud Sim">
|
||||
<dl_setting max="1" min="0" step="1" var="cloud_sim.mode" shortname="mode" values="WP|POLYGON"/>
|
||||
<dl_setting max="10." min="-10." step="0.1" var="cloud_sim.speed.x" shortname="v east"/>
|
||||
<dl_setting max="10." min="-10." step="0.1" var="cloud_sim.speed.y" shortname="v north"/>
|
||||
<dl_setting max="1" min="1" step="1" var="cloud_sim.reset" shortname="reset" values="RESET" module="meteo/cloud_sim" handler="reset"/>
|
||||
<dl_setting max="500" min="50" step="10." var="cloud_sim.radius" shortname="radius"/>
|
||||
</dl_settings>
|
||||
</dl_settings>
|
||||
</settings>
|
||||
<header>
|
||||
<file name="cloud_sim.h"/>
|
||||
</header>
|
||||
<init fun="cloud_sim_init()"/>
|
||||
<periodic fun="cloud_sim_detect()" freq="10.0" autorun="TRUE"/>
|
||||
<periodic fun="cloud_sim_move()" freq="1.0" autorun="FALSE"/>
|
||||
<makefile>
|
||||
<file name="cloud_sim.c"/>
|
||||
</makefile>
|
||||
</module>
|
||||
@@ -25,7 +25,9 @@
|
||||
<configure name="MS_EEPROM_SLAVE_IDX" value="X" description="Index of the SPI slave for calibration EEPROM"/>
|
||||
<section name="METEO_STICK">
|
||||
<define name="LOG_MS" value="TRUE|FALSE" description="Log data on SD card (ascii format, raw PTU data + GPS data)"/>
|
||||
<define name="SEND_MS" value="TRUE|FALSE" description="Send data over telemetry (PAYLOAD_FLOAT message, scaled PTU data)"/>
|
||||
<define name="LOG_MS_FLIGHTRECORDER" value="TRUE|FALSE" description="Log data on SD card (METEO_STICK message in binary format)"/>
|
||||
<define name="MS_LOG_FILE" value="flightrecorder_sdlog" description="Log for binary format (only available with LOG_MS_FLIGHTRECORDER to TRUE)"/>
|
||||
<define name="SEND_MS_SYNC" value="TRUE|FALSE" description="Send data over telemetry sync to reading (METEO_STICK message, scaled PTU data)"/>
|
||||
<define name="USE_MS_EEPROM" value="TRUE|FALSE" description="Enable/disable usage of calibration data from EEPROM (default TRUE, set to FALSE if MS_EEPROM_SLAVE_IDX is not configured"/>
|
||||
<define name="MS_PRESSURE_OFFSET" value="0." description="Offset of the absolute pressure sensor (only used if EEPROM is not used)"/>
|
||||
<define name="MS_PRESSURE_SCALE" value="1." description="Scale factor of the absolute pressure sensor (only used if EEPROM is not used)"/>
|
||||
@@ -52,6 +54,7 @@
|
||||
</header>
|
||||
<init fun="meteo_stick_init()"/>
|
||||
<periodic fun="meteo_stick_periodic()" freq="10" autorun="TRUE"/>
|
||||
<periodic fun="meteo_stick_report()" freq="1" autorun="TRUE"/>
|
||||
<event fun="meteo_stick_event()"/>
|
||||
<makefile target="ap">
|
||||
<configure name="MS_SPI_DEV" case="upper|lower"/>
|
||||
|
||||
@@ -3,33 +3,14 @@
|
||||
<module name="mission_fw" dir="mission" task="control">
|
||||
<doc>
|
||||
<description>
|
||||
Interface for mission control of fixed wing aircraft.
|
||||
This module parse datalink commands for basic navigation routines
|
||||
and store them in a queue.
|
||||
Specific interface for mission control of fixed wing aircraft.
|
||||
</description>
|
||||
</doc>
|
||||
<header>
|
||||
<file name="mission_common.h"/>
|
||||
</header>
|
||||
<init fun="mission_init()"/>
|
||||
<periodic fun="mission_status_report()" freq="2" autorun="TRUE"/>
|
||||
|
||||
<datalink message="MISSION_GOTO_WP" fun="mission_parse_GOTO_WP(buf)"/>
|
||||
<datalink message="MISSION_GOTO_WP_LLA" fun="mission_parse_GOTO_WP_LLA(buf)"/>
|
||||
<datalink message="MISSION_CIRCLE" fun="mission_parse_CIRCLE(buf)"/>
|
||||
<datalink message="MISSION_CIRCLE_LLA" fun="mission_parse_CIRCLE_LLA(buf)"/>
|
||||
<datalink message="MISSION_SEGMENT" fun="mission_parse_SEGMENT(buf)"/>
|
||||
<datalink message="MISSION_SEGMENT_LLA" fun="mission_parse_SEGMENT_LLA(buf)"/>
|
||||
<datalink message="MISSION_PATH" fun="mission_parse_PATH(buf)"/>
|
||||
<datalink message="MISSION_PATH_LLA" fun="mission_parse_PATH_LLA(buf)"/>
|
||||
<datalink message="MISSION_CUSTOM" fun="mission_parse_CUSTOM(buf)"/>
|
||||
<datalink message="GOTO_MISSION" fun="mission_parse_GOTO_MISSION(buf)"/>
|
||||
<datalink message="NEXT_MISSION" fun="mission_parse_NEXT_MISSION(buf)"/>
|
||||
<datalink message="END_MISSION" fun="mission_parse_END_MISSION(buf)"/>
|
||||
|
||||
<dep>
|
||||
<depends>mission_common</depends>
|
||||
<provides>mission</provides>
|
||||
</dep>
|
||||
<makefile>
|
||||
<define name="USE_MISSION"/>
|
||||
<file name="mission_common.c"/>
|
||||
<file name="mission_fw_nav.c"/>
|
||||
<test firmware="fixedwing">
|
||||
<define name="CTRL_TYPE_H" value="firmwares/fixedwing/guidance/guidance_v.h" type="string"/>
|
||||
|
||||
@@ -3,33 +3,14 @@
|
||||
<module name="mission_rotorcraft" dir="mission" task="control">
|
||||
<doc>
|
||||
<description>
|
||||
Interface for mission control of rotorcraft.
|
||||
This module parse datalink commands for basic navigation routines
|
||||
and store them in a queue.
|
||||
Specific interface for mission control of rotorcraft.
|
||||
</description>
|
||||
</doc>
|
||||
<header>
|
||||
<file name="mission_common.h"/>
|
||||
</header>
|
||||
<init fun="mission_init()"/>
|
||||
<periodic fun="mission_status_report()" freq="2" autorun="TRUE"/>
|
||||
|
||||
<datalink message="MISSION_GOTO_WP" fun="mission_parse_GOTO_WP(buf)"/>
|
||||
<datalink message="MISSION_GOTO_WP_LLA" fun="mission_parse_GOTO_WP_LLA(buf)"/>
|
||||
<datalink message="MISSION_CIRCLE" fun="mission_parse_CIRCLE(buf)"/>
|
||||
<datalink message="MISSION_CIRCLE_LLA" fun="mission_parse_CIRCLE_LLA(buf)"/>
|
||||
<datalink message="MISSION_SEGMENT" fun="mission_parse_SEGMENT(buf)"/>
|
||||
<datalink message="MISSION_SEGMENT_LLA" fun="mission_parse_SEGMENT_LLA(buf)"/>
|
||||
<datalink message="MISSION_PATH" fun="mission_parse_PATH(buf)"/>
|
||||
<datalink message="MISSION_PATH_LLA" fun="mission_parse_PATH_LLA(buf)"/>
|
||||
<datalink message="MISSION_CUSTOM" fun="mission_parse_CUSTOM(buf)"/>
|
||||
<datalink message="GOTO_MISSION" fun="mission_parse_GOTO_MISSION(buf)"/>
|
||||
<datalink message="NEXT_MISSION" fun="mission_parse_NEXT_MISSION(buf)"/>
|
||||
<datalink message="END_MISSION" fun="mission_parse_END_MISSION(buf)"/>
|
||||
|
||||
<dep>
|
||||
<depends>mission_common</depends>
|
||||
<provides>mission</provides>
|
||||
</dep>
|
||||
<makefile>
|
||||
<define name="USE_MISSION"/>
|
||||
<file name="mission_common.c"/>
|
||||
<file name="mission_rotorcraft_nav.c"/>
|
||||
<test firmware="rotorcraft">
|
||||
<define name="DOWNLINK_TRANSPORT" value="pprz_tp"/>
|
||||
|
||||
@@ -0,0 +1,21 @@
|
||||
<!DOCTYPE module SYSTEM "module.dtd">
|
||||
|
||||
<module name="nav_lace" dir="nav">
|
||||
<doc>
|
||||
<description>
|
||||
Adaptive border pattern for cloud exploration
|
||||
Can be used in mission mode with custom pattern and ID "LACE"
|
||||
|
||||
See:
|
||||
Titouan Verdu, Gautier Hattenberger, Simon Lacroix. Flight patterns for clouds exploration with a fleet of UAVs. 2019 International Conference on Unmanned Aircraft Systems (ICUAS 2019), Jul 2019, Atlanta, United States.
|
||||
https://hal-enac.archives-ouvertes.fr/hal-02137839
|
||||
</description>
|
||||
</doc>
|
||||
<header>
|
||||
<file name="nav_lace.h"/>
|
||||
</header>
|
||||
<init fun="nav_lace_init()"/>
|
||||
<makefile target="ap|sim|nps|hitl">
|
||||
<file name="nav_lace.c"/>
|
||||
</makefile>
|
||||
</module>
|
||||
@@ -17,6 +17,7 @@
|
||||
<header>
|
||||
<file name="nav_line.h"/>
|
||||
</header>
|
||||
<init fun="nav_line_init()"/>
|
||||
<makefile target="ap|sim|nps|hitl">
|
||||
<file name="nav_line.c"/>
|
||||
<test firmware="fixedwing">
|
||||
|
||||
@@ -0,0 +1,21 @@
|
||||
<!DOCTYPE module SYSTEM "module.dtd">
|
||||
|
||||
<module name="nav_rosette" dir="nav">
|
||||
<doc>
|
||||
<description>
|
||||
Adaptive flower pattern for cloud exploration
|
||||
Can be used in mission mode with custom pattern and ID "RSTT"
|
||||
|
||||
See:
|
||||
Titouan Verdu, Gautier Hattenberger, Simon Lacroix. Flight patterns for clouds exploration with a fleet of UAVs. 2019 International Conference on Unmanned Aircraft Systems (ICUAS 2019), Jul 2019, Atlanta, United States.
|
||||
https://hal-enac.archives-ouvertes.fr/hal-02137839
|
||||
</description>
|
||||
</doc>
|
||||
<header>
|
||||
<file name="nav_rosette.h"/>
|
||||
</header>
|
||||
<init fun="nav_rosette_init()"/>
|
||||
<makefile target="ap|sim|nps|hitl">
|
||||
<file name="nav_rosette.c"/>
|
||||
</makefile>
|
||||
</module>
|
||||
@@ -0,0 +1,32 @@
|
||||
<!DOCTYPE module SYSTEM "module.dtd">
|
||||
|
||||
<module name="nav_spiral_3D" dir="nav">
|
||||
<doc>
|
||||
<description>
|
||||
Fixedwing navigation in a 3D spiral.
|
||||
creating a helix:
|
||||
- start center (X, Y)
|
||||
- start and stop altitude
|
||||
- start and stop radius
|
||||
- 3D speed of the circle
|
||||
- in case alt diff is too small, vz is used as expension speed at constant altitude
|
||||
- in case of radius diff too small, doing a constant radius circle
|
||||
- if alt diff is too small, basic circle with horizontal speed (vz as no effect)
|
||||
setup will fail if:
|
||||
- alt diff and vz are not coherent
|
||||
- vz is too small and not doing circle
|
||||
</description>
|
||||
<section name="NAV_SPIRAL_3D" prefix="NAV_SPIRAL_3D_">
|
||||
<define name="DIST_DIFF" value="10." description="horizontal distance to start and stop pattern"/>
|
||||
<define name="ALT_DIFF" value="10." description="vertical distance to start and stop pattern"/>
|
||||
<define name="MIN_CIRCLE_RADIUS" value="60." description="minium circle radius in meters"/>
|
||||
</section>
|
||||
</doc>
|
||||
<header>
|
||||
<file name="nav_spiral_3D.h"/>
|
||||
</header>
|
||||
<init fun="nav_spiral_3D_init()"/>
|
||||
<makefile target="ap|sim|nps|hitl">
|
||||
<file name="nav_spiral_3D.c"/>
|
||||
</makefile>
|
||||
</module>
|
||||
@@ -0,0 +1,22 @@
|
||||
<!DOCTYPE module SYSTEM "module.dtd">
|
||||
|
||||
<module name="nav_trinity" dir="nav">
|
||||
<doc>
|
||||
<description>
|
||||
Adaptive "trinity" pattern for cloud exploration
|
||||
Can be used in mission mode with custom pattern and ID "TRNTY"
|
||||
|
||||
See:
|
||||
Titouan Verdu, Gautier Hattenberger, Simon Lacroix. Flight patterns for clouds exploration with a fleet of UAVs. 2019 International Conference on Unmanned Aircraft Systems (ICUAS 2019), Jul 2019, Atlanta, United States.
|
||||
https://hal-enac.archives-ouvertes.fr/hal-02137839
|
||||
</description>
|
||||
</doc>
|
||||
<header>
|
||||
<file name="nav_trinity.h"/>
|
||||
</header>
|
||||
<init fun="nav_trinity_init()"/>
|
||||
<makefile>
|
||||
<file name="nav_trinity.c"/>
|
||||
</makefile>
|
||||
</module>
|
||||
|
||||
@@ -87,6 +87,20 @@ static void send_actuators(struct transport_tx *trans, struct link_device *dev)
|
||||
}
|
||||
#endif
|
||||
|
||||
static void send_minimal_com(struct transport_tx *trans, struct link_device *dev)
|
||||
{
|
||||
float lat = DegOfRad(stateGetPositionLla_f()->lat);
|
||||
float lon = DegOfRad(stateGetPositionLla_f()->lon);
|
||||
float hmsl = stateGetPositionUtm_f()->alt;
|
||||
float gspeed = stateGetHorizontalSpeedNorm_f();
|
||||
float course = stateGetHorizontalSpeedDir_f();
|
||||
float climb = stateGetSpeedEnu_f()->z;
|
||||
uint8_t throttle = (uint8_t)(100 * autopilot.throttle / MAX_PPRZ);
|
||||
pprz_msg_send_MINIMAL_COM(trans, dev, AC_ID,
|
||||
&lat, &lon, &hmsl, &gspeed, &course, &climb,
|
||||
&electrical.vsupply, &throttle, &autopilot.mode,
|
||||
&nav_block, &gps.fix, &autopilot.flight_time);
|
||||
}
|
||||
|
||||
void autopilot_init(void)
|
||||
{
|
||||
@@ -134,6 +148,7 @@ void autopilot_init(void)
|
||||
#ifdef RADIO_CONTROL
|
||||
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_RC, send_rc);
|
||||
#endif
|
||||
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_MINIMAL_COM, send_minimal_com);
|
||||
}
|
||||
|
||||
/** AP periodic call
|
||||
|
||||
@@ -86,6 +86,17 @@ static inline float get_first_order_low_pass(struct FirstOrderLowPass *filter)
|
||||
return filter->last_out;
|
||||
}
|
||||
|
||||
/** Update time constant (tau parameter) for first order low pass filter
|
||||
*
|
||||
* @param filter first order low pass filter structure
|
||||
* @param tau time constant of the first order low pass filter
|
||||
* @param sample_time sampling period of the signal
|
||||
*/
|
||||
static inline void update_first_order_low_pass_tau(struct FirstOrderLowPass *filter, float tau, float sample_time)
|
||||
{
|
||||
filter->time_const = 2.0f * tau / sample_time;
|
||||
}
|
||||
|
||||
/** Second order low pass filter structure.
|
||||
*
|
||||
* using biquad filter with bilinear z transform
|
||||
|
||||
@@ -149,6 +149,7 @@ void nav_circle_XY(float x, float y, float radius)
|
||||
fly_to_xy(x + cosf(alpha_carrot)*radius_carrot,
|
||||
y + sinf(alpha_carrot)*radius_carrot);
|
||||
nav_in_circle = true;
|
||||
nav_in_segment = false;
|
||||
nav_circle_x = x;
|
||||
nav_circle_y = y;
|
||||
nav_circle_radius = radius;
|
||||
@@ -347,7 +348,6 @@ bool nav_approaching_xy(float x, float y, float from_x, float from_y, float appr
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* \brief Computes \a desired_x, \a desired_y and \a desired_course.
|
||||
*/
|
||||
@@ -391,6 +391,7 @@ void nav_route_xy(float last_wp_x, float last_wp_y, float wp_x, float wp_y)
|
||||
float carrot = CARROT * NOMINAL_AIRSPEED;
|
||||
|
||||
nav_carrot_leg_progress = nav_leg_progress + Max(carrot / nav_leg_length, 0.);
|
||||
nav_in_circle = false;
|
||||
nav_in_segment = true;
|
||||
nav_segment_x_1 = last_wp_x;
|
||||
nav_segment_y_1 = last_wp_y;
|
||||
|
||||
@@ -527,4 +527,15 @@
|
||||
#define JOYSTICK_ID 1
|
||||
#endif
|
||||
|
||||
/*
|
||||
* PAYLOAD_DATA
|
||||
*/
|
||||
#ifndef LWC_SIM_ID
|
||||
#define LWC_SIM_ID 1
|
||||
#endif
|
||||
|
||||
#ifndef CLOUD_SENSOR_ID
|
||||
#define CLOUD_SENSOR_ID 1
|
||||
#endif
|
||||
|
||||
#endif /* ABI_SENDER_IDS_H */
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,71 @@
|
||||
/*
|
||||
* Copyright (C) 2019 Gautier Hattenberger <gautier.hattenberger@enac.fr>
|
||||
* Titouan Verdu <titouan.verdu@enac.fr>
|
||||
*
|
||||
* This file is part of paparazzi.
|
||||
*
|
||||
* paparazzi is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 2, or (at your option)
|
||||
* any later version.
|
||||
*
|
||||
* paparazzi is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with paparazzi; see the file COPYING. If not, see
|
||||
* <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
/**
|
||||
* @file "modules/meteo/cloud_sensor.h"
|
||||
*
|
||||
* Get data from Cloud Sensor
|
||||
* - compute coef value from PAYLOAD_FLOAT data
|
||||
* - Liquid Water Content (LWC)
|
||||
* - Angstrom coef
|
||||
* - single sensor
|
||||
* - get already computed LWC from PAYLOAD_COMMAND data
|
||||
*/
|
||||
|
||||
#ifndef CLOUD_SENSOR_H
|
||||
#define CLOUD_SENSOR_H
|
||||
|
||||
#include "std.h"
|
||||
|
||||
#define CLOUD_SENSOR_COEF_NONE 0
|
||||
#define CLOUD_SENSOR_COEF_SINGLE 1
|
||||
#define CLOUD_SENSOR_COEF_ANGSTROM 2
|
||||
|
||||
/**
|
||||
* variables for settings
|
||||
*/
|
||||
extern uint8_t cloud_sensor_compute_coef;
|
||||
extern uint8_t cloud_sensor_compute_background;
|
||||
extern float cloud_sensor_threshold;
|
||||
extern float cloud_sensor_hysteresis;
|
||||
extern float cloud_sensor_background;
|
||||
extern float cloud_sensor_calib_alpha;
|
||||
extern float cloud_sensor_calib_beta;
|
||||
extern float cloud_sensor_channel_scale;
|
||||
extern float cloud_sensor_tau;
|
||||
extern void cloud_sensor_update_tau(float tau); // setting handler
|
||||
|
||||
/** Init function
|
||||
*/
|
||||
extern void cloud_sensor_init(void);
|
||||
|
||||
/** New message/data callback
|
||||
*/
|
||||
extern void cloud_sensor_callback(uint8_t *buf);
|
||||
extern void LWC_callback(uint8_t *buf);
|
||||
|
||||
/** Report function
|
||||
*/
|
||||
extern void cloud_sensor_report(void);
|
||||
|
||||
|
||||
#endif
|
||||
|
||||
@@ -0,0 +1,172 @@
|
||||
/*
|
||||
* Copyright (C) 2021 Gautier Hattenberger <gautier.hattenberger@enac.fr>
|
||||
*
|
||||
* This file is part of paparazzi
|
||||
*
|
||||
* paparazzi is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 2, or (at your option)
|
||||
* any later version.
|
||||
*
|
||||
* paparazzi is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with paparazzi; see the file COPYING. If not, see
|
||||
* <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
/** @file "modules/meteo/cloud_sim.c"
|
||||
* @author Gautier Hattenberger <gautier.hattenberger@enac.fr>
|
||||
* Basic cloud simulation for testing adaptive navigation patterns
|
||||
*/
|
||||
|
||||
#include "modules/meteo/cloud_sim.h"
|
||||
#include "modules/nav/common_nav.h"
|
||||
#include "generated/flight_plan.h"
|
||||
#include "generated/airframe.h"
|
||||
#include "modules/core/abi.h"
|
||||
#include "state.h"
|
||||
|
||||
// default radius in WP mode
|
||||
#ifndef CLOUD_SIM_RADIUS
|
||||
#define CLOUD_SIM_RADIUS 150.f
|
||||
#endif
|
||||
|
||||
// default mode
|
||||
#ifndef CLOUD_SIM_MODE
|
||||
#define CLOUD_SIM_MODE CLOUD_SIM_WP
|
||||
#endif
|
||||
|
||||
// use CLOUD waypoint if any by default, 0 (none) otherwise
|
||||
#if (defined WP_CLOUD) && !(defined CLOUD_SIM_WP_ID)
|
||||
#define CLOUD_SIM_WP_ID WP_CLOUD
|
||||
#elif !(defined CLOUD_SIM_WP_ID)
|
||||
#define CLOUD_SIM_WP_ID 0
|
||||
#endif
|
||||
static uint8_t cloud_sim_circle_id = CLOUD_SIM_WP_ID;
|
||||
|
||||
#if (defined CLOUD_SIM_WP_POLYGON)
|
||||
static uint8_t cloud_sim_polygon[] = CLOUD_SIM_WP_POLYGON;
|
||||
#ifndef CLOUD_SIM_WPS_NB
|
||||
#pragma error "CLOUD_SIM: please define CLOUD_SIM_WPS_NB for custom polygon"
|
||||
#endif
|
||||
#elif (defined SECTOR_CLOUD)
|
||||
static uint8_t cloud_sim_polygon[] = SECTOR_CLOUD;
|
||||
#define CLOUD_SIM_WPS_NB SECTOR_CLOUD_NB
|
||||
#else
|
||||
#define CLOUD_SIM_WPS_NB 1
|
||||
static uint8_t cloud_sim_polygon[CLOUD_SIM_WPS_NB] = {0};
|
||||
#endif
|
||||
|
||||
#ifndef CLOUD_SIM_SPEED_X
|
||||
#define CLOUD_SIM_SPEED_X 0.f
|
||||
#endif
|
||||
|
||||
#ifndef CLOUD_SIM_SPEED_Y
|
||||
#define CLOUD_SIM_SPEED_Y 0.f
|
||||
#endif
|
||||
|
||||
struct CloudSim cloud_sim;
|
||||
|
||||
/*********************
|
||||
* Utility functions *
|
||||
*********************/
|
||||
|
||||
static float distance_to_wp(struct EnuCoor_f * pos, uint8_t id)
|
||||
{
|
||||
if (id > 0 && id < nb_waypoint) {
|
||||
struct FloatVect2 diff = { pos->x - waypoints[id].x, pos->y - waypoints[id].y };
|
||||
return float_vect2_norm(&diff);
|
||||
} else {
|
||||
return -1.f; // invalid WP id
|
||||
}
|
||||
}
|
||||
|
||||
/**********************
|
||||
* External functions *
|
||||
**********************/
|
||||
|
||||
void cloud_sim_init(void)
|
||||
{
|
||||
cloud_sim.reset = false;
|
||||
cloud_sim.mode = CLOUD_SIM_MODE;
|
||||
cloud_sim.speed.x = CLOUD_SIM_SPEED_X;
|
||||
cloud_sim.speed.y = CLOUD_SIM_SPEED_Y;
|
||||
cloud_sim.radius = CLOUD_SIM_RADIUS;
|
||||
}
|
||||
|
||||
/** periodic call for border detection */
|
||||
void cloud_sim_detect(void)
|
||||
{
|
||||
uint32_t stamp = get_sys_time_usec();
|
||||
struct EnuCoor_f * pos = stateGetPositionEnu_f();
|
||||
uint8_t inside = 0; // 1: inside, 0: outside
|
||||
|
||||
switch (cloud_sim.mode) {
|
||||
case CLOUD_SIM_WP:
|
||||
// Test the distance to the reference waypoint
|
||||
if (cloud_sim_circle_id > 0 && distance_to_wp(pos, cloud_sim_circle_id) > cloud_sim.radius) {
|
||||
inside = 0;
|
||||
} else {
|
||||
inside = 1;
|
||||
}
|
||||
AbiSendMsgPAYLOAD_DATA(CLOUD_SENSOR_ID, stamp, 1 /* CLOUD_BORDER */, 1, &inside);
|
||||
break;
|
||||
#ifdef SECTOR_CLOUD
|
||||
case CLOUD_SIM_POLYGON:
|
||||
inside = (uint8_t) InsideCloud(pos->x, pos->y);
|
||||
AbiSendMsgPAYLOAD_DATA(CLOUD_SENSOR_ID, stamp, 1 /* CLOUD_BORDER */, 1, &inside);
|
||||
break;
|
||||
#endif
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
/** periodic call for moving waypoints */
|
||||
void cloud_sim_move(void)
|
||||
{
|
||||
if (cloud_sim.mode == CLOUD_SIM_WP) {
|
||||
if (cloud_sim_circle_id > 0 && cloud_sim_circle_id < NB_WAYPOINT) {
|
||||
struct point wp = waypoints[cloud_sim_circle_id];
|
||||
wp.x += cloud_sim.speed.x; // assuming dt = 1.
|
||||
wp.y += cloud_sim.speed.y; // assuming dt = 1.
|
||||
nav_move_waypoint_point(cloud_sim_circle_id, &wp);
|
||||
nav_send_waypoint(cloud_sim_circle_id);
|
||||
}
|
||||
} else if (cloud_sim.mode == CLOUD_SIM_POLYGON) {
|
||||
for (int i = 0; i < CLOUD_SIM_WPS_NB; i++) {
|
||||
if (cloud_sim_polygon[i] > 0 && cloud_sim_polygon[i] < NB_WAYPOINT) {
|
||||
struct point wp = waypoints[cloud_sim_polygon[i]];
|
||||
wp.x += cloud_sim.speed.x; // assuming dt = 1.
|
||||
wp.y += cloud_sim.speed.y; // assuming dt = 1.
|
||||
nav_move_waypoint_point(cloud_sim_polygon[i], &wp);
|
||||
nav_send_waypoint(cloud_sim_polygon[i]);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/** reset handler */
|
||||
void cloud_sim_reset(bool reset)
|
||||
{
|
||||
if (reset) {
|
||||
struct point wps[NB_WAYPOINT] = WAYPOINTS_UTM;
|
||||
// reset WP
|
||||
if (cloud_sim_circle_id > 0 && cloud_sim_circle_id < NB_WAYPOINT) {
|
||||
nav_move_waypoint_point(cloud_sim_circle_id, &wps[cloud_sim_circle_id]);
|
||||
nav_send_waypoint(cloud_sim_circle_id);
|
||||
}
|
||||
for (int i = 0; i < CLOUD_SIM_WPS_NB; i++) {
|
||||
if (cloud_sim_polygon[i] > 0 && cloud_sim_polygon[i] < NB_WAYPOINT) {
|
||||
nav_move_waypoint_point(cloud_sim_polygon[i], &wps[cloud_sim_polygon[i]]);
|
||||
nav_send_waypoint(cloud_sim_polygon[i]);
|
||||
}
|
||||
}
|
||||
}
|
||||
cloud_sim.reset = false;
|
||||
}
|
||||
|
||||
@@ -0,0 +1,49 @@
|
||||
/*
|
||||
* Copyright (C) 2021 Gautier Hattenberger <gautier.hattenberger@enac.fr>
|
||||
*
|
||||
* This file is part of paparazzi
|
||||
*
|
||||
* paparazzi is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 2, or (at your option)
|
||||
* any later version.
|
||||
*
|
||||
* paparazzi is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with paparazzi; see the file COPYING. If not, see
|
||||
* <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
/** @file "modules/meteo/cloud_sim.h"
|
||||
* @author Gautier Hattenberger <gautier.hattenberger@enac.fr>
|
||||
* Basic cloud simulation for testing adaptive navigation patterns
|
||||
*/
|
||||
|
||||
#ifndef CLOUD_SIM_H
|
||||
#define CLOUD_SIM_H
|
||||
|
||||
#include "std.h"
|
||||
#include "math/pprz_algebra_float.h"
|
||||
|
||||
#define CLOUD_SIM_WP 0
|
||||
#define CLOUD_SIM_POLYGON 1
|
||||
|
||||
struct CloudSim {
|
||||
bool reset;
|
||||
uint8_t mode;
|
||||
struct FloatVect2 speed;
|
||||
float radius; ///< radius in WP mode
|
||||
};
|
||||
|
||||
extern struct CloudSim cloud_sim;
|
||||
|
||||
extern void cloud_sim_init(void);
|
||||
extern void cloud_sim_detect(void);
|
||||
extern void cloud_sim_move(void);
|
||||
extern void cloud_sim_reset(bool reset);
|
||||
|
||||
#endif // CLOUD_SIM_H
|
||||
@@ -117,12 +117,12 @@ void mf_daq_send_report(void)
|
||||
|
||||
void parse_mf_daq_msg(uint8_t *buf)
|
||||
{
|
||||
mf_daq.nb = buf[2];
|
||||
mf_daq.nb = pprzlink_get_PAYLOAD_FLOAT_values_length(buf);
|
||||
if (mf_daq.nb > 0) {
|
||||
if (mf_daq.nb > MF_DAQ_SIZE) { mf_daq.nb = MF_DAQ_SIZE; }
|
||||
// Store data struct directly from dl_buffer
|
||||
float *bufloc = (float*)(buf+3);
|
||||
memcpy(mf_daq.values, bufloc, mf_daq.nb * sizeof(float));
|
||||
float *b = pprzlink_get_DL_PAYLOAD_FLOAT_values(buf);
|
||||
memcpy(mf_daq.values, b, mf_daq.nb * sizeof(float));
|
||||
// Log on SD card
|
||||
if (log_started) {
|
||||
DOWNLINK_SEND_PAYLOAD_FLOAT(pprzlog_tp, chibios_sdlog, mf_daq.nb, mf_daq.values);
|
||||
|
||||
@@ -25,6 +25,8 @@
|
||||
#include "peripherals/ads1220.h"
|
||||
#include "mcu_periph/pwm_input.h"
|
||||
#include "generated/airframe.h"
|
||||
#include "pprzlink/messages.h"
|
||||
#include "modules/datalink/downlink.h"
|
||||
|
||||
/** Default scale and offset
|
||||
* Only used if calibration from EEPROM is not used/available
|
||||
@@ -163,8 +165,31 @@ static inline float get_humidity(uint32_t raw)
|
||||
#endif
|
||||
}
|
||||
|
||||
/** Funnction to send over different transport and device
|
||||
*/
|
||||
static void meteo_stick_send_data(struct transport_tx *trans, struct link_device *device)
|
||||
{
|
||||
pprz_msg_send_METEO_STICK(trans, device, AC_ID,
|
||||
&stateGetPositionLla_i()->lat,
|
||||
&stateGetPositionLla_i()->lon,
|
||||
&gps.hmsl,
|
||||
&gps.tow,
|
||||
&meteo_stick.current_pressure,
|
||||
&meteo_stick.current_temperature,
|
||||
&meteo_stick.current_humidity,
|
||||
&meteo_stick.current_airspeed);
|
||||
}
|
||||
|
||||
/** Includes to log on SD card
|
||||
/** Send over telemetry synchrone to reading
|
||||
*
|
||||
* FALSE by default (the asynchronous report function is used by default)
|
||||
*/
|
||||
#ifndef SEND_MS_SYNC
|
||||
#define SEND_MS_SYNC FALSE
|
||||
#endif
|
||||
|
||||
|
||||
/** Includes to log on SD card as ASCII csv
|
||||
*
|
||||
* TRUE by default
|
||||
*/
|
||||
@@ -174,38 +199,101 @@ static inline float get_humidity(uint32_t raw)
|
||||
|
||||
#if LOG_MS
|
||||
#include "modules/loggers/sdlog_chibios.h"
|
||||
bool log_ptu_started;
|
||||
#endif
|
||||
|
||||
/* Includes and function to send over telemetry
|
||||
*
|
||||
* TRUE by default
|
||||
*/
|
||||
#ifndef SEND_MS
|
||||
#define SEND_MS TRUE
|
||||
#endif
|
||||
|
||||
#if SEND_MS
|
||||
#include "mcu_periph/uart.h"
|
||||
#include "pprzlink/messages.h"
|
||||
#include "modules/datalink/downlink.h"
|
||||
#include "modules/gps/gps.h"
|
||||
static bool log_ptu_started;
|
||||
|
||||
#define MS_DATA_SIZE 4
|
||||
|
||||
static inline void meteo_stick_send_data(void)
|
||||
static inline void meteo_stick_log_data_ascii(void)
|
||||
{
|
||||
float ptu_data[MS_DATA_SIZE];
|
||||
ptu_data[0] = meteo_stick.current_pressure;
|
||||
ptu_data[1] = meteo_stick.current_temperature;
|
||||
ptu_data[2] = meteo_stick.current_humidity;
|
||||
ptu_data[3] = meteo_stick.current_airspeed;
|
||||
DOWNLINK_SEND_PAYLOAD_FLOAT(DefaultChannel, DefaultDevice, MS_DATA_SIZE, ptu_data);
|
||||
if (pprzLogFile != -1) {
|
||||
if (!log_ptu_started) {
|
||||
#if USE_MS_EEPROM
|
||||
if (meteo_stick.eeprom.data_available) {
|
||||
// Print calibration data in the log header
|
||||
sdLogWriteLog(pprzLogFile, "# Calibration data (UUID: %s)\n#\n", meteo_stick.calib.uuid);
|
||||
int i, j, k;
|
||||
for (i = 0; i < MTOSTK_NUM_SENSORS; i++) {
|
||||
sdLogWriteLog(pprzLogFile, "# Sensor: %d, time: %ld, num_temp: %d, num_coeff: %d\n", i,
|
||||
meteo_stick.calib.params[i].timestamp,
|
||||
meteo_stick.calib.params[i].num_temp,
|
||||
meteo_stick.calib.params[i].num_coeff);
|
||||
if (meteo_stick.calib.params[i].timestamp == 0) {
|
||||
continue; // No calibration
|
||||
}
|
||||
for (j = 0; j < meteo_stick.calib.params[i].num_temp; j++) {
|
||||
sdLogWriteLog(pprzLogFile, "# Reference temp: %.2f\n", meteo_stick.calib.params[i].temps[j]);
|
||||
sdLogWriteLog(pprzLogFile, "# Coeffs:");
|
||||
for (k = 0; k < meteo_stick.calib.params[i].num_coeff; k++) {
|
||||
sdLogWriteLog(pprzLogFile, " %.5f", meteo_stick.calib.params[i].coeffs[j][k]);
|
||||
}
|
||||
sdLogWriteLog(pprzLogFile, "\n");
|
||||
}
|
||||
}
|
||||
sdLogWriteLog(pprzLogFile, "#\n");
|
||||
sdLogWriteLog(pprzLogFile,
|
||||
"P(adc) T(adc) H(ticks) P_diff(adc) P(hPa) T(C) H(%%) CAS(m/s) FIX TOW(ms) WEEK Lat(1e7rad) Lon(1e7rad) HMSL(mm) GS(cm/s) course(1e7rad) VZ(cm/s)\n");
|
||||
log_ptu_started = true;
|
||||
}
|
||||
#else
|
||||
sdLogWriteLog(pprzLogFile,
|
||||
"P(adc) T(adc) H(ticks) P_diff(adc) P(hPa) T(C) H(%%) CAS(m/s) FIX TOW(ms) WEEK Lat(1e7rad) Lon(1e7rad) HMSL(mm) GS(cm/s) course(1e7rad) VZ(cm/s)\n");
|
||||
log_ptu_started = true;
|
||||
#endif
|
||||
} else {
|
||||
sdLogWriteLog(pprzLogFile, "%lu %lu %lu %lu %.2f %.2f %.2f %.2f %d %lu %d %lu %lu %lu %d %lu %lu\n",
|
||||
meteo_stick.pressure.data, meteo_stick.temperature.data,
|
||||
meteo_stick.humidity_period, meteo_stick.diff_pressure.data,
|
||||
meteo_stick.current_pressure, meteo_stick.current_temperature,
|
||||
meteo_stick.current_humidity, meteo_stick.current_airspeed,
|
||||
gps.fix, gps.tow, gps.week,
|
||||
gps.lla_pos.lat, gps.lla_pos.lon, gps.hmsl,
|
||||
gps.gspeed, gps.course, -gps.ned_vel.z);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
/** Includes and function to log to flight recorder
|
||||
*
|
||||
* FALSE by default
|
||||
*/
|
||||
#ifndef LOG_MS_FLIGHTRECORDER
|
||||
#define LOG_MS_FLIGHTRECORDER FALSE
|
||||
#endif
|
||||
|
||||
// log to flight recorder by default
|
||||
#ifndef MS_LOG_FILE
|
||||
#define MS_LOG_FILE flightrecorder_sdlog
|
||||
#endif
|
||||
|
||||
#if LOG_MS_FLIGHTRECORDER
|
||||
#include "modules/loggers/sdlog_chibios.h"
|
||||
#include "modules/loggers/pprzlog_tp.h"
|
||||
static bool log_tagged;
|
||||
|
||||
static inline void meteo_stick_log_data_fr(void)
|
||||
{
|
||||
if (MS_LOG_FILE.file != NULL && *(MS_LOG_FILE.file) != -1) {
|
||||
if (log_tagged == false && GpsFixValid()) {
|
||||
// write at least once ALIVE and GPS messages
|
||||
// to log for correct extraction of binary data
|
||||
DOWNLINK_SEND_ALIVE(pprzlog_tp, MS_LOG_FILE, 16, MD5SUM);
|
||||
// Log GPS for time reference
|
||||
uint8_t foo_u8 = 0;
|
||||
int16_t foo_16 = 0;
|
||||
uint16_t foo_u16 = 0;
|
||||
struct UtmCoor_f utm = *stateGetPositionUtm_f();
|
||||
int32_t east = utm.east * 100;
|
||||
int32_t north = utm.north * 100;
|
||||
DOWNLINK_SEND_GPS(pprzlog_tp, MS_LOG_FILE, &gps.fix,
|
||||
&east, &north, &foo_16, &gps.hmsl, &foo_u16, &foo_16,
|
||||
&gps.week, &gps.tow, &utm.zone, &foo_u8);
|
||||
log_tagged = true;
|
||||
}
|
||||
meteo_stick_send_data(&pprzlog_tp.trans_tx, &(MS_LOG_FILE).device);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
/** Init function
|
||||
*/
|
||||
@@ -279,6 +367,9 @@ void meteo_stick_init(void)
|
||||
#if LOG_MS
|
||||
log_ptu_started = false;
|
||||
#endif
|
||||
#if LOG_MS_FLIGHTRECORDER
|
||||
log_tagged = false;
|
||||
#endif
|
||||
}
|
||||
|
||||
/** Periodic function
|
||||
@@ -320,56 +411,15 @@ void meteo_stick_periodic(void)
|
||||
|
||||
// Log data
|
||||
#if LOG_MS
|
||||
if (pprzLogFile != -1) {
|
||||
if (!log_ptu_started) {
|
||||
#if USE_MS_EEPROM
|
||||
if (meteo_stick.eeprom.data_available) {
|
||||
// Print calibration data in the log header
|
||||
sdLogWriteLog(pprzLogFile, "# Calibration data (UUID: %s)\n#\n", meteo_stick.calib.uuid);
|
||||
int i, j, k;
|
||||
for (i = 0; i < MTOSTK_NUM_SENSORS; i++) {
|
||||
sdLogWriteLog(pprzLogFile, "# Sensor: %d, time: %d, num_temp: %d, num_coeff: %d\n", i,
|
||||
meteo_stick.calib.params[i].timestamp,
|
||||
meteo_stick.calib.params[i].num_temp,
|
||||
meteo_stick.calib.params[i].num_coeff);
|
||||
if (meteo_stick.calib.params[i].timestamp == 0) {
|
||||
continue; // No calibration
|
||||
}
|
||||
for (j = 0; j < meteo_stick.calib.params[i].num_temp; j++) {
|
||||
sdLogWriteLog(pprzLogFile, "# Reference temp: %.2f\n", meteo_stick.calib.params[i].temps[j]);
|
||||
sdLogWriteLog(pprzLogFile, "# Coeffs:");
|
||||
for (k = 0; k < meteo_stick.calib.params[i].num_coeff; k++) {
|
||||
sdLogWriteLog(pprzLogFile, " %.5f", meteo_stick.calib.params[i].coeffs[j][k]);
|
||||
}
|
||||
sdLogWriteLog(pprzLogFile, "\n");
|
||||
}
|
||||
}
|
||||
sdLogWriteLog(pprzLogFile, "#\n");
|
||||
sdLogWriteLog(pprzLogFile,
|
||||
"P(adc) T(adc) H(ticks) P_diff(adc) P(hPa) T(C) H(\%) CAS(m/s) FIX TOW(ms) WEEK Lat(1e7rad) Lon(1e7rad) HMSL(mm) GS(cm/s) course(1e7rad) VZ(cm/s)\n");
|
||||
log_ptu_started = true;
|
||||
}
|
||||
#else
|
||||
sdLogWriteLog(pprzLogFile,
|
||||
"P(adc) T(adc) H(ticks) P_diff(adc) P(hPa) T(C) H(%%) CAS(m/s) FIX TOW(ms) WEEK Lat(1e7rad) Lon(1e7rad) HMSL(mm) GS(cm/s) course(1e7rad) VZ(cm/s)\n");
|
||||
log_ptu_started = true;
|
||||
meteo_stick_log_data_ascii();
|
||||
#endif
|
||||
} else {
|
||||
sdLogWriteLog(pprzLogFile, "%lu %lu %lu %lu %.2f %.2f %.2f %.2f %d %lu %d %lu %lu %lu %d %lu %lu\n",
|
||||
meteo_stick.pressure.data, meteo_stick.temperature.data,
|
||||
meteo_stick.humidity_period, meteo_stick.diff_pressure.data,
|
||||
meteo_stick.current_pressure, meteo_stick.current_temperature,
|
||||
meteo_stick.current_humidity, meteo_stick.current_airspeed,
|
||||
gps.fix, gps.tow, gps.week,
|
||||
gps.lla_pos.lat, gps.lla_pos.lon, gps.hmsl,
|
||||
gps.gspeed, gps.course, -gps.ned_vel.z);
|
||||
}
|
||||
}
|
||||
#if LOG_MS_FLIGHTRECORDER
|
||||
meteo_stick_log_data_fr();
|
||||
#endif
|
||||
|
||||
// Send data
|
||||
#if SEND_MS
|
||||
meteo_stick_send_data();
|
||||
// Send data synch to reading
|
||||
#if SEND_MS_SYNC
|
||||
meteo_stick_send_data(&(DefaultChannel).trans_tx, &(DefaultDevice).device);
|
||||
#endif
|
||||
|
||||
// Check if DP offset reset is required
|
||||
@@ -444,3 +494,9 @@ void meteo_stick_event(void)
|
||||
#endif
|
||||
}
|
||||
|
||||
void meteo_stick_report(void)
|
||||
{
|
||||
// send meteo_stick data at a different frequency
|
||||
meteo_stick_send_data(&(DefaultChannel).trans_tx, &(DefaultDevice).device);
|
||||
}
|
||||
|
||||
|
||||
@@ -78,6 +78,7 @@ extern struct MeteoStick meteo_stick;
|
||||
extern void meteo_stick_init(void);
|
||||
extern void meteo_stick_periodic(void);
|
||||
extern void meteo_stick_event(void);
|
||||
extern void meteo_stick_report(void);
|
||||
|
||||
#endif
|
||||
|
||||
|
||||
@@ -32,6 +32,11 @@
|
||||
#include "modules/datalink/datalink.h"
|
||||
#include "modules/datalink/downlink.h"
|
||||
|
||||
// Check for unique ID by default
|
||||
#ifndef MISSION_CHECK_UNIQUE_ID
|
||||
#define MISSION_CHECK_UNIQUE_ID TRUE
|
||||
#endif
|
||||
|
||||
struct _mission mission = { 0 };
|
||||
|
||||
void mission_init(void)
|
||||
@@ -55,6 +60,14 @@ void mission_init(void)
|
||||
bool mission_insert(enum MissionInsertMode insert, struct _mission_element *element)
|
||||
{
|
||||
uint8_t tmp;
|
||||
|
||||
#if MISSION_CHECK_UNIQUE_ID
|
||||
// check that the new element id is not already in the list
|
||||
if (mission_get_from_index(element->index) != NULL) {
|
||||
return false;
|
||||
}
|
||||
#endif
|
||||
|
||||
// convert element if needed, return FALSE if failed
|
||||
if (!mission_element_convert(element)) { return false; }
|
||||
|
||||
@@ -71,16 +84,19 @@ bool mission_insert(enum MissionInsertMode insert, struct _mission_element *elem
|
||||
if (tmp == mission.insert_idx) { return false; } // no room to inser element
|
||||
mission.elements[tmp] = *element; // add element
|
||||
mission.current_idx = tmp; // move current index
|
||||
mission.element_time = 0.; // reset timer
|
||||
break;
|
||||
case ReplaceCurrent:
|
||||
// current element can always be modified, index are not changed
|
||||
mission.elements[mission.current_idx] = *element;
|
||||
mission.element_time = 0.; // reset timer
|
||||
break;
|
||||
case ReplaceAll:
|
||||
// reset queue and index
|
||||
mission.elements[0] = *element;
|
||||
mission.current_idx = 0;
|
||||
mission.insert_idx = 1;
|
||||
mission.element_time = 0.; // reset timer
|
||||
break;
|
||||
case ReplaceNexts:
|
||||
tmp = (mission.current_idx + 1) % MISSION_ELEMENT_NB;
|
||||
@@ -122,6 +138,22 @@ static struct _mission_registered *mission_get_registered(char *type)
|
||||
return NULL; // not found
|
||||
}
|
||||
|
||||
// Returns a pointer to a mission element struct with matching index, NULL if not found
|
||||
struct _mission_element *mission_get_from_index(uint8_t index)
|
||||
{
|
||||
uint8_t i = mission.current_idx;
|
||||
while (i != mission.insert_idx) {
|
||||
if (mission.elements[i].index == index) {
|
||||
return &mission.elements[i]; // return first next element with matching index
|
||||
}
|
||||
i++;
|
||||
if (i == MISSION_ELEMENT_NB) {
|
||||
i = 0;
|
||||
}
|
||||
}
|
||||
return NULL; // not found
|
||||
}
|
||||
|
||||
// Weak implementation of mission_element_convert (leave element unchanged)
|
||||
bool __attribute__((weak)) mission_element_convert(struct _mission_element *el __attribute__((unused))) { return true; }
|
||||
|
||||
@@ -378,12 +410,47 @@ int mission_parse_CUSTOM(uint8_t *buf)
|
||||
return mission_insert(insert, &me);
|
||||
}
|
||||
|
||||
int mission_parse_UPDATE(uint8_t *buf)
|
||||
{
|
||||
if (DL_MISSION_UPDATE_ac_id(buf) != AC_ID) { return false; } // not for this aircraft
|
||||
|
||||
struct _mission_element *me = mission_get_from_index(DL_MISSION_UPDATE_index(buf));
|
||||
if (me == NULL) {
|
||||
return false; // unknown type
|
||||
}
|
||||
|
||||
float duration = DL_MISSION_UPDATE_duration(buf);
|
||||
if (duration > -2.f) { // no update should in fact be -9
|
||||
me->duration = duration; // update
|
||||
}
|
||||
|
||||
uint8_t nb = DL_MISSION_UPDATE_params_length(buf);
|
||||
float params[MISSION_CUSTOM_MAX];
|
||||
memcpy(params, DL_MISSION_UPDATE_params(buf), nb*sizeof(float));
|
||||
|
||||
switch (me->type) {
|
||||
case MissionCustom:
|
||||
return me->element.mission_custom.reg->cb(nb, params, MissionUpdate);
|
||||
case MissionWP:
|
||||
case MissionCircle:
|
||||
case MissionSegment:
|
||||
case MissionPath:
|
||||
default:
|
||||
// TODO handle update param for standard patterns
|
||||
break;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
int mission_parse_GOTO_MISSION(uint8_t *buf)
|
||||
{
|
||||
if (DL_GOTO_MISSION_ac_id(buf) != AC_ID) { return false; } // not for this aircraft
|
||||
|
||||
uint8_t mission_id = DL_GOTO_MISSION_mission_id(buf);
|
||||
if (mission_id < MISSION_ELEMENT_NB) {
|
||||
// reset timer
|
||||
mission.element_time = 0.;
|
||||
// set current index
|
||||
mission.current_idx = mission_id;
|
||||
} else { return false; }
|
||||
|
||||
@@ -396,6 +463,8 @@ int mission_parse_NEXT_MISSION(uint8_t *buf)
|
||||
|
||||
if (mission.current_idx == mission.insert_idx) { return false; } // already at the last position
|
||||
|
||||
// reset timer
|
||||
mission.element_time = 0.;
|
||||
// increment current index
|
||||
mission.current_idx = (mission.current_idx + 1) % MISSION_ELEMENT_NB;
|
||||
return true;
|
||||
@@ -405,6 +474,8 @@ int mission_parse_END_MISSION(uint8_t *buf)
|
||||
{
|
||||
if (DL_END_MISSION_ac_id(buf) != AC_ID) { return false; } // not for this aircraft
|
||||
|
||||
// reset timer
|
||||
mission.element_time = 0.;
|
||||
// set current index to insert index (last position)
|
||||
mission.current_idx = mission.insert_idx;
|
||||
return true;
|
||||
|
||||
@@ -50,6 +50,12 @@ enum MissionInsertMode {
|
||||
ReplaceNexts ///< replace the next element and remove all the others
|
||||
};
|
||||
|
||||
enum MissionRunFlag {
|
||||
MissionRun = 0, ///< normal run
|
||||
MissionInit = 1, ///< first exec
|
||||
MissionUpdate = 2 ///< param update
|
||||
};
|
||||
|
||||
struct _mission_wp {
|
||||
union {
|
||||
struct EnuCoor_f wp_f;
|
||||
@@ -98,7 +104,7 @@ struct _mission_path {
|
||||
* @param[in] init true if the function is called for the first time
|
||||
* @return true until the function ends
|
||||
*/
|
||||
typedef bool (*mission_custom_cb)(uint8_t nb, float *params, bool init);
|
||||
typedef bool (*mission_custom_cb)(uint8_t nb, float *params, enum MissionRunFlag flag);
|
||||
|
||||
struct _mission_registered {
|
||||
mission_custom_cb cb; ///< navigation/action function callback
|
||||
@@ -178,6 +184,11 @@ extern bool mission_element_convert(struct _mission_element *el);
|
||||
*/
|
||||
extern struct _mission_element *mission_get(void);
|
||||
|
||||
/** Get mission element by index
|
||||
* @return return a pointer to the mission element or NULL if no more elements
|
||||
*/
|
||||
extern struct _mission_element *mission_get_from_index(uint8_t index);
|
||||
|
||||
/** Get the ENU component of LLA mission point
|
||||
* This function is firmware specific.
|
||||
* @param point pointer to the output ENU point (float)
|
||||
@@ -214,6 +225,7 @@ extern int mission_parse_SEGMENT_LLA(uint8_t *buf);
|
||||
extern int mission_parse_PATH(uint8_t *buf);
|
||||
extern int mission_parse_PATH_LLA(uint8_t *buf);
|
||||
extern int mission_parse_CUSTOM(uint8_t *buf);
|
||||
extern int mission_parse_UPDATE(uint8_t *buf);
|
||||
extern int mission_parse_GOTO_MISSION(uint8_t *buf);
|
||||
extern int mission_parse_NEXT_MISSION(uint8_t *buf);
|
||||
extern int mission_parse_END_MISSION(uint8_t *buf);
|
||||
|
||||
@@ -142,17 +142,50 @@ static inline bool mission_nav_path(struct _mission_path *path)
|
||||
*/
|
||||
static inline bool mission_nav_custom(struct _mission_custom *custom, bool init)
|
||||
{
|
||||
return custom->reg->cb(custom->nb, custom->params, init);
|
||||
if (init) {
|
||||
return custom->reg->cb(custom->nb, custom->params, MissionInit);
|
||||
} else {
|
||||
return custom->reg->cb(custom->nb, custom->params, MissionRun);
|
||||
}
|
||||
}
|
||||
|
||||
/** Implement waiting pattern
|
||||
* Only called when MISSION_WAIT_TIMEOUT is not 0
|
||||
*/
|
||||
#ifndef MISSION_WAIT_TIMEOUT
|
||||
#define MISSION_WAIT_TIMEOUT 120 // wait 2 minutes before ending mission
|
||||
#endif
|
||||
|
||||
static bool mission_wait_started = false;
|
||||
#if MISSION_WAIT_TIMEOUT
|
||||
static float mission_wait_time = 0.f;
|
||||
static struct _mission_circle mission_wait_circle;
|
||||
static bool mission_wait_pattern(void) {
|
||||
if (!mission_wait_started) {
|
||||
mission_wait_circle.center.center_f = *stateGetPositionEnu_f();
|
||||
mission_wait_circle.center.center_f.z += GetAltRef();
|
||||
mission_wait_circle.radius = DEFAULT_CIRCLE_RADIUS;
|
||||
mission_wait_time = 0.f;
|
||||
mission_wait_started = true;
|
||||
}
|
||||
mission_nav_circle(&mission_wait_circle);
|
||||
mission_wait_time += dt_navigation;
|
||||
return (mission_wait_time < (float)MISSION_WAIT_TIMEOUT); // keep flying until TIMEOUT
|
||||
}
|
||||
#else
|
||||
static bool mission_wait_pattern(void) {
|
||||
return false; // no TIMEOUT, end mission now
|
||||
}
|
||||
#endif
|
||||
|
||||
int mission_run()
|
||||
{
|
||||
// current element
|
||||
struct _mission_element *el = NULL;
|
||||
if ((el = mission_get()) == NULL) {
|
||||
// TODO do something special like a waiting circle before ending the mission ?
|
||||
return false; // end of mission
|
||||
return mission_wait_pattern();
|
||||
}
|
||||
mission_wait_started = false;
|
||||
|
||||
bool el_running = false;
|
||||
switch (el->type) {
|
||||
|
||||
@@ -224,18 +224,48 @@ static inline bool mission_nav_path(struct _mission_element *el)
|
||||
*/
|
||||
static inline bool mission_nav_custom(struct _mission_custom *custom, bool init)
|
||||
{
|
||||
return custom->reg->cb(custom->nb, custom->params, init);
|
||||
if (init) {
|
||||
return custom->reg->cb(custom->nb, custom->params, MissionInit);
|
||||
} else {
|
||||
return custom->reg->cb(custom->nb, custom->params, MissionRun);
|
||||
}
|
||||
}
|
||||
|
||||
/** Implement waiting pattern
|
||||
* Only called when MISSION_WAIT_TIMEOUT is not 0
|
||||
*/
|
||||
#ifndef MISSION_WAIT_TIMEOUT
|
||||
#define MISSION_WAIT_TIMEOUT 30 // wait 30 seconds before ending mission
|
||||
#endif
|
||||
|
||||
static bool mission_wait_started = false;
|
||||
#if MISSION_WAIT_TIMEOUT
|
||||
static float mission_wait_time = 0.f;
|
||||
static struct _mission_element mission_wait_wp;
|
||||
static bool mission_wait_pattern(void) {
|
||||
if (!mission_wait_started) {
|
||||
mission_wait_wp.element.mission_wp.wp.wp_i = *stateGetPositionEnu_i();
|
||||
mission_wait_time = 0.f;
|
||||
mission_wait_started = true;
|
||||
}
|
||||
mission_nav_wp(&mission_wait_wp);
|
||||
mission_wait_time += dt_navigation;
|
||||
return (mission_wait_time < (float)MISSION_WAIT_TIMEOUT); // keep flying until TIMEOUT
|
||||
}
|
||||
#else
|
||||
static bool mission_wait_pattern(void) {
|
||||
return false; // no TIMEOUT, end mission now
|
||||
}
|
||||
#endif
|
||||
|
||||
int mission_run()
|
||||
{
|
||||
// current element
|
||||
struct _mission_element *el = NULL;
|
||||
if ((el = mission_get()) == NULL) {
|
||||
mission.element_time = 0;
|
||||
mission.current_idx = 0;
|
||||
return false; // end of mission
|
||||
return mission_wait_pattern();
|
||||
}
|
||||
mission_wait_started = false;
|
||||
|
||||
bool el_running = false;
|
||||
switch (el->type) {
|
||||
|
||||
@@ -28,6 +28,7 @@
|
||||
#include "generated/flight_plan.h"
|
||||
#include "modules/ins/ins.h"
|
||||
#include "math/pprz_geodetic_float.h"
|
||||
#include "modules/datalink/downlink.h"
|
||||
|
||||
float dist2_to_home;
|
||||
float dist2_to_wp;
|
||||
@@ -173,3 +174,48 @@ void nav_move_waypoint(uint8_t wp_id, float ux, float uy, float alt)
|
||||
waypoints[wp_id].a = alt;
|
||||
}
|
||||
}
|
||||
|
||||
/** Move a waypoint in local frame.
|
||||
* @param[in] wp_id Waypoint ID
|
||||
* @param[in] ux x (east) coordinate
|
||||
* @param[in] uy y (north) coordinate
|
||||
* @param[in] alt Altitude above MSL.
|
||||
*/
|
||||
void nav_move_waypoint_enu(uint8_t wp_id, float x, float y, float alt)
|
||||
{
|
||||
if (wp_id < nb_waypoint) {
|
||||
float dx, dy;
|
||||
dx = x - waypoints[WP_HOME].x;
|
||||
dy = y - waypoints[WP_HOME].y;
|
||||
BoundAbs(dx, max_dist_from_home);
|
||||
BoundAbs(dy, max_dist_from_home);
|
||||
waypoints[wp_id].x = waypoints[WP_HOME].x + dx;
|
||||
waypoints[wp_id].y = waypoints[WP_HOME].y + dy;
|
||||
waypoints[wp_id].a = alt;
|
||||
}
|
||||
}
|
||||
|
||||
/** Move a waypoint from point structure (local frame).
|
||||
* @param[in] wp_id Waypoint ID
|
||||
* @param[in] p new point
|
||||
*/
|
||||
void nav_move_waypoint_point(uint8_t wp_id, struct point *p)
|
||||
{
|
||||
nav_move_waypoint_enu(wp_id, p->x, p->y, p->a);
|
||||
}
|
||||
|
||||
/** Send a waypoint throught default telemetry channel
|
||||
* @param[in] wp_id Waypoint ID
|
||||
*/
|
||||
void nav_send_waypoint(uint8_t wp_id)
|
||||
{
|
||||
if (wp_id < nb_waypoint) {
|
||||
struct UtmCoor_f utm;
|
||||
utm.zone = nav_utm_zone0;
|
||||
utm.east = waypoints[wp_id].x + nav_utm_east0;
|
||||
utm.north = waypoints[wp_id].y + nav_utm_north0;
|
||||
utm.alt = waypoints[wp_id].a;
|
||||
DOWNLINK_SEND_WP_MOVED(DefaultChannel, DefaultDevice, &wp_id, &utm.east, &utm.north, &utm.alt, &utm.zone);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -49,6 +49,9 @@ struct point {
|
||||
#define Height(_h) (_h + ground_alt)
|
||||
|
||||
extern void nav_move_waypoint(uint8_t wp_id, float utm_east, float utm_north, float alt);
|
||||
extern void nav_move_waypoint_enu(uint8_t wp_id, float x, float y, float alt);
|
||||
extern void nav_move_waypoint_point(uint8_t wp_id, struct point *p);
|
||||
extern void nav_send_waypoint(uint8_t wp_id);
|
||||
|
||||
extern const uint8_t nb_waypoint;
|
||||
extern struct point waypoints[];
|
||||
|
||||
@@ -35,12 +35,12 @@
|
||||
#if USE_MISSION
|
||||
#include "modules/mission/mission_common.h"
|
||||
|
||||
static bool nav_flower_mission(uint8_t nb, float *params, bool init)
|
||||
static bool nav_flower_mission(uint8_t nb, float *params, enum MissionRunFlag flag)
|
||||
{
|
||||
if (nb != 2) {
|
||||
return false; // wrong number of parameters
|
||||
}
|
||||
if (init) {
|
||||
if (flag == MissionInit) {
|
||||
uint8_t center = (uint8_t)(params[0]);
|
||||
uint8_t edge = (uint8_t)(params[1]);
|
||||
nav_flower_setup(center, edge);
|
||||
|
||||
@@ -0,0 +1,359 @@
|
||||
/*
|
||||
* Copyright (C) 2018-2021 Gautier Hattenberger <gautier.hattenberger@enac.fr>
|
||||
* Titouan Verdu <titouan.verdu@enac.fr>
|
||||
*
|
||||
* This file is part of paparazzi.
|
||||
*
|
||||
* paparazzi is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 2, or (at your option)
|
||||
* any later version.
|
||||
*
|
||||
* paparazzi is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with paparazzi; see the file COPYING. If not, see
|
||||
* <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
/**
|
||||
* @file modules/nav/nav_lace.c
|
||||
* @author VERDU Titouan
|
||||
*
|
||||
* Adaptive border pattern for cloud exploration
|
||||
*/
|
||||
|
||||
#include "modules/nav/nav_lace.h"
|
||||
|
||||
#include "firmwares/fixedwing/nav.h"
|
||||
#include "state.h"
|
||||
#include "autopilot.h"
|
||||
#include "generated/flight_plan.h"
|
||||
#include "modules/core/abi.h"
|
||||
|
||||
#ifndef NAV_LACE_RECOVER_MAX_TURN
|
||||
#define NAV_LACE_RECOVER_MAX_TURN 1.5f
|
||||
#endif
|
||||
|
||||
#ifndef NAV_LACE_BORDER_FILTER
|
||||
#define NAV_LACE_BORDER_FILTER 20.f
|
||||
#endif
|
||||
|
||||
enum LaceStatus {
|
||||
LACE_ENTER,
|
||||
LACE_INSIDE_START,
|
||||
LACE_INSIDE,
|
||||
LACE_OUTSIDE_START,
|
||||
LACE_OUTSIDE,
|
||||
LACE_RECOVER_START,
|
||||
LACE_RECOVER_INSIDE,
|
||||
LACE_RECOVER_OUTSIDE
|
||||
};
|
||||
|
||||
enum RotationDir {
|
||||
LACE_LEFT,
|
||||
LACE_RIGHT
|
||||
};
|
||||
|
||||
struct NavLace {
|
||||
enum LaceStatus status;
|
||||
enum RotationDir rotation;
|
||||
bool inside_cloud;
|
||||
struct EnuCoor_f actual;
|
||||
struct EnuCoor_f target;
|
||||
struct EnuCoor_f circle;
|
||||
struct EnuCoor_f estim_border;
|
||||
struct EnuCoor_f recover_circle;
|
||||
struct FloatVect3 pos_incr;
|
||||
float direction;
|
||||
float radius;
|
||||
float radius_sign;
|
||||
float last_border_time;
|
||||
float recover_radius;
|
||||
float max_recover_radius;
|
||||
};
|
||||
|
||||
static struct NavLace nav_lace;
|
||||
|
||||
static const float nav_dt = 1.f / NAVIGATION_FREQUENCY;
|
||||
|
||||
static float change_rep(float dir)
|
||||
{
|
||||
return M_PI_2 - dir;
|
||||
}
|
||||
|
||||
static struct EnuCoor_f process_new_point_lace(struct EnuCoor_f *position, float alt_sp, float uav_direction)
|
||||
{
|
||||
struct EnuCoor_f new_point;
|
||||
float rot_angle;
|
||||
|
||||
if (nav_lace.rotation == LACE_RIGHT) {
|
||||
rot_angle = -M_PI_2;
|
||||
nav_lace.rotation = LACE_LEFT;
|
||||
} else {
|
||||
rot_angle = M_PI_2;
|
||||
nav_lace.rotation = LACE_RIGHT;
|
||||
}
|
||||
|
||||
new_point.x = position->x + (cosf(rot_angle + uav_direction) * nav_lace.radius);
|
||||
new_point.y = position->y + (sinf(rot_angle + uav_direction) * nav_lace.radius);
|
||||
new_point.z = alt_sp;
|
||||
|
||||
return new_point;
|
||||
}
|
||||
|
||||
static void update_target_point(struct EnuCoor_f *target, struct EnuCoor_f *border, float dt, float tau)
|
||||
{
|
||||
// with a proper discrete transform, coeff should be exp(-dt/tau) and (1-exp(-dt/tau))
|
||||
// but to make it simpler with the same behavior at the limits, we use tau/(dt+tau) and dt/(dt+tau)
|
||||
// with a positive value for tau
|
||||
if (tau > 1e-4) {
|
||||
float alpha = dt / (dt + tau);
|
||||
target->x = (border->x * alpha) + (target->x * (1.f - alpha));
|
||||
target->y = (border->y * alpha) + (target->y * (1.f - alpha));
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
#if USE_MISSION
|
||||
#include "modules/mission/mission_common.h"
|
||||
|
||||
static bool nav_lace_mission(uint8_t nb, float *params, enum MissionRunFlag flag)
|
||||
{
|
||||
if (flag == MissionInit && nb == 8) {
|
||||
float start_x = params[0];
|
||||
float start_y = params[1];
|
||||
float start_z = params[2];
|
||||
int first_turn = params[3];
|
||||
float circle_radius = params[4];
|
||||
float vx = params[5];
|
||||
float vy = params[6];
|
||||
float vz = params[7];
|
||||
nav_lace_setup(start_x, start_y, start_z, first_turn, circle_radius, vx, vy, vz);
|
||||
return true;
|
||||
}
|
||||
else if (flag == MissionUpdate && nb == 3) {
|
||||
// update target 3D position (ENU frame, above ground alt)
|
||||
float x = params[0];
|
||||
float y = params[1];
|
||||
float z = params[2];
|
||||
VECT3_ASSIGN(nav_lace.target, x, y, z);
|
||||
return true;
|
||||
}
|
||||
else if (flag == MissionUpdate && nb == 2) {
|
||||
// update horizontal speed
|
||||
float vx = params[0];
|
||||
float vy = params[1];
|
||||
nav_lace.pos_incr.x = vx * nav_dt;
|
||||
nav_lace.pos_incr.y = vy * nav_dt;
|
||||
return true;
|
||||
}
|
||||
else if (flag == MissionUpdate && nb == 1) {
|
||||
// update vertical speed
|
||||
float vz = params[0];
|
||||
nav_lace.pos_incr.z = vz * nav_dt;
|
||||
return true;
|
||||
}
|
||||
else if (flag == MissionRun) {
|
||||
return nav_lace_run();
|
||||
}
|
||||
return false; // not a valid case
|
||||
}
|
||||
#endif
|
||||
|
||||
// ABI message
|
||||
|
||||
#ifndef NAV_LACE_LWC_ID
|
||||
#define NAV_LACE_LWC_ID ABI_BROADCAST
|
||||
#endif
|
||||
|
||||
static abi_event lwc_ev;
|
||||
|
||||
static void lwc_cb(uint8_t sender_id UNUSED, uint32_t stamp UNUSED, int32_t data_type, uint32_t size, uint8_t * data) {
|
||||
if (data_type == 1 && size == 1) {
|
||||
nav_lace.inside_cloud = (bool) data[0];
|
||||
}
|
||||
}
|
||||
|
||||
void nav_lace_init(void)
|
||||
{
|
||||
nav_lace.status = LACE_ENTER;
|
||||
nav_lace.radius = DEFAULT_CIRCLE_RADIUS;
|
||||
nav_lace.recover_radius = DEFAULT_CIRCLE_RADIUS;
|
||||
nav_lace.max_recover_radius = DEFAULT_CIRCLE_RADIUS;
|
||||
nav_lace.last_border_time = 0.f;
|
||||
nav_lace.inside_cloud = false;
|
||||
|
||||
AbiBindMsgPAYLOAD_DATA(NAV_LACE_LWC_ID, &lwc_ev, lwc_cb);
|
||||
|
||||
#if USE_MISSION
|
||||
mission_register(nav_lace_mission, "LACE");
|
||||
#endif
|
||||
}
|
||||
|
||||
void nav_lace_setup(float init_x, float init_y,
|
||||
float init_z, int turn,
|
||||
float desired_radius, float vx,
|
||||
float vy, float vz)
|
||||
{
|
||||
struct EnuCoor_f start = {init_x, init_y, init_z};
|
||||
// increment based on speed
|
||||
VECT3_ASSIGN(nav_lace.pos_incr, vx*nav_dt, vy*nav_dt, vz*nav_dt);
|
||||
|
||||
nav_lace.target = start;
|
||||
nav_lace.status = LACE_ENTER;
|
||||
nav_lace.inside_cloud = false;
|
||||
nav_lace.radius = desired_radius;
|
||||
|
||||
if (turn == 1) {
|
||||
nav_lace.rotation = LACE_RIGHT;
|
||||
nav_lace.radius_sign = 1.0f;
|
||||
} else {
|
||||
nav_lace.rotation = LACE_LEFT;
|
||||
nav_lace.radius_sign = -1.0f;
|
||||
}
|
||||
|
||||
nav_lace.actual = *stateGetPositionEnu_f();
|
||||
}
|
||||
|
||||
bool nav_lace_run(void)
|
||||
{
|
||||
float pre_climb = 0.f;
|
||||
float time = get_sys_time_float();
|
||||
|
||||
NavVerticalAutoThrottleMode(0.f); /* No pitch */
|
||||
|
||||
switch (nav_lace.status) {
|
||||
case LACE_ENTER:
|
||||
// init stage
|
||||
nav_init_stage();
|
||||
// reach target point
|
||||
nav_route_xy(nav_lace.actual.x, nav_lace.actual.y, nav_lace.target.x, nav_lace.target.y);
|
||||
NavVerticalAltitudeMode(nav_lace.target.z + ground_alt, pre_climb);
|
||||
|
||||
if (nav_lace.inside_cloud) {
|
||||
// found border or already inside
|
||||
nav_lace.status = LACE_INSIDE_START;
|
||||
// update target for horizontal position
|
||||
nav_lace.target.x = stateGetPositionEnu_f()->x;
|
||||
nav_lace.target.y = stateGetPositionEnu_f()->y;
|
||||
}
|
||||
break;
|
||||
case LACE_INSIDE_START:
|
||||
// prepare inside circle
|
||||
nav_lace.actual = *stateGetPositionEnu_f();
|
||||
nav_lace.direction = change_rep(stateGetHorizontalSpeedDir_f());
|
||||
nav_lace.circle = process_new_point_lace(&nav_lace.actual, nav_lace.target.z, nav_lace.direction);
|
||||
// init stage
|
||||
nav_init_stage();
|
||||
// update border and target for recover
|
||||
nav_lace.estim_border = nav_lace.actual;
|
||||
update_target_point(&nav_lace.target, &nav_lace.estim_border, time - nav_lace.last_border_time, NAV_LACE_BORDER_FILTER);
|
||||
nav_lace.last_border_time = time;
|
||||
// fly inside
|
||||
nav_lace.status = LACE_INSIDE;
|
||||
break;
|
||||
case LACE_INSIDE:
|
||||
// increment center position
|
||||
VECT3_ADD(nav_lace.circle, nav_lace.pos_incr);
|
||||
nav_circle_XY(nav_lace.circle.x, nav_lace.circle.y , nav_lace.radius_sign * nav_lace.radius);
|
||||
pre_climb = nav_lace.pos_incr.z / nav_dt;
|
||||
NavVerticalAltitudeMode(nav_lace.circle.z + ground_alt, pre_climb);
|
||||
|
||||
if (!nav_lace.inside_cloud) {
|
||||
// found border, start outside
|
||||
nav_lace.status = LACE_OUTSIDE_START;
|
||||
nav_lace.radius_sign = -1.0f * nav_lace.radius_sign;
|
||||
}
|
||||
else if (NavCircleCountNoRewind() > NAV_LACE_RECOVER_MAX_TURN) {
|
||||
// most likely lost inside
|
||||
nav_lace.status = LACE_RECOVER_START;
|
||||
}
|
||||
break;
|
||||
case LACE_OUTSIDE_START:
|
||||
// prepare outside circle
|
||||
nav_lace.actual = *stateGetPositionEnu_f();
|
||||
nav_lace.direction = change_rep(stateGetHorizontalSpeedDir_f());
|
||||
nav_lace.circle = process_new_point_lace(&nav_lace.actual, nav_lace.circle.z, nav_lace.direction);
|
||||
// init stage
|
||||
nav_init_stage();
|
||||
// upadte border and target for recover
|
||||
nav_lace.estim_border = nav_lace.actual;
|
||||
update_target_point(&nav_lace.target, &nav_lace.estim_border, time - nav_lace.last_border_time, NAV_LACE_BORDER_FILTER);
|
||||
nav_lace.last_border_time = time;
|
||||
// fly outside
|
||||
nav_lace.status = LACE_OUTSIDE;
|
||||
break;
|
||||
case LACE_OUTSIDE:
|
||||
// increment center position
|
||||
VECT3_ADD(nav_lace.circle, nav_lace.pos_incr);
|
||||
pre_climb = nav_lace.pos_incr.z / nav_dt;
|
||||
nav_circle_XY(nav_lace.circle.x, nav_lace.circle.y , nav_lace.radius_sign * nav_lace.radius);
|
||||
NavVerticalAltitudeMode(nav_lace.circle.z + ground_alt, pre_climb);
|
||||
|
||||
if (nav_lace.inside_cloud) {
|
||||
// found border, start inside
|
||||
nav_lace.status = LACE_INSIDE_START;
|
||||
nav_lace.radius_sign = -1.0f * nav_lace.radius_sign;
|
||||
}
|
||||
else if (NavCircleCountNoRewind() > NAV_LACE_RECOVER_MAX_TURN) {
|
||||
// most likely lost outside
|
||||
nav_lace.status = LACE_RECOVER_START;
|
||||
}
|
||||
break;
|
||||
case LACE_RECOVER_START:
|
||||
// prepare recovery circle or line
|
||||
nav_lace.recover_circle = nav_lace.target;
|
||||
nav_lace.actual = *stateGetPositionEnu_f();
|
||||
// initial recovery radius
|
||||
nav_lace.recover_radius = nav_lace.radius;
|
||||
nav_lace.max_recover_radius = 2.0f * nav_lace.recover_radius; // FIXME ?
|
||||
// init stage
|
||||
nav_init_stage();
|
||||
if (nav_lace.inside_cloud) {
|
||||
nav_lace.status = LACE_RECOVER_INSIDE;
|
||||
}
|
||||
else {
|
||||
nav_lace.status = LACE_RECOVER_OUTSIDE;
|
||||
}
|
||||
break;
|
||||
case LACE_RECOVER_INSIDE:
|
||||
// increment border position
|
||||
// fly in opposite direction to cover more space
|
||||
nav_route_xy(nav_lace.estim_border.x, nav_lace.estim_border.y, nav_lace.actual.x, nav_lace.actual.y);
|
||||
if (!nav_lace.inside_cloud) {
|
||||
nav_lace.status = LACE_OUTSIDE_START;
|
||||
nav_lace.radius_sign = -1.0f * nav_lace.radius_sign;
|
||||
}
|
||||
break;
|
||||
case LACE_RECOVER_OUTSIDE:
|
||||
// increment center position
|
||||
VECT3_ADD(nav_lace.recover_circle, nav_lace.pos_incr);
|
||||
nav_circle_XY(nav_lace.recover_circle.x, nav_lace.recover_circle.y , nav_lace.radius_sign * nav_lace.recover_radius);
|
||||
// increment recover circle radius
|
||||
if (nav_lace.recover_radius < nav_lace.max_recover_radius) {
|
||||
nav_lace.recover_radius += 0.5;
|
||||
}
|
||||
// found a new border
|
||||
if (nav_lace.inside_cloud) {
|
||||
nav_lace.status = LACE_INSIDE_START;
|
||||
nav_lace.radius_sign = -1.0f * nav_lace.radius_sign;
|
||||
}
|
||||
break;
|
||||
default:
|
||||
// error, leaving
|
||||
return false;
|
||||
}
|
||||
// increment border and target positions
|
||||
VECT3_ADD(nav_lace.estim_border, nav_lace.pos_incr);
|
||||
VECT3_ADD(nav_lace.target, nav_lace.pos_incr);
|
||||
#ifdef WP_TARGET
|
||||
nav_move_waypoint_enu(WP_TARGET, nav_lace.target.x, nav_lace.target.y, nav_lace.target.z + ground_alt);
|
||||
RunOnceEvery(10, nav_send_waypoint(WP_TARGET));
|
||||
#endif
|
||||
|
||||
return true;
|
||||
}
|
||||
@@ -0,0 +1,56 @@
|
||||
/*
|
||||
* Copyright (C) 2018-2019 Gautier Hattenberger <gautier.hattenberger@enac.fr>
|
||||
* Titouan Verdu <titouan.verdu@enac.fr>
|
||||
*
|
||||
* This file is part of paparazzi.
|
||||
*
|
||||
* paparazzi is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 2, or (at your option)
|
||||
* any later version.
|
||||
*
|
||||
* paparazzi is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with paparazzi; see the file COPYING. If not, see
|
||||
* <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
/**
|
||||
* @file modules/nav/nav_lace.h
|
||||
*
|
||||
* Adaptive border pattern for cloud exploration
|
||||
* Can be used in mission mode with custom pattern and ID "LACE"
|
||||
*
|
||||
*
|
||||
* See:
|
||||
* Titouan Verdu, Gautier Hattenberger, Simon Lacroix. Flight patterns for clouds exploration with a fleet of UAVs. 2019 International Conference on Unmanned Aircraft Systems (ICUAS 2019), Jul 2019, Atlanta, United States.
|
||||
* https://hal-enac.archives-ouvertes.fr/hal-02137839
|
||||
*/
|
||||
|
||||
#ifndef NAV_LACE_H
|
||||
#define NAV_LACE_H
|
||||
|
||||
#include "std.h"
|
||||
#include "math/pprz_geodetic_float.h"
|
||||
|
||||
/** Init function called by modules init
|
||||
*/
|
||||
extern void nav_lace_init(void);
|
||||
|
||||
/** Initialized the exploration with a first target point inside the cloud
|
||||
* Called from flight plan or with mission parameters
|
||||
*/
|
||||
extern void nav_lace_setup(float init_x, float init_y, float init_z, int turn, float desired_radius, float vx, float vy, float vz);
|
||||
|
||||
/** Navigation function
|
||||
* Called by flight plan or mission run function
|
||||
* @return true until pattern ends or fail
|
||||
*/
|
||||
extern bool nav_lace_run(void);
|
||||
|
||||
#endif
|
||||
|
||||
@@ -29,10 +29,37 @@
|
||||
#include "modules/nav/nav_line.h"
|
||||
#include "firmwares/fixedwing/nav.h"
|
||||
|
||||
#if USE_MISSION
|
||||
#include "modules/mission/mission_common.h"
|
||||
|
||||
static bool nav_line_mission(uint8_t nb, float *params, enum MissionRunFlag flag)
|
||||
{
|
||||
if (nb != 3) {
|
||||
return false; // wrong number of parameters
|
||||
}
|
||||
if (flag == MissionInit) {
|
||||
nav_line_setup();
|
||||
} else if (flag == MissionUpdate) {
|
||||
return false; // nothing to do on update
|
||||
}
|
||||
uint8_t p1 = (uint8_t)(params[0]);
|
||||
uint8_t p2 = (uint8_t)(params[1]);
|
||||
float radius = params[2];
|
||||
return nav_line_run(p1, p2, radius);
|
||||
}
|
||||
#endif
|
||||
|
||||
/** Status along the pattern */
|
||||
enum line_status { LR12, LQC21, LTC2, LQC22, LR21, LQC12, LTC1, LQC11 };
|
||||
static enum line_status line_status;
|
||||
|
||||
void nav_line_init(void)
|
||||
{
|
||||
#if USE_MISSION
|
||||
mission_register(nav_line_mission, "LINE");
|
||||
#endif
|
||||
}
|
||||
|
||||
void nav_line_setup(void)
|
||||
{
|
||||
line_status = LR12;
|
||||
|
||||
@@ -30,6 +30,7 @@
|
||||
|
||||
#include "std.h"
|
||||
|
||||
extern void nav_line_init(void);
|
||||
extern void nav_line_setup(void);
|
||||
extern bool nav_line_run(uint8_t wp1, uint8_t wp2, float radius);
|
||||
|
||||
|
||||
@@ -0,0 +1,358 @@
|
||||
/*
|
||||
* Copyright (C) 2018-2019 Gautier Hattenberger <gautier.hattenberger@enac.fr>
|
||||
* Titouan Verdu <titouan.verdu@enac.fr>
|
||||
*
|
||||
* This file is part of paparazzi.
|
||||
*
|
||||
* paparazzi is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 2, or (at your option)
|
||||
* any later version.
|
||||
*
|
||||
* paparazzi is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with paparazzi; see the file COPYING. If not, see
|
||||
* <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
/**
|
||||
* @file modules/nav/nav_rosette.c
|
||||
*
|
||||
* Adaptive flower pattern for cloud exploration
|
||||
*/
|
||||
|
||||
#include "modules/nav/nav_rosette.h"
|
||||
|
||||
#include "firmwares/fixedwing/nav.h"
|
||||
#include "state.h"
|
||||
#include "autopilot.h"
|
||||
#include "generated/flight_plan.h"
|
||||
#include "modules/core/abi.h"
|
||||
|
||||
#ifndef NAV_ROSETTE_RECOVER_MAX_TURN
|
||||
#define NAV_ROSETTE_RECOVER_MAX_TURN 1.5f
|
||||
#endif
|
||||
|
||||
#ifndef NAV_ROSETTE_BORDER_FILTER
|
||||
#define NAV_ROSETTE_BORDER_FILTER 60.f
|
||||
#endif
|
||||
|
||||
enum RosetteStatus {
|
||||
RSTT_ENTER,
|
||||
RSTT_CROSSING_FIRST,
|
||||
RSTT_CROSSING_START,
|
||||
RSTT_CROSSING,
|
||||
RSTT_TURNING_START,
|
||||
RSTT_TURNING,
|
||||
RSTT_RECOVER_START,
|
||||
RSTT_RECOVER_OUTSIDE
|
||||
};
|
||||
|
||||
enum RotationDir {
|
||||
RSTT_LEFT,
|
||||
RSTT_RIGHT
|
||||
};
|
||||
|
||||
struct NavRosette {
|
||||
enum RosetteStatus status;
|
||||
enum RotationDir rotation;
|
||||
bool inside_cloud;
|
||||
struct EnuCoor_f actual;
|
||||
struct EnuCoor_f target;
|
||||
struct EnuCoor_f circle;
|
||||
struct EnuCoor_f estim_border;
|
||||
struct EnuCoor_f recover_circle;
|
||||
struct EnuCoor_f * first;
|
||||
struct FloatVect3 pos_incr;
|
||||
float direction;
|
||||
float radius;
|
||||
float radius_sign;
|
||||
float last_border_time;
|
||||
float recover_radius;
|
||||
float max_recover_radius;
|
||||
};
|
||||
|
||||
static struct NavRosette nav_rosette;
|
||||
|
||||
static const float nav_dt = 1.f / NAVIGATION_FREQUENCY;
|
||||
|
||||
static float change_rep(float dir)
|
||||
{
|
||||
return M_PI_2 - dir;
|
||||
}
|
||||
|
||||
static void update_barycenter(struct EnuCoor_f *bary, struct EnuCoor_f *border, struct EnuCoor_f *first, float alt_sp, float dt, float tau)
|
||||
{
|
||||
if (first != NULL) {
|
||||
bary->x = (border->x + first->x) / 2.f;
|
||||
bary->y = (border->y + first->y) / 2.f;
|
||||
bary->z = alt_sp;
|
||||
}
|
||||
else {
|
||||
if (tau > 1e-4) {
|
||||
float alpha = dt / (dt + tau);
|
||||
bary->x = (border->x * alpha) + (bary->x * (1.f - alpha));
|
||||
bary->y = (border->y * alpha) + (bary->y * (1.f - alpha));
|
||||
bary->z = alt_sp;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static struct EnuCoor_f process_new_point_rosette(struct EnuCoor_f *position, float uav_direction)
|
||||
{
|
||||
struct EnuCoor_f new_point;
|
||||
float rot_angle;
|
||||
|
||||
if (nav_rosette.rotation == RSTT_RIGHT) {
|
||||
rot_angle = -M_PI_2;
|
||||
} else {
|
||||
rot_angle = M_PI_2;
|
||||
}
|
||||
|
||||
if (nav_rosette.inside_cloud == true) {
|
||||
new_point.x = nav_rosette.target.x;
|
||||
new_point.y = nav_rosette.target.y;
|
||||
new_point.z = nav_rosette.target.z;
|
||||
}
|
||||
else if (nav_rosette.inside_cloud == false) {
|
||||
new_point.x = position->x + (cosf(rot_angle + uav_direction) * nav_rosette.radius);
|
||||
new_point.y = position->y + (sinf(rot_angle + uav_direction) * nav_rosette.radius);
|
||||
new_point.z = nav_rosette.target.z;
|
||||
}
|
||||
|
||||
return new_point;
|
||||
}
|
||||
|
||||
#if USE_MISSION
|
||||
#include "modules/mission/mission_common.h"
|
||||
|
||||
static bool nav_rosette_mission(uint8_t nb, float *params, enum MissionRunFlag flag)
|
||||
{
|
||||
if (flag == MissionInit && nb == 8) {
|
||||
float start_x = params[0];
|
||||
float start_y = params[1];
|
||||
float start_z = params[2];
|
||||
int first_turn = params[3];
|
||||
float circle_radius = params[4];
|
||||
float vx = params[5];
|
||||
float vy = params[6];
|
||||
float vz = params[7];
|
||||
nav_rosette_setup(start_x, start_y, start_z, first_turn, circle_radius, vx, vy, vz);
|
||||
return true;
|
||||
}
|
||||
else if (flag == MissionUpdate && nb == 3) {
|
||||
// update target 3D position (ENU frame, above ground alt)
|
||||
float x = params[0];
|
||||
float y = params[1];
|
||||
float z = params[2];
|
||||
VECT3_ASSIGN(nav_rosette.target, x, y, z);
|
||||
return true;
|
||||
}
|
||||
else if (flag == MissionUpdate && nb == 2) {
|
||||
// update horizontal speed
|
||||
float vx = params[0];
|
||||
float vy = params[1];
|
||||
nav_rosette.pos_incr.x = vx * nav_dt;
|
||||
nav_rosette.pos_incr.y = vy * nav_dt;
|
||||
return true;
|
||||
}
|
||||
else if (flag == MissionUpdate && nb == 1) {
|
||||
// update vertical speed
|
||||
float vz = params[0];
|
||||
nav_rosette.pos_incr.z = vz * nav_dt;
|
||||
return true;
|
||||
}
|
||||
else if (flag == MissionRun) {
|
||||
return nav_rosette_run();
|
||||
}
|
||||
return false; // not a valid case
|
||||
}
|
||||
#endif
|
||||
|
||||
// ABI message
|
||||
|
||||
#ifndef NAV_ROSETTE_LWC_ID
|
||||
#define NAV_ROSETTE_LWC_ID ABI_BROADCAST
|
||||
#endif
|
||||
|
||||
static abi_event lwc_ev;
|
||||
|
||||
static void lwc_cb(uint8_t sender_id UNUSED, uint32_t stamp UNUSED, int32_t data_type, uint32_t size, uint8_t * data)
|
||||
{
|
||||
if (data_type == 1 && size == 1) {
|
||||
nav_rosette.inside_cloud = (bool) data[0];
|
||||
}
|
||||
}
|
||||
|
||||
void nav_rosette_init(void)
|
||||
{
|
||||
nav_rosette.status = RSTT_ENTER;
|
||||
nav_rosette.radius = DEFAULT_CIRCLE_RADIUS;
|
||||
nav_rosette.recover_radius = DEFAULT_CIRCLE_RADIUS;
|
||||
nav_rosette.max_recover_radius = DEFAULT_CIRCLE_RADIUS;
|
||||
nav_rosette.inside_cloud = false;
|
||||
nav_rosette.last_border_time = 0.f;
|
||||
|
||||
AbiBindMsgPAYLOAD_DATA(NAV_ROSETTE_LWC_ID, &lwc_ev, lwc_cb);
|
||||
|
||||
#if USE_MISSION
|
||||
mission_register(nav_rosette_mission, "RSTT");
|
||||
#endif
|
||||
}
|
||||
|
||||
void nav_rosette_setup(float init_x, float init_y, float init_z,
|
||||
int turn, float desired_radius,
|
||||
float vx, float vy, float vz)
|
||||
{
|
||||
struct EnuCoor_f start = {init_x, init_y, init_z};
|
||||
// increment based on speed
|
||||
VECT3_ASSIGN(nav_rosette.pos_incr, vx*nav_dt, vy*nav_dt, vz*nav_dt);
|
||||
|
||||
nav_rosette.target = start;
|
||||
nav_rosette.status = RSTT_ENTER;
|
||||
nav_rosette.inside_cloud = false;
|
||||
nav_rosette.radius = desired_radius;
|
||||
nav_rosette.first = NULL;
|
||||
|
||||
if (turn == 1) {
|
||||
nav_rosette.rotation = RSTT_RIGHT;
|
||||
nav_rosette.radius_sign = 1.0f;
|
||||
} else {
|
||||
nav_rosette.rotation = RSTT_LEFT;
|
||||
nav_rosette.radius_sign = -1.0f;
|
||||
}
|
||||
|
||||
nav_rosette.actual = *stateGetPositionEnu_f();
|
||||
nav_rosette.last_border_time = get_sys_time_float();
|
||||
}
|
||||
|
||||
bool nav_rosette_run(void)
|
||||
{
|
||||
float pre_climb = 0.f;
|
||||
float time = get_sys_time_float();
|
||||
|
||||
NavVerticalAutoThrottleMode(0.f); /* No Pitch */
|
||||
|
||||
switch (nav_rosette.status) {
|
||||
case RSTT_ENTER:
|
||||
// init stage
|
||||
nav_init_stage();
|
||||
// reach target point
|
||||
nav_route_xy(nav_rosette.actual.x, nav_rosette.actual.y, nav_rosette.target.x, nav_rosette.target.y);
|
||||
NavVerticalAltitudeMode(nav_rosette.target.z + ground_alt, pre_climb);
|
||||
|
||||
if (nav_rosette.inside_cloud) {
|
||||
// found border or already inside
|
||||
nav_rosette.status = RSTT_CROSSING_FIRST;
|
||||
}
|
||||
break;
|
||||
case RSTT_CROSSING_FIRST:
|
||||
// prepare first crossing
|
||||
nav_rosette.actual = *stateGetPositionEnu_f();
|
||||
nav_rosette.last_border_time = time;
|
||||
nav_rosette.estim_border = nav_rosette.actual;
|
||||
nav_rosette.first = &nav_rosette.estim_border;
|
||||
// init stage
|
||||
nav_init_stage();
|
||||
// cross
|
||||
nav_rosette.status = RSTT_CROSSING;
|
||||
break;
|
||||
case RSTT_CROSSING_START:
|
||||
// prepare crossing
|
||||
nav_rosette.actual = *stateGetPositionEnu_f();
|
||||
update_barycenter(&nav_rosette.target, &nav_rosette.actual, nav_rosette.first,
|
||||
nav_rosette.target.z, time - nav_rosette.last_border_time, NAV_ROSETTE_BORDER_FILTER);
|
||||
nav_rosette.last_border_time = time;
|
||||
nav_rosette.estim_border = nav_rosette.actual;
|
||||
// init stage
|
||||
nav_init_stage();
|
||||
// cross
|
||||
nav_rosette.status = RSTT_CROSSING;
|
||||
break;
|
||||
case RSTT_CROSSING:
|
||||
// cross towards target
|
||||
pre_climb = nav_rosette.pos_incr.z / nav_dt;
|
||||
nav_route_xy(nav_rosette.actual.x, nav_rosette.actual.y, nav_rosette.target.x, nav_rosette.target.y);
|
||||
NavVerticalAltitudeMode(nav_rosette.target.z + ground_alt, pre_climb);
|
||||
|
||||
if (!nav_rosette.inside_cloud) {
|
||||
// found a border, starting turning back inside
|
||||
// note that you don't need a recover as you always expect to go out after some time
|
||||
nav_rosette.status = RSTT_TURNING_START;
|
||||
}
|
||||
break;
|
||||
case RSTT_TURNING_START:
|
||||
// update target
|
||||
nav_rosette.actual = *stateGetPositionEnu_f();
|
||||
update_barycenter(&nav_rosette.target, &nav_rosette.actual, nav_rosette.first,
|
||||
nav_rosette.target.z, time - nav_rosette.last_border_time, NAV_ROSETTE_BORDER_FILTER);
|
||||
nav_rosette.last_border_time = time;
|
||||
nav_rosette.estim_border = nav_rosette.actual;
|
||||
nav_rosette.first = NULL;
|
||||
// prepare next circle
|
||||
nav_rosette.direction = change_rep(stateGetHorizontalSpeedDir_f());
|
||||
nav_rosette.circle = process_new_point_rosette(&nav_rosette.actual, nav_rosette.direction);
|
||||
// init stage
|
||||
nav_init_stage();
|
||||
// turn
|
||||
nav_rosette.status = RSTT_TURNING;
|
||||
break;
|
||||
case RSTT_TURNING:
|
||||
// update circle center
|
||||
VECT3_ADD(nav_rosette.circle, nav_rosette.pos_incr);
|
||||
pre_climb = nav_rosette.pos_incr.z / nav_dt;
|
||||
nav_circle_XY(nav_rosette.circle.x, nav_rosette.circle.y , nav_rosette.radius_sign * nav_rosette.radius);
|
||||
NavVerticalAltitudeMode(nav_rosette.circle.z + ground_alt, pre_climb);
|
||||
|
||||
if (nav_rosette.inside_cloud) {
|
||||
// found a border, cross again
|
||||
nav_rosette.status = RSTT_CROSSING_START;
|
||||
}
|
||||
else if (NavCircleCountNoRewind() > NAV_ROSETTE_RECOVER_MAX_TURN) {
|
||||
// most likely lost outside
|
||||
nav_rosette.status = RSTT_RECOVER_START;
|
||||
}
|
||||
break;
|
||||
case RSTT_RECOVER_START:
|
||||
// prepare recovery circle
|
||||
nav_rosette.recover_circle = nav_rosette.target;
|
||||
nav_rosette.actual = *stateGetPositionEnu_f();
|
||||
// initial recovery radius
|
||||
nav_rosette.recover_radius = nav_rosette.radius;
|
||||
nav_rosette.max_recover_radius = 2.0f * nav_rosette.recover_radius; // FIXME ?
|
||||
// init stage
|
||||
nav_init_stage();
|
||||
// recover
|
||||
nav_rosette.status = RSTT_RECOVER_OUTSIDE;
|
||||
break;
|
||||
case RSTT_RECOVER_OUTSIDE:
|
||||
// increment center position
|
||||
VECT3_ADD(nav_rosette.recover_circle, nav_rosette.pos_incr);
|
||||
nav_circle_XY(nav_rosette.recover_circle.x, nav_rosette.recover_circle.y , nav_rosette.radius_sign * nav_rosette.recover_radius);
|
||||
// increment recover circle radius
|
||||
if (nav_rosette.recover_radius < nav_rosette.max_recover_radius) {
|
||||
nav_rosette.recover_radius += 0.5;
|
||||
}
|
||||
// found a new border
|
||||
if (nav_rosette.inside_cloud) {
|
||||
nav_rosette.status = RSTT_CROSSING_START;
|
||||
}
|
||||
break;
|
||||
default:
|
||||
// error, leaving
|
||||
return false;
|
||||
}
|
||||
// update target and border positions
|
||||
VECT3_ADD(nav_rosette.estim_border, nav_rosette.pos_incr);
|
||||
VECT3_ADD(nav_rosette.target, nav_rosette.pos_incr);
|
||||
#ifdef WP_TARGET
|
||||
nav_move_waypoint_enu(WP_TARGET, nav_rosette.target.x, nav_rosette.target.y, nav_rosette.target.z + ground_alt);
|
||||
RunOnceEvery(10, nav_send_waypoint(WP_TARGET));
|
||||
#endif
|
||||
|
||||
return true;
|
||||
}
|
||||
@@ -0,0 +1,55 @@
|
||||
/*
|
||||
* Copyright (C) 2018-2019 Gautier Hattenberger <gautier.hattenberger@enac.fr>
|
||||
* Titouan Verdu <titouan.verdu@enac.fr>
|
||||
*
|
||||
* This file is part of paparazzi.
|
||||
*
|
||||
* paparazzi is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 2, or (at your option)
|
||||
* any later version.
|
||||
*
|
||||
* paparazzi is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with paparazzi; see the file COPYING. If not, see
|
||||
* <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
/**
|
||||
* @file modules/nav/nav_rosette.h
|
||||
*
|
||||
* Adaptive flower pattern for cloud exploration
|
||||
* Can be used in mission mode with custom pattern and ID "RSTT"
|
||||
*
|
||||
* See:
|
||||
* Titouan Verdu, Gautier Hattenberger, Simon Lacroix. Flight patterns for clouds exploration with a fleet of UAVs. 2019 International Conference on Unmanned Aircraft Systems (ICUAS 2019), Jul 2019, Atlanta, United States.
|
||||
* https://hal-enac.archives-ouvertes.fr/hal-02137839
|
||||
*/
|
||||
|
||||
#ifndef NAV_ROSETTE_H
|
||||
#define NAV_ROSETTE_H
|
||||
|
||||
#include "std.h"
|
||||
#include "math/pprz_geodetic_float.h"
|
||||
|
||||
/** Init function called by modules init
|
||||
*/
|
||||
extern void nav_rosette_init(void);
|
||||
|
||||
/** Initialized the exploration with a first target point inside the cloud
|
||||
* Called from flight plan or with mission parameters
|
||||
*/
|
||||
extern void nav_rosette_setup(float init_x, float init_y, float init_z, int turn, float desired_radius, float vx, float vy, float vz);
|
||||
|
||||
/** Navigation function
|
||||
* Called by flight plan or mission run function
|
||||
* @return true until pattern ends or fail
|
||||
*/
|
||||
extern bool nav_rosette_run(void);
|
||||
|
||||
#endif
|
||||
|
||||
@@ -0,0 +1,249 @@
|
||||
/*
|
||||
* Copyright (C) 2019 Gautier Hattenberger <gautier.hattenberger@enac.fr>
|
||||
*
|
||||
* This file is part of paparazzi.
|
||||
*
|
||||
* paparazzi is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 2, or (at your option)
|
||||
* any later version.
|
||||
*
|
||||
* paparazzi is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with paparazzi; see the file COPYING. If not, see
|
||||
* <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
/**
|
||||
* @file modules/nav/nav_spiral_3D.c
|
||||
*
|
||||
* Fixedwing navigation in a 3D spiral.
|
||||
*
|
||||
*/
|
||||
|
||||
#include "modules/nav/nav_spiral_3D.h"
|
||||
|
||||
#include "firmwares/fixedwing/nav.h"
|
||||
#include "state.h"
|
||||
#include "autopilot.h"
|
||||
#include "generated/flight_plan.h"
|
||||
|
||||
#ifndef NAV_SPIRAL_3D_DIST_DIFF
|
||||
#define NAV_SPIRAL_3D_DIST_DIFF 10.f // horizontal distance difference before starting
|
||||
#endif
|
||||
|
||||
#ifndef NAV_SPIRAL_3D_ALT_DIFF
|
||||
#define NAV_SPIRAL_3D_ALT_DIFF 10.f // vertical distance difference before starting
|
||||
#endif
|
||||
|
||||
#ifndef NAV_SPIRAL_3D_MIN_CIRCLE_RADIUS
|
||||
#define NAV_SPIRAL_3D_MIN_CIRCLE_RADIUS 50.f
|
||||
#endif
|
||||
|
||||
enum Spiral3DStatus { Spiral3DStart, Spiral3DCircle, Spiral3DFail };
|
||||
|
||||
struct NavSpiral3D {
|
||||
struct FloatVect3 center;
|
||||
struct FloatVect3 pos_incr;
|
||||
float alt_start;
|
||||
float alt_stop;
|
||||
float radius;
|
||||
float radius_min;
|
||||
float radius_start;
|
||||
float radius_stop;
|
||||
float radius_increment;
|
||||
enum Spiral3DStatus status;
|
||||
};
|
||||
|
||||
struct NavSpiral3D nav_spiral_3D;
|
||||
|
||||
static const float nav_dt = 1.f / NAVIGATION_FREQUENCY;
|
||||
|
||||
#if USE_MISSION
|
||||
#include "modules/mission/mission_common.h"
|
||||
|
||||
static bool nav_spiral_3D_mission(uint8_t nb, float *params, enum MissionRunFlag flag)
|
||||
{
|
||||
if (flag == MissionInit && nb == 9) {
|
||||
float cx = params[0];
|
||||
float cy = params[1];
|
||||
float alt_start = params[2];
|
||||
float alt_stop = params[3];
|
||||
float r_start = params[4];
|
||||
float r_stop = params[5];
|
||||
float vx = params[6];
|
||||
float vy = params[7];
|
||||
float vz = params[8];
|
||||
nav_spiral_3D_setup(cx, cy, alt_start, alt_stop, r_start, r_stop, vx, vy, vz);
|
||||
return true;
|
||||
}
|
||||
else if (flag == MissionUpdate && nb == 4) {
|
||||
// update current position and horizontal speed
|
||||
float cx = params[0];
|
||||
float cy = params[1];
|
||||
float vx = params[2];
|
||||
float vy = params[3];
|
||||
nav_spiral_3D.center.x = cx;
|
||||
nav_spiral_3D.center.y = cy;
|
||||
nav_spiral_3D.pos_incr.x = vx * nav_dt;
|
||||
nav_spiral_3D.pos_incr.y = vy * nav_dt;
|
||||
return true;
|
||||
}
|
||||
else if (flag == MissionUpdate && nb == 2) {
|
||||
// update horizontal speed
|
||||
float vx = params[0];
|
||||
float vy = params[1];
|
||||
nav_spiral_3D.pos_incr.x = vx * nav_dt;
|
||||
nav_spiral_3D.pos_incr.y = vy * nav_dt;
|
||||
return true;
|
||||
}
|
||||
else if (flag == MissionUpdate && nb == 1) {
|
||||
// update vertical speed
|
||||
float vz = params[0];
|
||||
nav_spiral_3D.pos_incr.z = vz * nav_dt;
|
||||
return true;
|
||||
}
|
||||
else if (flag == MissionRun) {
|
||||
return nav_spiral_3D_run();
|
||||
}
|
||||
return false; // not a valid case
|
||||
}
|
||||
#endif
|
||||
|
||||
void nav_spiral_3D_init(void)
|
||||
{
|
||||
// no speed by default
|
||||
FLOAT_VECT3_ZERO(nav_spiral_3D.pos_incr);
|
||||
|
||||
#if USE_MISSION
|
||||
mission_register(nav_spiral_3D_mission, "SPIR3");
|
||||
#endif
|
||||
}
|
||||
|
||||
void nav_spiral_3D_setup(float center_x, float center_y,
|
||||
float alt_start, float alt_stop,
|
||||
float radius_start, float radius_stop,
|
||||
float vx, float vy, float vz)
|
||||
{
|
||||
// initial center position
|
||||
VECT3_ASSIGN(nav_spiral_3D.center, center_x, center_y, alt_start);
|
||||
nav_spiral_3D.alt_start = alt_start;
|
||||
nav_spiral_3D.alt_stop = alt_stop;
|
||||
float deltaZ = alt_stop - alt_start;
|
||||
if (deltaZ * vz < 0.f &&
|
||||
fabsf(deltaZ) > 1.f &&
|
||||
fabsf(radius_start - radius_stop) > 0.1f) {
|
||||
// error vz and delta Z don't have the same sign
|
||||
// and we are not doing a circle at constant alt
|
||||
nav_spiral_3D.status = Spiral3DFail;
|
||||
return;
|
||||
}
|
||||
// increment based on speed
|
||||
VECT3_ASSIGN(nav_spiral_3D.pos_incr, vx*nav_dt, vy*nav_dt, vz*nav_dt);
|
||||
// radius
|
||||
nav_spiral_3D.radius_min = NAV_SPIRAL_3D_MIN_CIRCLE_RADIUS;
|
||||
nav_spiral_3D.radius_start = radius_start;
|
||||
if (nav_spiral_3D.radius_start < nav_spiral_3D.radius_min) {
|
||||
nav_spiral_3D.radius_start = nav_spiral_3D.radius_min;
|
||||
}
|
||||
nav_spiral_3D.radius = nav_spiral_3D.radius_start; // start radius
|
||||
nav_spiral_3D.radius_stop = radius_stop;
|
||||
if (nav_spiral_3D.radius_stop < nav_spiral_3D.radius_min) {
|
||||
nav_spiral_3D.radius_stop = nav_spiral_3D.radius_min;
|
||||
}
|
||||
// Compute radius increment
|
||||
float deltaR = radius_stop - radius_start;
|
||||
if (fabsf(deltaR) < 1.f) {
|
||||
// radius diff is too small we are doing a circle of constant radius
|
||||
nav_spiral_3D.radius_increment = 0.f;
|
||||
} else if (fabsf(deltaZ) < 1.f && fabsf(vz) > 0.1f) {
|
||||
// alt diff is too small, use Vz as increment rate at fix altitude
|
||||
// Rinc = deltaR / deltaT
|
||||
// deltaT = deltaR / Vz
|
||||
// Rinc = Vz
|
||||
float sign = deltaR < 0.f ? -1.f : 1.0;
|
||||
nav_spiral_3D.pos_incr.z = 0.f;
|
||||
nav_spiral_3D.radius_increment = sign * fabsf(vz) * nav_dt;
|
||||
} else if (fabsf(vz) < 0.1f) {
|
||||
// vz is too small, fail
|
||||
nav_spiral_3D.status = Spiral3DFail;
|
||||
return;
|
||||
} else {
|
||||
// normal case, vz and alt diff are large enough and in the same direction
|
||||
// Rinc = deltaR / deltaT
|
||||
// deltaT = deltaZ / Vz
|
||||
// Rinc = deltaR * Vz / deltaZ;
|
||||
nav_spiral_3D.radius_increment = deltaR * vz * nav_dt / deltaZ;
|
||||
}
|
||||
nav_spiral_3D.status = Spiral3DStart;
|
||||
}
|
||||
|
||||
bool nav_spiral_3D_run(void)
|
||||
{
|
||||
struct EnuCoor_f pos_enu = *stateGetPositionEnu_f();
|
||||
|
||||
struct FloatVect2 pos_diff;
|
||||
VECT2_DIFF(pos_diff, pos_enu, nav_spiral_3D.center);
|
||||
float dist_to_center = float_vect2_norm(&pos_diff);
|
||||
float dist_diff, alt_diff;
|
||||
float pre_climb = 0.f;
|
||||
|
||||
switch (nav_spiral_3D.status) {
|
||||
case Spiral3DFail:
|
||||
// error during setup
|
||||
return false;
|
||||
|
||||
case Spiral3DStart:
|
||||
// fly to start circle until dist to center and alt are acceptable
|
||||
// flys until center of the helix is reached and start helix
|
||||
VECT2_DIFF(pos_diff, pos_enu, nav_spiral_3D.center);
|
||||
dist_diff = fabs(dist_to_center - nav_spiral_3D.radius_start);
|
||||
alt_diff = fabs(stateGetPositionUtm_f()->alt - nav_spiral_3D.alt_start);
|
||||
if (dist_diff < NAV_SPIRAL_3D_DIST_DIFF && alt_diff < NAV_SPIRAL_3D_ALT_DIFF) {
|
||||
nav_spiral_3D.status = Spiral3DCircle;
|
||||
}
|
||||
break;
|
||||
|
||||
case Spiral3DCircle:
|
||||
// increment center position
|
||||
VECT3_ADD(nav_spiral_3D.center, nav_spiral_3D.pos_incr);
|
||||
if (fabsf(nav_spiral_3D.radius_increment) > 0.001f) {
|
||||
// increment radius
|
||||
nav_spiral_3D.radius += nav_spiral_3D.radius_increment;
|
||||
// test end condition
|
||||
dist_diff = fabs(dist_to_center - nav_spiral_3D.radius_stop);
|
||||
alt_diff = fabs(stateGetPositionUtm_f()->alt - nav_spiral_3D.alt_stop);
|
||||
if (dist_diff < NAV_SPIRAL_3D_DIST_DIFF && alt_diff < NAV_SPIRAL_3D_ALT_DIFF) {
|
||||
// reaching desired altitude and radius
|
||||
return false; // spiral is finished
|
||||
}
|
||||
} else {
|
||||
// we are doing a circle of constant radius
|
||||
if (fabsf(nav_spiral_3D.pos_incr.z) > 0.1f) {
|
||||
// alt should change, check for arrival
|
||||
alt_diff = fabs(stateGetPositionUtm_f()->alt - nav_spiral_3D.alt_stop);
|
||||
if (alt_diff < NAV_SPIRAL_3D_ALT_DIFF) {
|
||||
// reaching desired altitude
|
||||
return false; // spiral is finished
|
||||
}
|
||||
}
|
||||
}
|
||||
pre_climb = nav_spiral_3D.pos_incr.z / nav_dt;
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
// horizontal control setting
|
||||
nav_circle_XY(nav_spiral_3D.center.x, nav_spiral_3D.center.y, nav_spiral_3D.radius);
|
||||
// vertical control setting
|
||||
NavVerticalAutoThrottleMode(0.); /* No pitch */
|
||||
NavVerticalAltitudeMode(nav_spiral_3D.center.z, pre_climb); /* No preclimb */
|
||||
|
||||
return true;
|
||||
}
|
||||
@@ -0,0 +1,52 @@
|
||||
/*
|
||||
* Copyright (C) 2019 Gautier Hattenberger <gautier.hattenberger@enac.fr>
|
||||
*
|
||||
* This file is part of paparazzi.
|
||||
*
|
||||
* paparazzi is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 2, or (at your option)
|
||||
* any later version.
|
||||
*
|
||||
* paparazzi is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with paparazzi; see the file COPYING. If not, see
|
||||
* <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
/**
|
||||
* @file modules/nav/nav_spiral_3D.h
|
||||
*
|
||||
* Fixedwing navigation in a 3D spiral.
|
||||
*
|
||||
*/
|
||||
|
||||
#ifndef NAV_SPIRAL_3D_H
|
||||
#define NAV_SPIRAL_3D_H
|
||||
|
||||
#include "std.h"
|
||||
#include "math/pprz_algebra_float.h"
|
||||
|
||||
extern void nav_spiral_3D_init(void);
|
||||
|
||||
/** Run spiral 3D navigation
|
||||
*/
|
||||
extern bool nav_spiral_3D_run(void);
|
||||
|
||||
/** Initialize spiral 3D based on:
|
||||
* - position X, Y
|
||||
* - start and stop altitude
|
||||
* - start and stop radius
|
||||
* - speeds (horizontal and vertical)
|
||||
*/
|
||||
extern void nav_spiral_3D_setup(float center_x, float center_y,
|
||||
float alt_start, float alt_stop,
|
||||
float radius_start, float radius_stop,
|
||||
float vx, float vy, float vz);
|
||||
|
||||
#endif // NAV_SPIRAL_3D_H
|
||||
|
||||
@@ -0,0 +1,359 @@
|
||||
/*
|
||||
* Copyright (C) 2018-2021 Gautier Hattenberger <gautier.hattenberger@enac.fr>
|
||||
* Titouan Verdu <titouan.verdu@enac.fr>
|
||||
*
|
||||
* This file is part of paparazzi.
|
||||
*
|
||||
* paparazzi is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 2, or (at your option)
|
||||
* any later version.
|
||||
*
|
||||
* paparazzi is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with paparazzi; see the file COPYING. If not, see
|
||||
* <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
/**
|
||||
* @file "modules/nav/nav_trinity.c"
|
||||
* @author Titouan Verdu
|
||||
* Adaptative trinity pattern for cloud exploration.
|
||||
*/
|
||||
|
||||
#include "modules/nav/nav_trinity.h"
|
||||
|
||||
#include "firmwares/fixedwing/nav.h"
|
||||
#include "state.h"
|
||||
#include "autopilot.h"
|
||||
#include "generated/flight_plan.h"
|
||||
#include "modules/core/abi.h"
|
||||
|
||||
#ifndef NAV_TRINITY_RECOVER_MAX_TURN
|
||||
#define NAV_TRINITY_RECOVER_MAX_TURN 1.5f
|
||||
#endif
|
||||
|
||||
#ifndef NAV_TRINITY_BORDER_FILTER
|
||||
#define NAV_TRINITY_BORDER_FILTER 20.f
|
||||
#endif
|
||||
|
||||
enum TrinityStatus {
|
||||
TRINITY_ENTER,
|
||||
TRINITY_INSIDE_FIRST,
|
||||
TRINITY_INSIDE,
|
||||
TRINITY_OUTSIDE_START,
|
||||
TRINITY_OUTSIDE,
|
||||
TRINITY_RECOVER_START,
|
||||
TRINITY_RECOVER_INSIDE,
|
||||
TRINITY_RECOVER_OUTSIDE
|
||||
};
|
||||
|
||||
enum RotationDir {
|
||||
TRINITY_LEFT,
|
||||
TRINITY_RIGHT
|
||||
};
|
||||
|
||||
struct NavTrinity {
|
||||
enum TrinityStatus status;
|
||||
enum RotationDir rotation;
|
||||
bool inside_cloud;
|
||||
struct EnuCoor_f actual;
|
||||
struct EnuCoor_f target;
|
||||
struct EnuCoor_f circle;
|
||||
struct EnuCoor_f estim_border;
|
||||
struct EnuCoor_f recover_circle;
|
||||
struct FloatVect3 pos_incr;
|
||||
float direction;
|
||||
float radius;
|
||||
float radius_sign;
|
||||
float last_border_time;
|
||||
float recover_radius;
|
||||
float max_recover_radius;
|
||||
};
|
||||
|
||||
static struct NavTrinity nav_trinity;
|
||||
|
||||
static const float nav_dt = 1.f / NAVIGATION_FREQUENCY;
|
||||
|
||||
static float change_rep(float dir)
|
||||
{
|
||||
return M_PI_2 - dir;
|
||||
}
|
||||
|
||||
static struct EnuCoor_f process_new_point_trinity(struct EnuCoor_f *position, float alt_sp, float uav_direction)
|
||||
{
|
||||
struct EnuCoor_f new_point;
|
||||
float rot_angle;
|
||||
|
||||
if (nav_trinity.rotation == TRINITY_RIGHT) {
|
||||
rot_angle = -M_PI_2;
|
||||
} else{
|
||||
rot_angle = M_PI_2;
|
||||
}
|
||||
|
||||
new_point.x = position->x + (cosf(rot_angle + uav_direction) * nav_trinity.radius);
|
||||
new_point.y = position->y + (sinf(rot_angle + uav_direction) * nav_trinity.radius);
|
||||
new_point.z = alt_sp;
|
||||
|
||||
return new_point;
|
||||
}
|
||||
|
||||
static struct EnuCoor_f process_first_point_trinity(struct EnuCoor_f *position, float alt_sp, float uav_direction)
|
||||
{
|
||||
if (nav_trinity.rotation == TRINITY_RIGHT) {
|
||||
nav_trinity.rotation = TRINITY_LEFT;
|
||||
} else{
|
||||
nav_trinity.rotation = TRINITY_RIGHT;
|
||||
}
|
||||
|
||||
return process_new_point_trinity(position, alt_sp, uav_direction);
|
||||
}
|
||||
|
||||
static void update_target_point(struct EnuCoor_f *target, struct EnuCoor_f *border, float dt, float tau)
|
||||
{
|
||||
// with a proper discrete transform, coeff should be exp(-dt/tau) and (1-exp(-dt/tau))
|
||||
// but to make it simpler with the same behavior at the limits, we use tau/(dt+tau) and dt/(dt+tau)
|
||||
// with a positive value for tau
|
||||
if (tau > 1e-4) {
|
||||
float alpha = dt / (dt + tau);
|
||||
target->x = (border->x * alpha) + (target->x * (1.f - alpha));
|
||||
target->y = (border->y * alpha) + (target->y * (1.f - alpha));
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
#if USE_MISSION
|
||||
#include "modules/mission/mission_common.h"
|
||||
|
||||
static bool nav_trinity_mission(uint8_t nb, float *params, enum MissionRunFlag flag)
|
||||
{
|
||||
if (flag == MissionInit && nb == 8) {
|
||||
float start_x = params[0];
|
||||
float start_y = params[1];
|
||||
float start_z = params[2];
|
||||
int first_turn = params[3];
|
||||
float circle_radius = params[4];
|
||||
float vx = params[5];
|
||||
float vy = params[6];
|
||||
float vz = params[7];
|
||||
nav_trinity_setup(start_x, start_y, start_z, first_turn, circle_radius, vx, vy, vz);
|
||||
return true;
|
||||
}
|
||||
else if (flag == MissionUpdate && nb == 3) {
|
||||
// update target 3D position (ENU frame, above ground alt)
|
||||
float x = params[0];
|
||||
float y = params[1];
|
||||
float z = params[2];
|
||||
VECT3_ASSIGN(nav_trinity.target, x, y, z);
|
||||
return true;
|
||||
}
|
||||
else if (flag == MissionUpdate && nb == 2) {
|
||||
// update horizontal speed
|
||||
float vx = params[0];
|
||||
float vy = params[1];
|
||||
nav_trinity.pos_incr.x = vx * nav_dt;
|
||||
nav_trinity.pos_incr.y = vy * nav_dt;
|
||||
return true;
|
||||
}
|
||||
else if (flag == MissionUpdate && nb == 1) {
|
||||
// update vertical speed
|
||||
float vz = params[0];
|
||||
nav_trinity.pos_incr.z = vz * nav_dt;
|
||||
return true;
|
||||
}
|
||||
else if (flag == MissionRun) {
|
||||
return nav_trinity_run();
|
||||
}
|
||||
return false; // not a valid case
|
||||
}
|
||||
#endif
|
||||
|
||||
// ABI message
|
||||
|
||||
#ifndef NAV_TRINITY_LWC_ID
|
||||
#define NAV_TRINITY_LWC_ID ABI_BROADCAST
|
||||
#endif
|
||||
|
||||
static abi_event lwc_ev;
|
||||
|
||||
static void lwc_cb(uint8_t sender_id UNUSED, uint32_t stamp UNUSED, int32_t data_type, uint32_t size, uint8_t * data) {
|
||||
if (data_type == 1 && size == 1) {
|
||||
nav_trinity.inside_cloud = (bool) data[0];
|
||||
}
|
||||
}
|
||||
|
||||
void nav_trinity_init(void)
|
||||
{
|
||||
nav_trinity.status = TRINITY_ENTER;
|
||||
nav_trinity.radius = DEFAULT_CIRCLE_RADIUS;
|
||||
nav_trinity.recover_radius = DEFAULT_CIRCLE_RADIUS;
|
||||
nav_trinity.max_recover_radius = DEFAULT_CIRCLE_RADIUS;
|
||||
nav_trinity.last_border_time = 0.f;
|
||||
nav_trinity.inside_cloud = false;
|
||||
|
||||
AbiBindMsgPAYLOAD_DATA(NAV_TRINITY_LWC_ID, &lwc_ev, lwc_cb);
|
||||
|
||||
#if USE_MISSION
|
||||
mission_register(nav_trinity_mission, "TRNTY");
|
||||
#endif
|
||||
}
|
||||
|
||||
void nav_trinity_setup(float init_x, float init_y,
|
||||
float init_z, int turn,
|
||||
float desired_radius, float vx,
|
||||
float vy, float vz)
|
||||
{
|
||||
struct EnuCoor_f start = {init_x, init_y, init_z};
|
||||
// increment based on speed
|
||||
VECT3_ASSIGN(nav_trinity.pos_incr, vx*nav_dt, vy*nav_dt, vz*nav_dt);
|
||||
|
||||
nav_trinity.target = start;
|
||||
nav_trinity.status = TRINITY_ENTER;
|
||||
nav_trinity.inside_cloud = false;
|
||||
nav_trinity.radius = desired_radius;
|
||||
|
||||
if (turn == 1) {
|
||||
nav_trinity.rotation = TRINITY_RIGHT;
|
||||
nav_trinity.radius_sign = 1.0f;
|
||||
} else {
|
||||
nav_trinity.rotation = TRINITY_LEFT;
|
||||
nav_trinity.radius_sign = -1.0f;
|
||||
}
|
||||
|
||||
nav_trinity.actual = *stateGetPositionEnu_f();
|
||||
}
|
||||
|
||||
bool nav_trinity_run(void)
|
||||
{
|
||||
float pre_climb = 0.f;
|
||||
float time = get_sys_time_float();
|
||||
|
||||
NavVerticalAutoThrottleMode(0.f); /* No pitch */
|
||||
|
||||
switch (nav_trinity.status) {
|
||||
case TRINITY_ENTER:
|
||||
// init stage
|
||||
nav_init_stage();
|
||||
// reach target point
|
||||
nav_route_xy(nav_trinity.actual.x, nav_trinity.actual.y, nav_trinity.target.x, nav_trinity.target.y);
|
||||
NavVerticalAltitudeMode(nav_trinity.target.z + ground_alt, pre_climb);
|
||||
|
||||
if (nav_trinity.inside_cloud) {
|
||||
nav_trinity.status = TRINITY_INSIDE_FIRST;
|
||||
}
|
||||
break;
|
||||
case TRINITY_INSIDE_FIRST:
|
||||
// prepare inside circle
|
||||
nav_trinity.actual = *stateGetPositionEnu_f();
|
||||
nav_trinity.direction = change_rep(stateGetHorizontalSpeedDir_f());
|
||||
nav_trinity.circle = process_first_point_trinity(&nav_trinity.actual, nav_trinity.target.z, nav_trinity.direction);
|
||||
// init stage
|
||||
nav_init_stage();
|
||||
// update border and target for recover
|
||||
nav_trinity.estim_border = nav_trinity.actual;
|
||||
update_target_point(&nav_trinity.target, &nav_trinity.estim_border, time - nav_trinity.last_border_time, NAV_TRINITY_BORDER_FILTER);
|
||||
nav_trinity.last_border_time = time;
|
||||
// fly inside
|
||||
nav_trinity.status = TRINITY_INSIDE;
|
||||
break;
|
||||
case TRINITY_INSIDE:
|
||||
// increment center position
|
||||
VECT3_ADD(nav_trinity.circle, nav_trinity.pos_incr);
|
||||
nav_circle_XY(nav_trinity.circle.x, nav_trinity.circle.y , nav_trinity.radius_sign * nav_trinity.radius);
|
||||
pre_climb = nav_trinity.pos_incr.z / nav_dt;
|
||||
NavVerticalAltitudeMode(nav_trinity.circle.z + ground_alt, pre_climb);
|
||||
|
||||
if (!nav_trinity.inside_cloud) {
|
||||
// found border, start outside
|
||||
nav_trinity.status = TRINITY_OUTSIDE_START;
|
||||
}
|
||||
else if (NavCircleCountNoRewind() > NAV_TRINITY_RECOVER_MAX_TURN) {
|
||||
// most likely lost inside
|
||||
nav_trinity.status = TRINITY_RECOVER_START;
|
||||
}
|
||||
break;
|
||||
case TRINITY_OUTSIDE_START:
|
||||
// prepare outside circle
|
||||
nav_trinity.actual = *stateGetPositionEnu_f();
|
||||
nav_trinity.direction = change_rep(stateGetHorizontalSpeedDir_f());
|
||||
nav_trinity.circle = process_new_point_trinity(&nav_trinity.actual, nav_trinity.target.z, nav_trinity.direction);
|
||||
// init stage
|
||||
nav_init_stage();
|
||||
// upadte border and target for recover
|
||||
nav_trinity.estim_border = nav_trinity.actual;
|
||||
update_target_point(&nav_trinity.target, &nav_trinity.estim_border, time - nav_trinity.last_border_time, NAV_TRINITY_BORDER_FILTER);
|
||||
nav_trinity.last_border_time = time;
|
||||
// fly outside
|
||||
nav_trinity.status = TRINITY_OUTSIDE;
|
||||
break;
|
||||
case TRINITY_OUTSIDE:
|
||||
// increment center position
|
||||
VECT3_ADD(nav_trinity.circle, nav_trinity.pos_incr);
|
||||
pre_climb = nav_trinity.pos_incr.z / nav_dt;
|
||||
nav_circle_XY(nav_trinity.circle.x, nav_trinity.circle.y , nav_trinity.radius_sign * nav_trinity.radius);
|
||||
NavVerticalAltitudeMode(nav_trinity.circle.z + ground_alt, pre_climb);
|
||||
|
||||
if(nav_trinity.inside_cloud){
|
||||
// found border, continue inside
|
||||
nav_trinity.status = TRINITY_INSIDE;
|
||||
}
|
||||
else if (NavCircleCountNoRewind() > NAV_TRINITY_RECOVER_MAX_TURN) {
|
||||
// most likely lost outside
|
||||
nav_trinity.status = TRINITY_RECOVER_START;
|
||||
}
|
||||
break;
|
||||
case TRINITY_RECOVER_START:
|
||||
// prepare recovery circle or line
|
||||
nav_trinity.recover_circle = nav_trinity.target;
|
||||
nav_trinity.actual = *stateGetPositionEnu_f();
|
||||
// initial recovery radius
|
||||
nav_trinity.recover_radius = nav_trinity.radius;
|
||||
nav_trinity.max_recover_radius = 2.0f * nav_trinity.recover_radius; // FIXME ?
|
||||
// init stage
|
||||
nav_init_stage();
|
||||
if (nav_trinity.inside_cloud) {
|
||||
nav_trinity.status = TRINITY_RECOVER_INSIDE;
|
||||
}
|
||||
else {
|
||||
nav_trinity.status = TRINITY_RECOVER_OUTSIDE;
|
||||
}
|
||||
break;
|
||||
case TRINITY_RECOVER_INSIDE:
|
||||
// increment border position
|
||||
// fly in opposite direction to cover more space
|
||||
nav_route_xy(nav_trinity.estim_border.x, nav_trinity.estim_border.y, nav_trinity.actual.x, nav_trinity.actual.y);
|
||||
if (!nav_trinity.inside_cloud) {
|
||||
nav_trinity.status = TRINITY_OUTSIDE_START;
|
||||
}
|
||||
break;
|
||||
case TRINITY_RECOVER_OUTSIDE:
|
||||
// increment center position
|
||||
VECT3_ADD(nav_trinity.recover_circle, nav_trinity.pos_incr);
|
||||
nav_circle_XY(nav_trinity.recover_circle.x, nav_trinity.recover_circle.y , nav_trinity.radius_sign * nav_trinity.recover_radius);
|
||||
// increment recover circle radius
|
||||
if (nav_trinity.recover_radius < nav_trinity.max_recover_radius) {
|
||||
nav_trinity.recover_radius += 0.5;
|
||||
}
|
||||
// found a new border
|
||||
if (nav_trinity.inside_cloud) {
|
||||
nav_trinity.status = TRINITY_INSIDE;
|
||||
}
|
||||
break;
|
||||
default:
|
||||
// error, leaving
|
||||
return false;
|
||||
}
|
||||
// increment border and target positions
|
||||
VECT3_ADD(nav_trinity.estim_border, nav_trinity.pos_incr);
|
||||
VECT3_ADD(nav_trinity.target, nav_trinity.pos_incr);
|
||||
#ifdef WP_TARGET
|
||||
nav_move_waypoint_enu(WP_TARGET, nav_trinity.target.x, nav_trinity.target.y, nav_trinity.target.z + ground_alt);
|
||||
RunOnceEvery(10, nav_send_waypoint(WP_TARGET));
|
||||
#endif
|
||||
|
||||
return true;
|
||||
}
|
||||
@@ -0,0 +1,48 @@
|
||||
/*
|
||||
* Copyright (C) Titouan Verdu
|
||||
*
|
||||
* This file is part of paparazzi
|
||||
*
|
||||
* paparazzi is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 2, or (at your option)
|
||||
* any later version.
|
||||
*
|
||||
* paparazzi is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with paparazzi; see the file COPYING. If not, see
|
||||
* <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
/**
|
||||
* @file "modules/nav/nav_trinity.h"
|
||||
* @author Titouan Verdu
|
||||
* Adaptative trinity pattern for cloud exploration.
|
||||
*/
|
||||
|
||||
#ifndef NAV_TRINITY_H
|
||||
#define NAV_TRINITY_H
|
||||
|
||||
#include "std.h"
|
||||
#include "math/pprz_geodetic_float.h"
|
||||
|
||||
/** Init function called by modules init
|
||||
*/
|
||||
extern void nav_trinity_init(void);
|
||||
|
||||
/** Initialized the exploration with a first target point inside the cloud
|
||||
* Called from flight plan or with mission parameters
|
||||
*/
|
||||
extern void nav_trinity_setup(float init_x, float init_y, float init_z, int turn, float desired_radius, float vx, float vy, float vz);
|
||||
|
||||
/** Navigation function
|
||||
* Called by flight plan or mission run function
|
||||
* @return true until pattern ends or fail
|
||||
*/
|
||||
extern bool nav_trinity_run(void);
|
||||
|
||||
#endif
|
||||
|
||||
+1
-1
Submodule sw/ext/pprzlink updated: e2e248d518...cbf3682922
@@ -485,6 +485,26 @@ let log_and_parse = fun ac_name (a:Aircraft_server.aircraft) msg values ->
|
||||
a.ap_mode <- check_index (ivalue "ap_mode") (modes_of_aircraft a) "AP_MODE";
|
||||
a.cur_block <- ivalue "nav_block";
|
||||
end
|
||||
| "MINIMAL_COM" ->
|
||||
let lat = fvalue "lat"
|
||||
and lon = fvalue "lon" in
|
||||
let geo = make_geo_deg lat lon in
|
||||
a.pos <- geo;
|
||||
a.alt <- fvalue "hmsl";
|
||||
a.gspeed <- fvalue "gspeed";
|
||||
a.course <- norm_course (fvalue "course");
|
||||
if !heading_from_course || a.vehicle_type == FixedWing then
|
||||
a.heading <- a.course;
|
||||
a.climb <- fvalue "climb";
|
||||
a.agl <- a.alt -. (try float (Srtm.of_wgs84 a.pos) with _ -> a.ground_alt);
|
||||
a.bat <- fvalue "vsupply";
|
||||
a.throttle <- fvalue "throttle";
|
||||
a.ap_mode <- check_index (ivalue "ap_mode") (modes_of_aircraft a) "AP_MODE";
|
||||
a.cur_block <- ivalue "nav_block";
|
||||
a.gps_mode <- check_index (ivalue "gps_mode") gps_modes "GPS_MODE";
|
||||
a.flight_time <- ivalue "flight_time";
|
||||
if a.gspeed > 3. && a.ap_mode = _AUTO2 then
|
||||
Wind.update ac_name a.gspeed a.course
|
||||
| "FORMATION_SLOT_TM" ->
|
||||
Dl_Pprz.message_send "ground_dl" "FORMATION_SLOT" values
|
||||
| "FORMATION_STATUS_TM" ->
|
||||
|
||||
@@ -551,7 +551,21 @@ let rec select_gps_values = function
|
||||
l := (t, wgs84, a) :: !l
|
||||
done;
|
||||
List.rev !l
|
||||
|
||||
| (m, values)::_ when m.PprzLink.name = "MINIMAL_COM" ->
|
||||
let lats = List.assoc "lat" values
|
||||
and lons = List.assoc "lon" values
|
||||
and alts = List.assoc "hmsl" values in
|
||||
let l = ref [] in
|
||||
for i = 0 to Array.length lats - 1 do
|
||||
let a = snd alts.(i) in
|
||||
if a > 0. then
|
||||
let t = fst lats.(i)
|
||||
and lat = snd lats.(i)
|
||||
and lon = snd lons.(i) in
|
||||
let wgs84 = make_geo_deg lat lon in
|
||||
l := (t, wgs84, a) :: !l
|
||||
done;
|
||||
List.rev !l
|
||||
|
||||
| _ :: rest ->
|
||||
select_gps_values rest
|
||||
|
||||
Reference in New Issue
Block a user