304 changes: 304 additions & 0 deletions conf/flight_plans/competitions/IMAV2023.xml
View file
@@ -0,0 +1,304 @@
<?xml version="1.0"?>
<!DOCTYPE flight_plan SYSTEM "../flight_plan.dtd">
<flight_plan alt="148" ground_alt="118" lat0="50.9097" lon0="6.22823" max_dist_from_home="610" name="IMAV2023" security_height="10">

<header>
#define FP_NONE 0
#define FP_ENDURANCE 1
#define FP_MAPPING 2
#define FP_DYNAMIC 3
#define FP_HICKERS 4

#ifdef NAV_C
#ifndef TAG_TRACKING_COORD_TO_M
#define TAG_TRACKING_COORD_TO_M (1.f / 1000.f)
#ifdef SITL
#define jevois_stream(_x) {}
#endif
#endif

#if DIGITAL_CAM
#define LINE_START_FUNCTION dc_Survey(fp_shot_distance);
#define LINE_STOP_FUNCTION dc_autoshoot = DC_AUTOSHOOT_STOP;
#endif

static void fp_tag_cb(uint8_t sender_id UNUSED,
uint8_t type, char * id UNUSED,
uint8_t nb UNUSED, int16_t * coord, uint16_t * dim UNUSED,
struct FloatQuat quat UNUSED, char * extra UNUSED)
{
if (type == JEVOIS_MSG_D3) {
tag_distance = coord[2] * TAG_TRACKING_COORD_TO_M;
tag_valid = true;
}
}

static inline bool delay_test_rc(bool test, int delay) {
static int nb = 0;
if (test) {
nb++;
if (nb == delay) {
nb = 0;
return true;
}
return false;
} else {
nb = 0;
return false;
}
}

#endif
</header>
<waypoints>
<waypoint lat="50.9099" lon="6.22682" name="HOME"/>
<waypoint lat="50.9097" lon="6.22773" name="STDBY"/>
<waypoint lat="50.9097" lon="6.22823" name="LANDPAD"/>
<waypoint lat="50.908377" lon="6.222250" name="TAG"/>
<waypoint lat="50.909228" lon="6.226700" name="HICKERS"/>
<waypoint lat="50.9110" lon="6.22635" name="S1"/>
<waypoint lat="50.9071" lon="6.22589" name="S2"/>
<waypoint lat="50.9072" lon="6.22426" name="S3"/>
<waypoint lat="50.9111" lon="6.22465" name="S4"/>
<waypoint lat="50.910595" lon="6.227356" name="START_DYN"/>
<waypoint lat="50.9113" lon="6.22717" name="D1"/>
<waypoint lat="50.9120" lon="6.22760" name="D2"/>
<waypoint lat="50.9120" lon="6.22883" name="D3"/>
<waypoint lat="50.9116" lon="6.22870" name="D4"/>
<waypoint lat="50.9103" lon="6.22656" name="ENDURANCE"/>

<waypoint lat="50.906886" lon="6.223927" name="_FLY1"/>
<waypoint lat="50.910946" lon="6.223798" name="_FLY2"/>
<waypoint lat="50.913247" lon="6.226908" name="_FLY3"/>
<waypoint lat="50.912701" lon="6.229972" name="_FLY4"/>
<waypoint lat="50.912425" lon="6.230648" name="_FLY5"/>
<waypoint lat="50.911652" lon="6.231214" name="_FLY6"/>
<waypoint lat="50.911392" lon="6.229977" name="_FLY7"/>
<waypoint lat="50.910842" lon="6.229131" name="_FLY8"/>
<waypoint lat="50.909282" lon="6.228386" name="_FLY9"/>
<waypoint lat="50.908253" lon="6.226671" name="_FLY10"/>
<waypoint lat="50.907502" lon="6.226436" name="_FLY11"/>
<waypoint lat="50.907083" lon="6.226495" name="_FLY12"/>
<waypoint lat="50.906788" lon="6.226718" name="_FLY13"/>
<waypoint lat="50.906549" lon="6.226418" name="_FLY14"/>

<waypoint lat="50.906376" lon="6.223196" name="_KILL1"/>
<waypoint lat="50.910998" lon="6.223086" name="_KILL2"/>
<waypoint lat="50.913740" lon="6.226769" name="_KILL3"/>
<waypoint lat="50.913135" lon="6.230278" name="_KILL4"/>
<waypoint lat="50.912821" lon="6.231021" name="_KILL5"/>
<waypoint lat="50.912073" lon="6.231793" name="_KILL6"/>
<waypoint lat="50.911361" lon="6.231674" name="_KILL7"/>
<waypoint lat="50.911074" lon="6.230432" name="_KILL8"/>
<waypoint lat="50.910664" lon="6.229780" name="_KILL9"/>
<waypoint lat="50.907942" lon="6.227214" name="_KILL10"/>
<waypoint lat="50.907432" lon="6.227156" name="_KILL11"/>
<waypoint lat="50.906610" lon="6.227501" name="_KILL12"/>
<waypoint lat="50.905917" lon="6.226675" name="_KILL13"/>
<waypoint lat="50.906223" lon="6.225644" name="_KILL14"/>

<waypoint lat="50.9109" lon="6.22871" name="_HERE"/>
</waypoints>
<sectors>
<sector color="green" name="Survey">
<corner name="S1"/>
<corner name="S2"/>
<corner name="S3"/>
<corner name="S4"/>
</sector>
<sector color="orange" name="Flight_Area">
<corner name="_FLY1"/>
<corner name="_FLY2"/>
<corner name="_FLY3"/>
<corner name="_FLY4"/>
<corner name="_FLY5"/>
<corner name="_FLY6"/>
<corner name="_FLY7"/>
<corner name="_FLY8"/>
<corner name="_FLY9"/>
<corner name="_FLY10"/>
<corner name="_FLY11"/>
<corner name="_FLY12"/>
<corner name="_FLY13"/>
<corner name="_FLY14"/>
</sector>
<sector color="red" name="Kill">
<corner name="_KILL1"/>
<corner name="_KILL2"/>
<corner name="_KILL3"/>
<corner name="_KILL4"/>
<corner name="_KILL5"/>
<corner name="_KILL6"/>
<corner name="_KILL7"/>
<corner name="_KILL8"/>
<corner name="_KILL9"/>
<corner name="_KILL10"/>
<corner name="_KILL11"/>
<corner name="_KILL12"/>
<corner name="_KILL13"/>
<corner name="_KILL14"/>
</sector>
</sectors>
<variables>
<variable init="0" type="int" var="mission_nb"/>
<variable init="20." var="goto_height" min="0.5" max="15." step="0.1"/>
<variable init="7." var="goto_speed" min="0.5" max="10." step="0.1"/>
<variable init="5." var="takeoff_height" min="0.5" max="15." step="0.1"/>
<variable init="4." var="land_speed" min="0.5" max="10." step="0.1"/>
<variable init="40." var="mapping_height" min="0.5" max="15." step="0.1"/>
<variable init="40." var="mapping_sweep" min="5." max="20." step="0.1"/>
<variable init="7." var="mapping_speed" min="0.5" max="10." step="0.1"/>
<variable init="-1." var="mapping_radius" min="-1." max="100." step="1.0"/>
<variable init="10." var="endurance_speed" min="0.5" max="20." step="0.1"/>
<variable init="100." var="endurance_radius" min="20" max="150." step="1.0"/>
<variable init="500" type="int" var="endurance_time" min="1" max="1200" step="1"/>
<variable init="30." var="hickers_radius" min="-60." max="60." step="1.0"/>
<variable init="42." var="tag_distance"/>
<variable init="false" type="bool" var="tag_valid"/>
<abi_binding name="JEVOIS_MSG" handler="fp_tag_cb"/>
</variables>
<modules>
<module name="nav" type="takeoff_and_landing"/>
<module name="nav" type="survey_hybrid">
<define name="SURVEY_HYBRID_MAX_SWEEP_BACK" value="1"/>
<define name="SURVEY_HYBRID_APPROACHING_TIME" value="0"/>
<define name="SURVEY_HYBRID_ENTRY_DISTANCE" value="survey_private.sweep_distance"/>
</module>
<module name="tag_tracking">
<define name="TAG_TRACKING_WP" value="WP_LANDPAD"/>
<define name="TAG_TRACKING_SIM_WP" value="WP_TAG"/>
</module>
<module name="dynamic_inspection_imav2023"/>
</modules>
<exceptions>
<exception cond="(!InsideFlight_Area(GetPosX(), GetPosY()) @OR GetPosAlt() @GT GetAltRef() + 100) @AND (nav_block @GT IndexOfBlock('Standby')) @AND (nav_block @LT IndexOfBlock('Kill landed'))" deroute="Standby"/>
<exception cond="(!InsideKill(GetPosX(), GetPosY()) @OR GetPosAlt() @GT GetAltRef() + 120) @AND (nav_block == IndexOfBlock('Standby'))" deroute="Kill landed"/>
<exception cond="(nav_block @GT IndexOfBlock('Standby') @AND nav_block @LT IndexOfBlock('Kill landed')) @AND (GpsIsLost() @AND delay_test_rc(RadioControlIsLost(),20) @AND (datalink_time @GT 20))" deroute="Kill landed"/>
<exception cond="(nav_block @GT IndexOfBlock('Standby') @AND nav_block @LT IndexOfBlock('Kill landed')) @AND (delay_test_rc(RadioControlIsLost(),20) @AND (datalink_time @GT 15))" deroute="Land here"/>
</exceptions>
<blocks>
<block name="Wait GPS">
<call_once fun="NavKillThrottle()"/>
<call_once fun="jevois_stream(false)"/>
<while cond="!GpsFixValid()"/>
</block>
<block name="Geo init">
<while cond="LessThan(NavBlockTime(), 10)"/>
</block>
<block name="Holding point" strip_button="H. Point" group="home">
<call_once fun="NavKillThrottle()"/>
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
</block>
<block name="Start Engine">
<call_once fun="NavResurrect()"/>
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
</block>
<block name="Takeoff" strip_button="Takeoff" strip_icon="takeoff.png" group="home">
<exception cond="GetPosHeight() @GT takeoff_height @AND mission_nb == FP_NONE" deroute="Standby"/>
<exception cond="GetPosHeight() @GT takeoff_height @AND mission_nb == FP_ENDURANCE" deroute="Run Endurance"/>
<exception cond="GetPosHeight() @GT takeoff_height @AND mission_nb == FP_MAPPING" deroute="Goto Mapping"/>
<exception cond="GetPosHeight() @GT takeoff_height @AND mission_nb == FP_DYNAMIC" deroute="Goto Dynamic"/>
<exception cond="GetPosHeight() @GT takeoff_height @AND mission_nb == FP_HICKERS" deroute="Goto Hickers"/>
<call_once fun="ins_reset_vertical_pos()"/>
<call_once fun="NavResurrect()"/>
<attitude pitch="0" roll="0" throttle="0.1" until="stage_time @GT 2" vmode="throttle"/>
<call_once fun="NavSetWaypointHere(WP_LANDPAD)"/>
<stay climb="nav.climb_vspeed" vmode="climb" wp="LANDPAD"/>
</block>
<block name="Standby" strip_button="Standby" strip_icon="home.png" group="home">
<set var="mission_nb" value="FP_NONE"/>
<call_once fun="guidance_h_SetMaxSpeed(land_speed)"/>
<stay wp="STDBY"/>
</block>

<block group="mission" name="Start Endurance" strip_button="Endurance">
<set var="mission_nb" value="FP_ENDURANCE"/>
<deroute block="Takeoff"/>
</block>
<block group="mission" name="Start Mapping" strip_button="Mapping">
<set var="mission_nb" value="FP_MAPPING"/>
<deroute block="Takeoff"/>
</block>
<block group="mission" name="Start Dynamic" strip_button="Dynamic">
<set var="mission_nb" value="FP_DYNAMIC"/>
<deroute block="Takeoff"/>
</block>
<block group="mission" name="Start Area" strip_button="Hickers">
<set var="mission_nb" value="FP_HICKERS"/>
<deroute block="Takeoff"/>
</block>

<block name="Run Endurance">
<!-- exception cond="electrical.bat_low" deroute="Landing"/-->
<set var="mission_nb" value="FP_NONE"/>
<call_once fun="jevois_stream(false)"/>
<call_once fun="guidance_h_SetMaxSpeed(endurance_speed)"/>
<circle wp="ENDURANCE" radius="endurance_radius" until="stage_time @GT endurance_time"/>
<deroute block="Landing"/>
</block>

<block name="Goto Mapping">
<set var="mission_nb" value="FP_NONE"/>
<call_once fun="NavSetWaypointHere(WP__HERE)"/>
<call_once fun="jevois_stream(false)"/>
<call_once fun="guidance_h_SetMaxSpeed(mapping_speed)"/>
<stay wp="_HERE" until="stage_time @GT 3" height="mapping_height"/>
<go wp="S1" from="_HERE" hmode="route" height="mapping_height"/>
<call_once fun="nav_survey_hybrid_setup_towards(WP_S1, WP_S2, 4, mapping_sweep, mapping_radius, mapping_height)"/>
<deroute block="Run Mapping"/>
</block>
<block name="Run Mapping">
<call_once fun="guidance_h_SetMaxSpeed(mapping_speed)"/>
<call fun="nav_survey_hybrid_run()"/>
<deroute block="Landing"/>
</block>

<block name="Goto Dynamic">
<set var="mission_nb" value="FP_NONE"/>
<call_once fun="NavSetWaypointHere(WP__HERE)"/>
<call_once fun="jevois_stream(false)"/>
<call_once fun="guidance_h_SetMaxSpeed(endurance_speed)"/>
<stay wp="_HERE" until="stage_time @GT 3" height="goto_height"/>
<go from="_HERE" hmode="route" wp="START_DYN" height="goto_height"/>
<call_once fun="dynamic_inspection_setup(WP_START_DYN, 4, endurance_speed)"/>
<deroute block="Run Dynamic"/>
</block>
<block name="Run Dynamic">
<!--path wpts="START_DYN,D1,D2,D3,D4,START_DYN"/-->
<call fun="dynamic_inspection_run()"/>
<deroute block="Landing"/>
</block>

<block name="Goto Hickers">
<set var="mission_nb" value="FP_NONE"/>
<call_once fun="NavSetWaypointHere(WP__HERE)"/>
<call_once fun="jevois_stream(false)"/>
<call_once fun="guidance_h_SetMaxSpeed(goto_speed)"/>
<circle wp="HICKERS" radius="hickers_radius" height="goto_height" until="NavCircleCount() @GT 1.0" pre_call="nav_set_heading_towards_waypoint(WP_HICKERS)"/>
<deroute block="Landing"/>
</block>

<block name="Land here" strip_button="Land here" strip_icon="land-right.png" group="land">
<call_once fun="NavSetWaypointHere(WP_LANDPAD)"/>
</block>
<block name="Landing" strip_button="Land on pad" group="land">
<call_once fun="jevois_stream(false)"/>
<call_once fun="NavSetWaypointHere(WP__HERE)"/>
<call_once fun="guidance_h_SetMaxSpeed(goto_speed)"/>
<go wp="LANDPAD" from="_HERE" hmode="route" height="goto_height"/>
</block>
<block name="Flare">
<exception cond="!nav_is_in_flight()" deroute="Kill landed"/>
<call_once fun="guidance_h_SetMaxSpeed(land_speed)"/>
<call_once fun="jevois_stream(true)"/>
<stay climb="2*nav.descend_vspeed" vmode="climb" wp="LANDPAD" until="GetPosHeight() @LT 10"/>
<stay climb="nav.descend_vspeed" vmode="climb" wp="LANDPAD"/>
</block>
<block name="Kill landed">
<call_once fun="NavKillThrottle()"/>
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
</block>

</blocks>

</flight_plan>
122 changes: 122 additions & 0 deletions conf/flight_plans/test_falcon_Muret.xml
View file
@@ -0,0 +1,122 @@
<?xml version="1.0"?>
<!DOCTYPE flight_plan SYSTEM "flight_plan.dtd">
<flight_plan alt="215" ground_alt="185" lat0="43.46223" lon0="1.27289" max_dist_from_home="400" name="Muret test wind quad" security_height="1" wp_frame="ltp">
<header>
#if DIGITAL_CAM
#define LINE_START_FUNCTION dc_Survey(20);
#define LINE_STOP_FUNCTION dc_autoshoot = DC_AUTOSHOOT_STOP;
#endif
</header>
<waypoints>
<waypoint name="HOME" x="-8.1" y="9.8"/>
<waypoint name="CLIMB" x="-28.0" y="29.0"/>
<waypoint name="STDBY" x="40.209" y="42.714"/>
<waypoint name="p1" x="67.492" y="32.136"/>
<waypoint name="p2" x="-35.356" y="78.947"/>
<waypoint name="p3" x="-16.718" y="133.25"/>
<waypoint name="p4" x="90.03" y="73.004"/>
<waypoint name="TD" x="31.809" y="46.501"/>
<waypoint name="_FLY1" x="-120.135" y="264.94"/>
<waypoint name="_FLY2" x="299.403" y="176.814"/>
<waypoint name="_FLY3" x="236.684" y="-88.655"/>
<waypoint name="_FLY4" x="-210.99" y="29.788"/>
<waypoint name="_KILL1" x="-130.009" y="292.981"/>
<waypoint name="_KILL2" x="324.67" y="196.132"/>
<waypoint name="_KILL3" x="256.662" y="-119.374"/>
<waypoint name="_KILL4" x="-251.128" y="10.603"/>
</waypoints>
<sectors>
<sector color="lime" name="Flight_Area">
<corner name="_FLY1"/>
<corner name="_FLY2"/>
<corner name="_FLY3"/>
<corner name="_FLY4"/>
</sector>
<sector color="red" name="Kill">
<corner name="_KILL1"/>
<corner name="_KILL2"/>
<corner name="_KILL3"/>
<corner name="_KILL4"/>
</sector>
</sectors>
<modules>
<module name="nav" type="takeoff_and_landing"/>
</modules>
<exceptions>
<!--exception cond="Or(!InsideFlight_Area(GetPosX(), GetPosY()), GetPosAlt() &gt; GetAltRef() + 50) @AND (nav_block &gt;= IndexOfBlock('Takeoff')) @AND !(nav_block == IndexOfBlock('Kill landed'))" deroute="Standby"/-->
</exceptions>
<blocks>
<block name="Wait GPS">
<call_once fun="NavKillThrottle()"/>
<while cond="!GpsFixValid()"/>
</block>
<block name="Geo init">
<while cond="LessThan(NavBlockTime(), 10)"/>
<call_once fun="NavSetAltitudeReferenceHere()"/>
</block>
<block name="Holding point">
<call_once fun="NavKillThrottle()"/>
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
</block>
<block name="Start Engine">
<call_once fun="NavResurrect()"/>
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
</block>
<block name="Takeoff" strip_button="Takeoff" strip_icon="takeoff.png">
<exception cond="GetPosHeight() @GT 10.0" deroute="Standby"/>
<call_once fun="NavSetWaypointHere(WP_CLIMB)"/>
<call_once fun="NavResurrect()"/>
<attitude pitch="0" roll="0" throttle="0" until="stage_time @GT 1" vmode="throttle"/>
<stay climb="nav.climb_vspeed" vmode="climb" wp="CLIMB"/>
</block>
<block name="Takeoff (module)" strip_button="Takeoff (module)" group="module">
<call fun="nav_takeoff_from_here()"/>
<deroute block="Standby"/>
</block>
<!--block name="Standby" pre_call="if(!InsideKill(GetPosX(), GetPosY())) NavKillThrottle();" strip_button="Standby" strip_icon="home.png"-->
<block name="Standby" strip_button="Standby" strip_icon="home.png">
<stay wp="STDBY"/>
</block>
<block name="stay_p1">
<stay wp="p1"/>
</block>
<block name="line_p1_p2">
<stay wp="p1" until="stage_time @GT 5"/>
<go wp="p2" from="p1" hmode="route"/>
<stay wp="p2"/>
</block>
<block name="square">
<go wp="p1"/>
<path wpts="p1,p2,p3,p4,p1"/>
<deroute block="stay_p1"/>
</block>
<block name="circle">
<circle radius="nav.radius" wp="STDBY"/>
</block>
<block name="oval">
<oval p1="p2" p2="p1" radius="nav.radius"/>
</block>
<block name="Carto Oval" strip_button="Carto Oval">
<oval p1="p2" p2="p1" radius="nav.radius" until="NavOvalCount @GEQ 2"/>
<oval p1="p3" p2="p4" radius="nav.radius" until="NavOvalCount @GEQ 2"/>
<deroute block="Standby"/>
</block>
<block name="Land (module)" strip_button="Land (module)" group="module">
<call fun="nav_land_at_wp(WP_TD, WP_TD, 0.)"/>
<deroute block="Kill landed"/>
</block>
<block name="land here" strip_button="Land Here" strip_icon="land-right.png">
<call_once fun="NavSetWaypointHere(WP_TD)"/>
</block>
<block name="land">
<go wp="TD"/>
</block>
<block name="flare">
<exception cond="!nav_is_in_flight()" deroute="Kill landed"/>
<stay climb="nav.descend_vspeed" vmode="climb" wp="TD"/>
</block>
<block name="Kill landed">
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
</block>
</blocks>
</flight_plan>
16 changes: 16 additions & 0 deletions conf/mag/germany_aachen.xml
View file
@@ -0,0 +1,16 @@
<!DOCTYPE airframe SYSTEM "../airframes/airframe.dtd">

<airframe>
<description>
Germany / Aachen
50.9097° N, 6.22823° W
Date Declination (+ E|-W) Inclination (+D|-U) Horizontal Intensity North Comp (+N|-S) East Comp (+E|-W) Vertical Comp (+D|-U) Total Field
2023-08-17 -1.3339° 65.7508° 19,995.7 nT 19,990.2 nT -465.5 nT 44,390.4 nT 48,686.0 nT
</description>
<section name="MAG_MODEL" prefix="INS_">
<define name="H_X" value="0.410594"/>
<define name="H_Y" value="-0.00956127"/>
<define name="H_Z" value="0.911769"/>
</section>
</airframe>

2 changes: 1 addition & 1 deletion conf/modules/digital_cam.xml
View file
Expand Up @@ -36,7 +36,7 @@
<strip_button name="ZoomOut" icon="zoom.png" value="119" group="dczoom"/>
</dl_setting>

<dl_setting max="3" min="0" step="1" var="dc_autoshoot" values="STOP|PERIODIC|DISTANCE|EXT_TRIG">
<dl_setting max="3" min="0" step="1" var="dc_autoshoot" values="STOP|PERIODIC|DISTANCE|EXT_TRIG|SURVEY|CIRCLE">
<strip_button name="Start Autoshoot" icon="dcstart.png" value="1" group="dcauto"/>
<strip_button name="Stop Autoshoot" icon="dcstart.png" value="0" group="dcauto"/>
</dl_setting>
Expand Down
19 changes: 19 additions & 0 deletions conf/modules/dynamic_inspection_imav2023.xml
View file
@@ -0,0 +1,19 @@
<!DOCTYPE module SYSTEM "module.dtd">
<module name="dynamic_inspection_imav2023" dir="nav" task="control">
<doc>
<description>
Compute and fly sequence of waypoints to maximize the score at IMAV2023, task 3
</description>
</doc>
<dep>
<depends>math</depends>
</dep>
<header>
<file name="dynamic_inspection_imav2023.h"/>
</header>
<init fun="dynamic_inspection_init()"/>
<makefile>
<file name="dynamic_inspection_imav2023.c"/>
<test firmware="rotorcraft"/>
</makefile>
</module>
18 changes: 18 additions & 0 deletions conf/modules/eff_scheduling_falcon.xml
View file
@@ -0,0 +1,18 @@
<!DOCTYPE module SYSTEM "module.dtd">
<module name="eff_scheduling_falcon" dir="ctrl">
<doc>
<description>
Interpolation of control effectivenss matrix of the Falcon hybride plane
</description>
</doc>

<header>
<file name="eff_scheduling_falcon.h"/>
</header>
<init fun="ctrl_eff_scheduling_init()"/>
<periodic fun="ctrl_eff_scheduling_periodic()" freq="20.0"/>
<periodic fun="ctrl_eff_scheduling_report()" freq="0.5" autorun="FALSE"/> <!-- TRUE FALSE-->
<makefile target="ap|nps" firmware="rotorcraft" >
<file name="eff_scheduling_falcon.c"/>
</makefile>
</module>
7 changes: 7 additions & 0 deletions conf/modules/guidance_indi_hybrid.xml
View file
Expand Up @@ -17,8 +17,10 @@
<dl_setting var="gih_params.speed_gain" min="0" step="0.1" max="10.0" shortname="kd" param="GUIDANCE_INDI_SPEED_GAIN" persistent="true"/>
<dl_setting var="gih_params.speed_gainz" min="0" step="0.1" max="10.0" shortname="kd_z" param="GUIDANCE_INDI_SPEED_GAINZ" persistent="true"/>
<dl_setting var="gih_params.heading_bank_gain" min="0" step="0.1" max="30.0" shortname="bank gain" param="GUIDANCE_INDI_HEADING_BANK_GAIN" persistent="true"/>
<dl_setting var="gih_params.pitch_offset_gain" min="0." step="0.001" max="0.1" shortname="pitch gain" param="GUIDANCE_INDI_PITCH_OFFSET_GAIN" persistent="true"/>
<dl_setting var="guidance_indi_max_airspeed" min="12.0" step="1.0" max="30.0" shortname="cruise_airspeed"/>
<dl_setting var="guidance_indi_max_bank" min="10.0" step="1.0" max="50.0" shortname="max_bank"/>
<dl_setting var="guidance_indi_min_pitch" min="-130.0" step="1.0" max="-30.0" shortname="min_pitch"/>
<dl_setting var="take_heading_control" min="0" step="1" max="1" values="OFF|ON" shortname="take_heading"/>
</dl_settings>
</dl_settings>
Expand All @@ -35,6 +37,11 @@
<!--<periodic fun="guidance_indi_propagate_filters()" freq="PERIODIC_FREQUENCY" autorun="TRUE"/>-->
<makefile target="ap|nps" firmware="rotorcraft">
<file name="guidance_indi_hybrid.c" dir="$(SRC_FIRMWARE)/guidance"/>
<file name="wls_alloc.c" dir="$(SRC_FIRMWARE)/guidance/wls"/>
<file name="qr_solve.c" dir="math/qr_solve"/>
<file name="r8lib_min.c" dir="math/qr_solve"/>
<define name="CA_N_V_GUIDANCE" value="3"/>
<define name="CA_N_U_GUIDANCE" value="3"/>
<define name="GUIDANCE_INDI_HYBRID" value="TRUE"/>
<define name="GUIDANCE_PID_USE_AS_DEFAULT" value="FALSE"/>
<define name="GUIDANCE_INDI_HYBRID_USE_AS_DEFAULT" value="TRUE"/>
Expand Down
3 changes: 2 additions & 1 deletion conf/modules/nav_hybrid.xml
View file
Expand Up @@ -9,7 +9,8 @@
<settings>
<dl_settings>
<dl_settings NAME="nav_hybrid">
<dl_setting var="nav_max_speed" min="1.0" step="1.0" max="50.0" shortname="nav_max_speed"/>
<dl_setting var="nav_max_speed" min="1.0" step="1.0" max="50.0"/>
<dl_setting var="nav_hover_speed" min="0.1" step="0.1" max="10.0"/>
</dl_settings>
</dl_settings>
</settings>
Expand Down
109 changes: 109 additions & 0 deletions conf/simulator/jsbsim/aircraft/Systems/aerodynamics_falcon.xml
View file
@@ -0,0 +1,109 @@
<aerodynamics>

<axis name="YAW">
<function name="elevon_yaw" frame="BODY" unit="FT*LBS">
<sum>
<product>
<property>fcs/ele_left_lag</property>
<value> -0.35 </value>
</product>
<product>
<property>fcs/ele_right_lag</property>
<value> -.35 </value>
</product>
</sum>
</function>
</axis>

<axis name="PITCH">
<function name="elevon_pitch" frame="BODY" unit="FT*LBS">
<sum>
<product>
<property>fcs/ele_left_lag</property>
<value> -0.0417 </value>
</product>
<product>
<property>fcs/ele_right_lag</property>
<value> .0417 </value>
</product>
</sum>
</function>
</axis>

<axis name="LIFT">

<function name="aero/force/Lift_alpha">
<description>Lift due to alpha</description>
<product>
<property>aero/qbar-psf</property>
<property>metrics/Sw-sqft</property>
<table>
<independentVar lookup="row">aero/alpha-deg</independentVar>
<tableData>
-180 0.14
-140 -0.80
-110 -0.63
-102 -0.74
-98 -0.64
-90 0.0
-85 0.15
-80 0.25
-75 0.42
0 0.0
45 0.8
90 0.0
135 -0.8
180 0.0
</tableData>
</table>
</product>
</function>

</axis>

<axis name="DRAG">

<function name="aero/force/Drag_basic">
<description>Drag</description>
<product>
<property>aero/qbar-psf</property>
<property>metrics/Sw-sqft</property>
<table>
<independentVar lookup="row">aero/alpha-deg</independentVar>
<tableData>
-180 1.7
-160 1.62
-100 0.4
-90 0.15
-85 0.17
-75 0.22
-70 0.33
-65 0.6128
-20 1.36
-4 1.45
15 1.35
30 1.12
</tableData>
</table>
<value>2.0</value> <!-- FIXME why this factor 2 ? -->
</product>
</function>

</axis>

<axis name="SIDE">

<function name="aero/force/Side_beta">
<description>Side force due to beta</description>
<product>
<property>aero/qbar-psf</property>
<property>metrics/Sw-sqft</property>
<property>aero/beta-rad</property>
<!--value>-4</value-->
<value>0.02</value>
</product>
</function>

</axis>

</aerodynamics>
442 changes: 442 additions & 0 deletions conf/simulator/jsbsim/aircraft/anton.xml
View file

Large diffs are not rendered by default.

394 changes: 394 additions & 0 deletions conf/simulator/jsbsim/aircraft/falcon.xml
View file
@@ -0,0 +1,394 @@
<?xml version="1.0"?>
<?xml-stylesheet type="text/xsl" href="http://jsbsim.sourceforge.net/JSBSim.xsl"?>
<fdm_config name="QUAD COMPLETE EXT" version="2.0" release="BETA" xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xsi:noNamespaceSchemaLocation="http://jsbsim.sourceforge.net/JSBSim.xsd">

<fileheader>
<author>ENAC</author>
<filecreationdate>09-06-2022</filecreationdate>
<version>Version 0.92 - beta</version>
<description>Falcon</description>
</fileheader>

<metrics>
<wingarea unit="M2"> 0.075 </wingarea>
<wingspan unit="M"> 0.9 </wingspan>
<chord unit="M"> 0.12 </chord>
<htailarea unit="M2"> 0 </htailarea>
<htailarm unit="M"> 0 </htailarm>
<vtailarea unit="M2"> 0 </vtailarea>
<vtailarm unit="M"> 0 </vtailarm>
<wing_incidence unit="DEG"> 90 </wing_incidence>
<location name="AERORP" unit="M">
<x> 0 </x>
<y> 0 </y>
<z> 0 </z>
</location>
<location name="EYEPOINT" unit="M">
<x> 0 </x>
<y> 0 </y>
<z> 0 </z>
</location>
<location name="VRP" unit="M">
<x> 0 </x>
<y> 0 </y>
<z> 0 </z>
</location>
</metrics>

<mass_balance>
<ixx unit="KG*M2"> 0.005 </ixx>
<iyy unit="KG*M2"> 0.005 </iyy>
<izz unit="KG*M2"> 0.015 </izz>
<ixy unit="KG*M2"> 0. </ixy>
<ixz unit="KG*M2"> 0. </ixz>
<iyz unit="KG*M2"> 0. </iyz>
<emptywt unit="KG"> 0.72 </emptywt>
<location name="CG" unit="M">
<x> 0 </x>
<y> 0 </y>
<z> 0 </z>
</location>
</mass_balance>

<ground_reactions>
<contact type="STRUCTURE" name="CONTACT_FRONT">
<location unit="M">
<x> -0.12 </x>
<y> 0.0 </y>
<z>-0.1 </z>
</location>
<static_friction> 0.8 </static_friction>
<dynamic_friction> 0.5 </dynamic_friction>
<spring_coeff unit="N/M"> 500 </spring_coeff>
<damping_coeff unit="N/M/SEC"> 100 </damping_coeff>
<damping_coeff_rebound type="SQUARE" unit="N/M2/SEC2"> 1000 </damping_coeff_rebound>
<max_steer unit="DEG"> 0.0 </max_steer>
<brake_group> NONE </brake_group>
<retractable>0</retractable>
</contact>

<contact type="STRUCTURE" name="CONTACT_BACK">
<location unit="M">
<x> 0.12</x>
<y> 0.0</y>
<z> -0.1</z>
</location>
<static_friction> 0.8 </static_friction>
<dynamic_friction> 0.5 </dynamic_friction>
<spring_coeff unit="N/M"> 500 </spring_coeff>
<damping_coeff unit="N/M/SEC"> 100 </damping_coeff>
<damping_coeff_rebound type="SQUARE" unit="N/M2/SEC2"> 1000 </damping_coeff_rebound>
<max_steer unit="DEG"> 0.0 </max_steer>
<brake_group> NONE </brake_group>
<retractable>0</retractable>
</contact>

<contact type="STRUCTURE" name="CONTACT_RIGHT">
<location unit="M">
<x> 0.0 </x>
<y> 0.12</y>
<z> -0.1 </z>
</location>
<static_friction> 0.8 </static_friction>
<dynamic_friction> 0.5 </dynamic_friction>
<spring_coeff unit="N/M"> 500 </spring_coeff>
<damping_coeff unit="N/M/SEC"> 100 </damping_coeff>
<damping_coeff_rebound type="SQUARE" unit="N/M2/SEC2"> 1000 </damping_coeff_rebound>
<max_steer unit="DEG"> 0.0 </max_steer>
<brake_group> NONE </brake_group>
<retractable>0</retractable>
</contact>

<contact type="STRUCTURE" name="CONTACT_LEFT">
<location unit="M">
<x> 0.0 </x>
<y>-0.12</y>
<z> -0.1 </z>
</location>
<static_friction> 0.8 </static_friction>
<dynamic_friction> 0.5 </dynamic_friction>
<spring_coeff unit="N/M"> 500 </spring_coeff>
<damping_coeff unit="N/M/SEC"> 100 </damping_coeff>
<damping_coeff_rebound type="SQUARE" unit="N/M2/SEC2"> 1000 </damping_coeff_rebound>
<max_steer unit="DEG"> 0.0 </max_steer>
<brake_group> NONE </brake_group>
<retractable>0</retractable>
</contact>
</ground_reactions>

<flight_control name="actuator_dynamics">
<property value="30.">fcs/motor_lag</property>
<property value="54.">fcs/elevon_lag</property>

<channel name="filtering">

<!--First order filter represents actuator dynamics-->
<lag_filter name="UR_lag">
<input> fcs/UR </input>
<clipto>
<min>0</min>
<max>1</max>
</clipto>
<c1> fcs/motor_lag </c1>
<output> fcs/UR_lag</output>
</lag_filter>
<lag_filter name="BR_lag">
<input> fcs/BR </input>
<clipto>
<min>0</min>
<max>1</max>
</clipto>
<c1> fcs/motor_lag </c1>
<output> fcs/BR_lag</output>
</lag_filter>
<lag_filter name="BL_lag">
<input> fcs/BL </input>
<clipto>
<min>0</min>
<max>1</max>
</clipto>
<c1> fcs/motor_lag </c1>
<output> fcs/BL_lag</output>
</lag_filter>
<lag_filter name="UL_lag">
<input> fcs/UL </input>
<clipto>
<min>0</min>
<max>1</max>
</clipto>
<c1> fcs/motor_lag </c1>
<output> fcs/UL_lag</output>
</lag_filter>

<lag_filter name="ele_left_lag">
<input> fcs/ele_left </input>
<c1> fcs/elevon_lag </c1>
<output> fcs/ele_left_lag</output>
</lag_filter>
<lag_filter name="ele_right_lag">
<input> fcs/ele_right </input>
<c1> fcs/elevon_lag </c1>
<output> fcs/ele_right_lag</output>
</lag_filter>
</channel>
</flight_control>

<external_reactions>

<property>fcs/UR</property>
<property>fcs/BR</property>
<property>fcs/BL</property>
<property>fcs/UL</property>
<property>fcs/UR_lag</property>
<property>fcs/BR_lag</property>
<property>fcs/BL_lag</property>
<property>fcs/UL_lag</property>
<property>fcs/ele_left</property>
<property>fcs/ele_left_lag</property>
<property>fcs/ele_right</property>
<property>fcs/ele_right_lag</property>

<property value="6.">fcs/motor_max_thrust</property> <!-- FIXME should be 4. from flight data -->
<property value="0.00001">fcs/motor_max_torque</property>

<!-- First the lift forces produced by each propeller -->

<force name="UR" frame="BODY" unit="LBS">
<function>
<product>
<property>fcs/UR_lag</property>
<property>fcs/motor_max_thrust</property>
<value>0.224808943</value> <!-- N to LBS -->
<table>
<independentVar lookup="row">aero/qbar-psf</independentVar>
<tableData> <!-- 1 psf = 47.88 Pa , 18m/s : 200Pa, 5m/s :15Pa 3m/s:5.5Pa-->
0. 1.
0.1148 1.
4.17 0.7 <!-- 18m/s -->
</tableData>
</table>
</product>
</function>
<location unit="M">
<x>0.105</x>
<y>0.12</y>
<z>0</z>
</location>
<direction>
<x>0.342</x>
<y>0</y>
<z>-0.94</z>
</direction>
</force>
<force name="BR" frame="BODY" unit="LBS">
<function>
<product>
<property>fcs/BR_lag</property>
<property>fcs/motor_max_thrust</property>
<value>0.224808943</value> <!-- N to LBS -->
<table>
<independentVar lookup="row">aero/qbar-psf</independentVar>
<tableData> <!-- 1 psf = 47.88 Pa , 18m/s : 200Pa, 5m/s :15Pa 3m/s:5.5Pa-->
0. 1.
0.1148 1.
4.17 0.7 <!-- 18m/s -->
</tableData>
</table>
</product>
</function>
<location unit="M">
<x>-0.105</x>
<y>0.12</y>
<z>0</z>
</location>
<direction>
<x>-0.342</x>
<y>0</y>
<z>-0.94</z>
</direction>
</force>
<force name="BL" frame="BODY" unit="LBS">
<function>
<product>
<property>fcs/BL_lag</property>
<property>fcs/motor_max_thrust</property>
<value>0.224808943</value> <!-- N to LBS -->
<table>
<independentVar lookup="row">aero/qbar-psf</independentVar>
<tableData> <!-- 1 psf = 47.88 Pa , 18m/s : 200Pa, 5m/s :15Pa 3m/s:5.5Pa-->
0. 1.
0.1148 1.
4.17 0.7 <!-- 18m/s -->
</tableData>
</table>
</product>
</function>
<location unit="M">
<x>-0.105</x>
<y>-0.12</y>
<z>0</z>
</location>
<direction>
<x>-0.342</x>
<y>0</y>
<z>-0.94</z>
</direction>
</force>
<force name="UL" frame="BODY" unit="LBS">
<function>
<product>
<property>fcs/UL_lag</property>
<property>fcs/motor_max_thrust</property>
<value>0.224808943</value> <!-- N to LBS -->
<table>
<independentVar lookup="row">aero/qbar-psf</independentVar>
<tableData> <!-- 1 psf = 47.88 Pa , 18m/s : 200Pa, 5m/s :15Pa 3m/s:5.5Pa-->
0. 1.
0.1148 1.
4.17 0.7 <!-- 18m/s -->
</tableData>
</table>
</product>
</function>
<location unit="M">
<x>0.105</x>
<y>-0.12</y>
<z>0</z>
</location>
<direction>
<x>0.342</x>
<y>0</y>
<z>-0.94</z>
</direction>
</force>

<!-- Then the Moment Couples -->

<moment name="UR_couple" frame="BODY">
<function>
<product>
<property>fcs/UR_lag</property>
<property>fcs/motor_max_torque</property>
<value> 0.738 </value> <!-- N.m to FT.LBS -->
</product>
</function>
<location unit="M">
<x>0.105</x>
<y>0.12</y>
<z>0</z>
</location>
<direction>
<x>0</x>
<y>0</y>
<z>-1</z>
</direction>
</moment>

<moment name="BR_couple" frame="BODY">
<function>
<product>
<property>fcs/BR_lag</property>
<property>fcs/motor_max_torque</property>
<value> 0.738 </value> <!-- N.m to FT.LBS -->
</product>
</function>
<location unit="M">
<x>-0.105</x>
<y>0.12</y>
<z>0</z>
</location>
<direction>
<x>0</x>
<y>0</y>
<z>1</z>
</direction>
</moment>

<moment name="BL_couple" frame="BODY">
<function>
<product>
<property>fcs/BL_lag</property>
<property>fcs/motor_max_torque</property>
<value> 0.738 </value> <!-- N.m to FT.LBS -->
</product>
</function>
<location unit="M">
<x>-0.105</x>
<y>-0.12</y>
<z>0</z>
</location>
<direction>
<x>0</x>
<y>0</y>
<z>-1</z>
</direction>
</moment>

<moment name="UL_couple" frame="BODY">
<function>
<product>
<property>fcs/UL_lag</property>
<property>fcs/motor_max_torque</property>
<value> 0.738 </value> <!-- N.m to FT.LBS -->
</product>
</function>
<location unit="M">
<x>0.105</x>
<y>-0.12</y>
<z>0</z>
</location>
<direction>
<x>0</x>
<y>0</y>
<z>1</z>
</direction>
</moment>

</external_reactions>

<propulsion/>

<flight_control name="FGFCS"/>
<aerodynamics file="Systems/aerodynamics_falcon.xml"/>

</fdm_config>
220 changes: 220 additions & 0 deletions conf/simulator/pybullet/tailsitter_falcon_2.urdf
View file
@@ -0,0 +1,220 @@
<?xml version="1.0" ?>

<robot name="FalconTailSitterV2">

<material name="white">
<color rgba="1. 1. 1. 1.0"/>
</material>

<material name="black">
<color rgba="0. 0. 0. 1.0"/>
</material>

<material name="blue0">
<color rgba="15./255. 141./255. 238./255. 1.0"/>
</material>

<material name="darkgrey">
<color rgba="0.4 0.4 0.4 1.0"/>
</material>

<material name="blue1">
<color rgba="0.06 0.46 0.9 0.6"/>
</material>

<material name="orange_redish">
<color rgba="1 0.25 0 0.4"/>
</material>

<configuration type="tailsitter"/>

<properties arm="0.0635" kf="2.0e-8" km="2.74e-10" thrust2weight="2" max_speed_kmh="30" gnd_eff_coeff="11.36859" prop_radius="3.31348e-2" drag_coeff_xy="9.1785e-7" drag_coeff_z="10.311e-7" dw_coeff_1="2267.18" dw_coeff_2=".16" dw_coeff_3="-.11"/>

<control>
<indi actuator_nr="4" output_nr="4" />
<indi_1 roll=" 50.0 50.0 -50.0 -50.0" />
<indi_2 pitch="-50.0 50.0 50.0 -50.0" />
<indi_3 yaw=" -7.0 7.0 -7.0 7.0" />
<indi_4 thrust=" 1.7 1.7 1.7 1.7" />

<indi_guidance_gains>
<pos kp="1.0" kd="2.2"/>
</indi_guidance_gains>

<indi_att_gains>
<att p="7." q="7." r="5." />
<rate p="18." q="18." r="10." />
</indi_att_gains>

<pwm>
<pwm2rpm scale="20000. 20000. 20000. 20000." const="0. 0. 0. 0." />
<limit min="0 0 0 0" max="1 1 1 1" />
</pwm>
</control>


<link name="base_link">

<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value="0.75"/>
<inertia ixx="6.2e-4" ixy="0.0" ixz="0.0" iyy="6.2e-4" iyz="0.0" izz="1.1e-3"/>
</inertial>

<visual>
<origin rpy="0 1.65 3.1415" xyz="-0.02 0 0.135"/>
<geometry>
<mesh filename="./tailsitter_falcon_geom/body.obj" scale="1. 1. 1."/>
</geometry>
<material name="darkgrey"/>
</visual>

<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<cylinder radius="0.15" length=".1"/>
</geometry>
</collision>

</link>

<link name="prop0_link">
<inertial>
<origin rpy="0 0 0" xyz="0.11 0.11 0"/>
<mass value="0"/>
<inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0.11 0.11 0.01"/>
<geometry>
<cylinder radius="0.0635" length=".001"/>
</geometry>
<material name="orange_redish"/>
</visual>
</link>
<joint name="prop0_joint" type="fixed">
<parent link="base_link"/>
<child link="prop0_link"/>
</joint>

<link name="prop1_link">
<inertial>
<origin rpy="0 0 0" xyz="-0.11 0.11 0"/>
<mass value="0"/>
<inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="-0.11 0.11 0.02"/>
<geometry>
<cylinder radius="0.0635" length=".001"/>
</geometry>
<material name="blue1"/>
</visual>
</link>
<joint name="prop1_joint" type="fixed">
<parent link="base_link"/>
<child link="prop1_link"/>
</joint>

<link name="prop2_link">
<inertial>
<origin rpy="0 0 0" xyz="-0.11 -0.11 0.03"/>
<mass value="0"/>
<inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="-0.11 -0.11 0.01"/>
<geometry>
<cylinder radius="0.0635" length=".001"/>
</geometry>
<material name="blue1"/>
</visual>
</link>
<joint name="prop2_joint" type="fixed">
<parent link="base_link"/>
<child link="prop2_link"/>
</joint>

<link name="prop3_link">
<inertial>
<origin rpy="0 0 0" xyz="0.11 -0.11 0.04"/>
<mass value="0"/>
<inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0.11 -0.11 0.01"/>
<geometry>
<cylinder radius="0.0635" length=".001"/>
</geometry>
<material name="orange_redish"/>
</visual>
</link>
<joint name="prop3_joint" type="fixed">
<parent link="base_link"/>
<child link="prop3_link"/>
</joint>

<link name="Rflap_link">
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value="0"/>
<inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
</inertial>
<visual>
<origin rpy="0 1.65 3.1415" xyz="-0.02 0 0.205"/>
<geometry>
<mesh filename="./tailsitter_falcon_geom/Rflap.obj" scale="1. 1. 1."/>
</geometry>
<material name="orange_red">
<color rgba="1 0.25 0 0.9"/>
</material>
</visual>
</link>
<joint name="Rflap_joint" type="revolute">
<axis xyz="0 1 0"/>
<origin rpy="0 0 0" xyz="0. 0. -0.07"/>
<parent link="base_link"/>
<child link="Rflap_link"/>
<limit effort="1" velocity="0.5"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>

<link name="Lflap_link">
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value="0"/>
<inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
</inertial>
<visual>
<origin rpy="0 1.65 3.1415" xyz="-0.02 0 0.205"/>
<geometry>
<mesh filename="./tailsitter_falcon_geom/Lflap.obj" scale="1. 1. 1."/>
</geometry>
<material name="orange_red">
<color rgba="1 0.25 0 0.9"/>
</material>
</visual>
</link>
<joint name="Lflap_joint" type="revolute">
<axis xyz="0 1 0"/>
<origin rpy="0 0 0" xyz="0. 0. -0.07"/>
<parent link="base_link"/>
<child link="Lflap_link"/>
<limit effort="1" velocity="0.5"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>

<link name="center_of_mass_link">
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value="0"/>
<inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
</inertial>
</link>
<joint name="center_of_mass_joint" type="fixed">
<parent link="base_link"/>
<child link="center_of_mass_link"/>
</joint>

</robot>

7 changes: 7 additions & 0 deletions conf/simulator/pybullet/tailsitter_falcon_geom/Lflap.mtl
View file
@@ -0,0 +1,7 @@
newmtl 0.498039_0.498039_0.498039_0.000000_0.000000
Ka 0.0000 0.0000 0.0000
Kd 0.498 0.498 0.498
Ks 0.0000 0.0000 0.0000
Tf 0.0000 0.0000 0.0000
d 1
Ns 0
1,689 changes: 1,689 additions & 0 deletions conf/simulator/pybullet/tailsitter_falcon_geom/Lflap.obj
View file

Large diffs are not rendered by default.

7 changes: 7 additions & 0 deletions conf/simulator/pybullet/tailsitter_falcon_geom/Rflap.mtl
View file
@@ -0,0 +1,7 @@
newmtl 0.498039_0.498039_0.498039_0.000000_0.000000
Ka 0.0000 0.0000 0.0000
Kd 0.498 0.498 0.498
Ks 0.0000 0.0000 0.0000
Tf 0.0000 0.0000 0.0000
d 1
Ns 0
1,681 changes: 1,681 additions & 0 deletions conf/simulator/pybullet/tailsitter_falcon_geom/Rflap.obj
View file

Large diffs are not rendered by default.

56 changes: 56 additions & 0 deletions conf/simulator/pybullet/tailsitter_falcon_geom/body.mtl
View file
@@ -0,0 +1,56 @@
newmtl 0.231373_0.380392_0.705882_0.000000_0.000000
Ka 0.0000 0.0000 0.0000
Kd 0.2314 0.3804 0.7059
Ks 0.0000 0.0000 0.0000
Tf 0.0000 0.0000 0.0000
d 1
Ns 0
newmtl 0.498039_0.498039_0.498039_0.000000_0.000000
Ka 0.0000 0.0000 0.0000
Kd 0.498 0.498 0.498
Ks 0.0000 0.0000 0.0000
Tf 0.0000 0.0000 0.0000
d 1
Ns 0
newmtl 0.615686_0.811765_0.929412_0.000000_0.000000
Ka 0.0000 0.0000 0.0000
Kd 0.6157 0.8118 0.9294
Ks 0.0000 0.0000 0.0000
Tf 0.0000 0.0000 0.0000
d 1
Ns 0
newmtl 0.647059_0.647059_0.647059_0.000000_0.000000
Ka 0.0000 0.0000 0.0000
Kd 0.6471 0.6471 0.6471
Ks 0.0000 0.0000 0.0000
Tf 0.0000 0.0000 0.0000
d 1
Ns 0
newmtl 0.768627_0.886275_0.952941_0.000000_0.000000
Ka 0.0000 0.0000 0.0000
Kd 0.7686 0.8863 0.9529
Ks 0.0000 0.0000 0.0000
Tf 0.0000 0.0000 0.0000
d 1
Ns 0
newmtl 0.917647_0.917647_0.917647_0.000000_0.000000
Ka 0.0000 0.0000 0.0000
Kd 0.9176 0.9176 0.9176
Ks 0.0000 0.0000 0.0000
Tf 0.0000 0.0000 0.0000
d 1
Ns 0
newmtl 0.972549_0.529412_0.003922_0.000000_0.000000
Ka 0.0000 0.0000 0.0000
Kd 0.9725 0.5294 0.003922
Ks 0.0000 0.0000 0.0000
Tf 0.0000 0.0000 0.0000
d 1
Ns 0
newmtl 0.980392_0.713725_0.003922_0.000000_0.000000
Ka 0.0000 0.0000 0.0000
Kd 0.9804 0.7137 0.003922
Ks 0.0000 0.0000 0.0000
Tf 0.0000 0.0000 0.0000
d 1
Ns 0
122,443 changes: 122,443 additions & 0 deletions conf/simulator/pybullet/tailsitter_falcon_geom/body.obj
View file

Large diffs are not rendered by default.

231 changes: 231 additions & 0 deletions conf/telemetry/cohoma_debug.xml
View file
@@ -0,0 +1,231 @@
<?xml version="1.0"?>
<!DOCTYPE telemetry SYSTEM "telemetry.dtd">
<telemetry>


<process name="Main">

<mode name="default" key_press="d">
<message name="AUTOPILOT_VERSION" period="11.1"/>
<message name="DL_VALUE" period="1.1"/>
<message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="ROTORCRAFT_FP" period="0.25"/>
<message name="ALIVE" period="2.1"/>
<message name="INS_REF" period="5.1"/>
<message name="ROTORCRAFT_NAV_STATUS" period="1.6"/>
<message name="WP_MOVED" period="1.3"/>
<message name="ROTORCRAFT_CAM" period="1."/>
<message name="GPS_INT" period=".25"/>
<message name="INS" period=".25"/>
<message name="I2C_ERRORS" period="4.1"/>
<message name="UART_ERRORS" period="3.1"/>
<message name="SUPERBITRF" period="3"/>
<message name="ENERGY" period="2.5"/>
<message name="DATALINK_REPORT" period="5.1"/>
<message name="STATE_FILTER_STATUS" period="3.2"/>
<message name="AIR_DATA" period="1.3"/>
<message name="SURVEY" period="2.5"/>
<message name="OPTIC_FLOW_EST" period="0.05"/>
<message name="VECTORNAV_INFO" period="0.5"/>
<message name="OPTICAL_FLOW_HOVER" period="0.05"/>
<message name="VISUALTARGET" period="0.10"/>
<message name="VISION_POSITION_ESTIMATE" period="0.1"/>
<message name="DIVERGENCE" period="0.05"/>
<message name="DRAGSPEED" period="0.02"/>
<message name="LOGGER_STATUS" period="5.1"/>
<message name="LIDAR" period="1.2"/>
<message name="INS_EKF2" period=".25"/>
<message name="WIND_INFO_RET" period="1."/>
<message name="AHRS_REF_QUAT" period="0.5"/>
<message name="GUIDANCE_INDI_HYBRID" period="0.1"/>
<message name="ACTUATORS" period="0.1"/>
<message name="STAB_ATTITUDE_INDI" period=".25"/>
</mode>

<mode name="ppm">
<message name="DL_VALUE" period="0.5"/>
<message name="ALIVE" period="2.1"/>
<message name="ROTORCRAFT_CMD" period=".05"/>
<message name="PPM" period="0.5"/>
<message name="RC" period="0.5"/>
<message name="ROTORCRAFT_RADIO_CONTROL" period="0.5"/>
<message name="ROTORCRAFT_STATUS" period="1"/>
<message name="BEBOP_ACTUATORS" period="0.2"/>
</mode>

<mode name="raw_sensors">
<message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="DL_VALUE" period="0.5"/>
<message name="ALIVE" period="2.1"/>
<message name="IMU_ACCEL_RAW" period=".05"/>
<message name="IMU_GYRO_RAW" period=".05"/>
<message name="IMU_MAG_RAW" period=".05"/>
<message name="BARO_RAW" period=".1"/>
<message name="IMU_MAG_CURRENT_CALIBRATION" period=".05"/>
<message name="ARDRONE_NAVDATA" period=".05"/>
</mode>

<mode name="scaled_sensors">
<message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="DL_VALUE" period="0.5"/>
<message name="ALIVE" period="2.1"/>
<message name="IMU_GYRO_SCALED" period=".075"/>
<message name="IMU_ACCEL_SCALED" period=".075"/>
<message name="IMU_MAG_SCALED" period=".1"/>
</mode>

<mode name="ahrs">
<message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="DL_VALUE" period="0.5"/>
<message name="ALIVE" period="2.1"/>
<message name="FILTER_ALIGNER" period="2.2"/>
<message name="FILTER" period=".5"/>
<message name="GEO_MAG" period="5."/>
<message name="AHRS_GYRO_BIAS_INT" period="0.08"/>
<message name="AHRS_QUAT_INT" period=".25"/>
<message name="AHRS_EULER_INT" period=".1"/>
<message name="AHRS_EULER" period=".1"/>
<!-- <message name="AHRS_RMAT_INT" period=".5"/> -->
</mode>

<mode name="rate_loop">
<message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="DL_VALUE" period="0.5"/>
<message name="ALIVE" period="2.1"/>
<message name="RATE_LOOP" period=".02"/>
</mode>

<mode name="attitude_setpoint_viz" key_press="v">
<message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="DL_VALUE" period="0.5"/>
<message name="ALIVE" period="0.9"/>
<message name="ROTORCRAFT_RADIO_CONTROL" period="0.1"/>
<message name="AHRS_REF_QUAT" period="0.05"/>
</mode>

<mode name="attitude_loop" key_press="a">
<message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="DL_VALUE" period="0.5"/>
<message name="ALIVE" period="0.9"/>
<message name="STAB_ATTITUDE_INT" period=".03"/>
<message name="STAB_ATTITUDE_REF_INT" period=".03"/>
<message name="STAB_ATTITUDE_FLOAT" period=".03"/>
<message name="STAB_ATTITUDE_REF_FLOAT" period=".03"/>
<message name="STAB_ATTITUDE_INDI" period=".25"/>
<message name="INDI_G" period="2.0"/>
</mode>

<mode name="vert_loop" key_press="v">
<message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="DL_VALUE" period="0.5"/>
<message name="ALIVE" period="0.9"/>
<message name="VFF" period=".05"/>
<message name="VFF_EXTENDED" period=".05"/>
<message name="VERT_LOOP" period=".05"/>
<message name="INS_Z" period=".05"/>
<message name="INS" period=".11"/>
<message name="INS_REF" period="5.1"/>
</mode>

<mode name="vel_guidance" key_press="q">
<message name="ALIVE" period="0.9"/>
<message name="HYBRID_GUIDANCE" period="0.062"/>
<message name="GUIDANCE_INDI_HYBRID" period="0.1"/>
<!--<message name="STAB_ATTITUDE_REF" period="0.4"/>-->
<message name="ROTORCRAFT_FP" period="0.8"/>
<message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="ROTORCRAFT_NAV_STATUS" period="1.6"/>
<message name="INS_REF" period="5.1"/>
<message name="WP_MOVED" period="1.3"/>
<message name="GPS_INT" period="1.0"/>
<message name="INS" period="1.0"/>
</mode>

<mode name="h_loop" key_press="h">
<message name="ALIVE" period="0.9"/>
<message name="HOVER_LOOP" period="0.062"/>
<message name="GUIDANCE_H_REF_INT" period="0.062"/>
<message name="STAB_ATTITUDE_INT" period="0.4"/>
<message name="STAB_ATTITUDE_FLOAT" period="0.4"/>
<!--<message name="STAB_ATTITUDE_REF_INT" period="0.4"/>-->
<message name="ROTORCRAFT_FP" period="0.8"/>
<message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="ROTORCRAFT_NAV_STATUS" period="1.6"/>
<message name="INS_REF" period="5.1"/>
<!-- HFF messages are only sent if USE_HFF -->
<message name="HFF" period=".05"/>
<message name="HFF_GPS" period=".03"/>
<message name="HFF_DBG" period=".2"/>
</mode>

<mode name="aligner">
<message name="ALIVE" period="0.9"/>
<message name="FILTER_ALIGNER" period="0.02"/>
</mode>

<mode name="tune_hover">
<message name="DL_VALUE" period="1.1"/>
<message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="ALIVE" period="2.1"/>
<message name="GUIDANCE_H_INT" period="0.05"/>
<message name="ROTORCRAFT_TUNE_HOVER" period=".1"/>
<!-- <message name="GPS_INT" period=".20"/> -->
<!--<message name="INS2" period=".05"/>
<message name="INS3" period=".20"/>-->
<message name="INS_REF" period="5.1"/>
</mode>

<mode name="RTCM3" >
<message name="GPS_RXMRTCM" period="1"/>
<message name="GPS_INT" period=".25"/>
<message name="GPS_RTK" period="1"/>
</mode>

</process>

<process name="FlightRecorder">
<mode name="default">
<message name="AUTOPILOT_VERSION" period="11.1"/>
<message name="DL_VALUE" period="1.1"/>
<message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="ROTORCRAFT_FP" period="0.25"/>
<message name="INS_REF" period="5.1"/>
<message name="ROTORCRAFT_NAV_STATUS" period="1.6"/>
<message name="WP_MOVED" period="1.3"/>
<message name="GPS_INT" period=".1"/>
<message name="INS" period=".1"/>
<message name="I2C_ERRORS" period="4.1"/>
<message name="UART_ERRORS" period="3.1"/>
<message name="ENERGY" period="2.5"/>
<message name="DATALINK_REPORT" period="5.1"/>
<message name="STATE_FILTER_STATUS" period="3.2"/>
<message name="AIR_DATA" period="1.3"/>
<message name="SURVEY" period="2.5"/>
<message name="IMU_GYRO_SCALED" period=".075"/>
<message name="IMU_ACCEL_SCALED" period=".075"/>
<message name="IMU_MAG_SCALED" period=".2"/>
<message name="LIDAR" period="0.05"/>
<message name="GUIDANCE_INDI_HYBRID" period="0.1"/>
<message name="ACTUATORS" period="0.1"/>
<message name="STAB_ATTITUDE_INT" period=".03"/>
<message name="STAB_ATTITUDE_REF_INT" period=".03"/>
<message name="STAB_ATTITUDE_FLOAT" period=".03"/>
<message name="STAB_ATTITUDE_REF_FLOAT" period=".03"/>
<message name="STAB_ATTITUDE_INDI" period=".25"/>
<message name="INDI_G" period="2.0"/>
</mode>
</process>

<process name="Extra">
<mode name="default">
<message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="GPS_INT" period=".25"/>
<message name="MISSION_STATUS" period="0.5"/>
<message name="ROTORCRAFT_FP" period="0.25"/>
<message name="INS_REF" period="5.1"/>
<message name="INS" period=".25"/>
</mode>
</process>

</telemetry>

177 changes: 177 additions & 0 deletions conf/telemetry/rotorcraft_imav2023.xml
View file
@@ -0,0 +1,177 @@
<?xml version="1.0"?>
<!DOCTYPE telemetry SYSTEM "telemetry.dtd">
<telemetry>


<process name="Main">

<mode name="default" key_press="d">
<message name="AUTOPILOT_VERSION" period="11.1"/>
<message name="DL_VALUE" period="1.1"/>
<message name="ROTORCRAFT_STATUS" period="3.2"/>
<message name="ROTORCRAFT_FP" period="0.5"/>
<message name="ALIVE" period="3.1"/>
<message name="INS_REF" period="5.1"/>
<message name="ROTORCRAFT_NAV_STATUS" period="2.6"/>
<message name="WP_MOVED" period="2.3"/>
<message name="GPS_INT" period="1."/>
<!--message name="INS" period="1.2"/-->
<message name="ENERGY" period="3.5"/>
<message name="DATALINK_REPORT" period="6.1"/>
<message name="STATE_FILTER_STATUS" period="4.2"/>
<message name="AIR_DATA" period="2.3"/>
<!--message name="SURVEY" period="4.5"/-->
<message name="FBW_STATUS" period="2.0"/>
</mode>

<mode name="ppm">
<message name="DL_VALUE" period="0.5"/>
<message name="ALIVE" period="2.1"/>
<message name="ROTORCRAFT_CMD" period=".05"/>
<message name="PPM" period="0.5"/>
<message name="RC" period="0.5"/>
<message name="ROTORCRAFT_RADIO_CONTROL" period="0.5"/>
<message name="ROTORCRAFT_STATUS" period="1"/>
<message name="BEBOP_ACTUATORS" period="0.2"/>
</mode>

<mode name="raw_sensors">
<message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="DL_VALUE" period="0.5"/>
<message name="ALIVE" period="2.1"/>
<message name="IMU_ACCEL_RAW" period=".05"/>
<message name="IMU_GYRO_RAW" period=".05"/>
<message name="IMU_MAG_RAW" period=".05"/>
<message name="BARO_RAW" period=".1"/>
<message name="ARDRONE_NAVDATA" period=".05"/>
</mode>

<mode name="scaled_sensors">
<message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="DL_VALUE" period="0.5"/>
<message name="ALIVE" period="2.1"/>
<message name="IMU_GYRO_SCALED" period=".075"/>
<message name="IMU_ACCEL_SCALED" period=".075"/>
<message name="IMU_MAG_SCALED" period=".1"/>
</mode>

<mode name="ahrs">
<message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="DL_VALUE" period="0.5"/>
<message name="ALIVE" period="2.1"/>
<message name="FILTER_ALIGNER" period="2.2"/>
<message name="FILTER" period=".5"/>
<message name="GEO_MAG" period="5."/>
<message name="AHRS_GYRO_BIAS_INT" period="0.08"/>
<message name="AHRS_QUAT_INT" period=".25"/>
<message name="AHRS_EULER_INT" period=".1"/>
<!-- <message name="AHRS_RMAT_INT" period=".5"/> -->
</mode>

<mode name="rate_loop">
<message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="DL_VALUE" period="0.5"/>
<message name="ALIVE" period="2.1"/>
<message name="RATE_LOOP" period=".02"/>
</mode>

<mode name="attitude_setpoint_viz" key_press="v">
<message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="DL_VALUE" period="0.5"/>
<message name="ALIVE" period="0.9"/>
<message name="ROTORCRAFT_RADIO_CONTROL" period="0.1"/>
<message name="AHRS_REF_QUAT" period="0.05"/>
</mode>

<mode name="attitude_loop" key_press="a">
<message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="DL_VALUE" period="0.5"/>
<message name="ALIVE" period="0.9"/>
<message name="STAB_ATTITUDE_INT" period=".03"/>
<message name="STAB_ATTITUDE_REF_INT" period=".03"/>
<message name="STAB_ATTITUDE_INDI" period=".25"/>
</mode>

<mode name="vert_loop" key_press="v">
<message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="DL_VALUE" period="0.5"/>
<message name="ALIVE" period="0.9"/>
<message name="VFF" period=".05"/>
<message name="VFF_EXTENDED" period=".05"/>
<message name="VERT_LOOP" period=".05"/>
<message name="INS_Z" period=".05"/>
<message name="INS" period=".11"/>
<message name="INS_REF" period="5.1"/>
</mode>

<mode name="h_loop" key_press="h">
<message name="ALIVE" period="0.9"/>
<message name="HOVER_LOOP" period="0.062"/>
<message name="GUIDANCE_H_REF_INT" period="0.062"/>
<message name="STAB_ATTITUDE_INT" period="0.4"/>
<!--<message name="STAB_ATTITUDE_REF_INT" period="0.4"/>-->
<message name="ROTORCRAFT_FP" period="0.8"/>
<message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="ROTORCRAFT_NAV_STATUS" period="1.6"/>
<message name="INS_REF" period="5.1"/>
<!-- HFF messages are only sent if USE_HFF -->
<message name="HFF" period=".05"/>
<message name="HFF_GPS" period=".03"/>
<message name="HFF_DBG" period=".2"/>
</mode>

<mode name="aligner">
<message name="ALIVE" period="0.9"/>
<message name="FILTER_ALIGNER" period="0.02"/>
</mode>

<mode name="tune_hover">
<message name="DL_VALUE" period="1.1"/>
<message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="ALIVE" period="2.1"/>
<message name="GUIDANCE_H_INT" period="0.05"/>
<message name="ROTORCRAFT_TUNE_HOVER" period=".1"/>
<!-- <message name="GPS_INT" period=".20"/> -->
<!--<message name="INS2" period=".05"/>
<message name="INS3" period=".20"/>-->
<message name="INS_REF" period="5.1"/>
</mode>

</process>

<process name="FlightRecorder">
<mode name="default">
<message name="AUTOPILOT_VERSION" period="11.1"/>
<message name="DL_VALUE" period="1.1"/>
<message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="ROTORCRAFT_FP" period="0.1"/>
<message name="INS_REF" period="5.1"/>
<message name="ROTORCRAFT_NAV_STATUS" period="1.6"/>
<message name="WP_MOVED" period="1.3"/>
<message name="GPS_INT" period=".1"/>
<message name="INS" period=".1"/>
<message name="I2C_ERRORS" period="4.1"/>
<message name="UART_ERRORS" period="3.1"/>
<message name="ENERGY" period="0.1"/>
<message name="DATALINK_REPORT" period="5.1"/>
<message name="STATE_FILTER_STATUS" period="3.2"/>
<message name="AIR_DATA" period="0.1"/>
<message name="SURVEY" period="2.5"/>
<message name="IMU_GYRO_SCALED" period=".075"/>
<message name="IMU_ACCEL_SCALED" period=".075"/>
<message name="IMU_MAG_SCALED" period=".2"/>
<message name="AHRS_QUAT_INT" period=".05"/>
<message name="LIDAR" period="0.05"/>
<message name="GUIDANCE_INDI_HYBRID" period="0.05"/>
</mode>
</process>

<process name="Extra">
<mode name="default">
<message name="ROTORCRAFT_FP" period="0.1"/>
<message name="AHRS_QUAT_INT" period="0.1"/>
</mode>
</process>

</telemetry>

2 changes: 1 addition & 1 deletion sw/airborne/arch/sim/modules/actuators/actuators_dshot_arch.c
View file
Expand Up @@ -27,7 +27,7 @@

#include "modules/actuators/actuators_dshot.h"

uint16_t actuators_dshot_values[ACTUATORS_DSHOT_NB];
struct dshot actuators_dshot_values[ACTUATORS_DSHOT_NB];

void actuators_dshot_arch_init(void) {}

Expand Down
138 changes: 108 additions & 30 deletions sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.c
View file
Expand Up @@ -73,6 +73,15 @@
#define GUIDANCE_INDI_LIFTD_P50 (GUIDANCE_INDI_LIFTD_P80/2)
#endif

#ifndef GUIDANCE_INDI_DRAG_OVER_LIFT
#define GUIDANCE_INDI_DRAG_OVER_LIFT 0.2f
#endif

#ifndef GUIDANCE_INDI_PITCH_OFFSET_GAIN
#define GUIDANCE_INDI_PITCH_OFFSET_GAIN 0.f
#endif


struct guidance_indi_hybrid_params gih_params = {
.pos_gain = GUIDANCE_INDI_POS_GAIN,
.pos_gainz = GUIDANCE_INDI_POS_GAINZ,
Expand All @@ -84,6 +93,8 @@ struct guidance_indi_hybrid_params gih_params = {
.liftd_asq = GUIDANCE_INDI_LIFTD_ASQ, // coefficient of airspeed squared
.liftd_p80 = GUIDANCE_INDI_LIFTD_P80,
.liftd_p50 = GUIDANCE_INDI_LIFTD_P50,

.pitch_offset_gain = GUIDANCE_INDI_PITCH_OFFSET_GAIN,
};

#ifndef GUIDANCE_INDI_MAX_AIRSPEED
Expand Down Expand Up @@ -134,6 +145,7 @@ float inv_eff[4];

// Max bank angle in radians
float guidance_indi_max_bank = GUIDANCE_H_MAX_BANK;
float guidance_indi_min_pitch = GUIDANCE_INDI_MIN_PITCH;

/** state eulers in zxy order */
struct FloatEulers eulers_zxy;
Expand All @@ -147,10 +159,31 @@ Butterworth2LowPass accely_filt;

struct FloatVect2 desired_airspeed;

struct FloatMat33 Ga;
struct FloatMat33 Ga_inv;
float Ga[3][3];
float Ga_inv[3][3];
struct FloatVect3 euler_cmd;

#define GUIDANCE_INDI_HYBRID_USE_WLS 1

#if GUIDANCE_INDI_HYBRID_USE_WLS
#include "firmwares/rotorcraft/stabilization/stabilization_indi.h"
#include "wls/wls_alloc.h"
float du_min_gih[3];
float du_max_gih[3];
float du_pref_gih[3];
float *Bwls_gih[3];
#ifdef GUIDANCE_INDI_HYBRID_WLS_PRIORITIES
float Wv_gih[3] = GUIDANCE_INDI_HYBRID_WLS_PRIORITIES;
#else
float Wv_gih[3] = { 100.f, 100.f, 1.f }; // X,Y accel, Z accel
#endif
#ifdef GUIDANCE_INDI_HYBRID_WLS_WU
float Wu_gih[3] = GUIDANCE_INDI_HYBRID_WLS_WU;
#else
float Wu_gih[3] = { 1.f, 1.f, 1.f };
#endif
#endif

float filter_cutoff = GUIDANCE_INDI_FILTER_CUTOFF;

float guidance_indi_hybrid_heading_sp = 0.f;
Expand All @@ -168,7 +201,7 @@ struct FloatVect3 indi_vel_sp = {0.0, 0.0, 0.0};
float time_of_vel_sp = 0.0;

void guidance_indi_propagate_filters(void);
static void guidance_indi_calcg_wing(struct FloatMat33 *Gmat);
static void guidance_indi_calcg_wing(float Gmat[3][3]);
static float guidance_indi_get_liftd(float pitch, float theta);

#if PERIODIC_TELEMETRY
Expand Down Expand Up @@ -209,6 +242,12 @@ void guidance_indi_init(void)
init_butterworth_2_low_pass(&thrust_filt, tau, sample_time, 0.0);
init_butterworth_2_low_pass(&accely_filt, tau, sample_time, 0.0);

#if GUIDANCE_INDI_HYBRID_USE_WLS
for (int8_t i = 0; i < 3; i++) {
Bwls_gih[i] = Ga[i];
}
#endif

#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_GUIDANCE_INDI_HYBRID, send_guidance_indi_hybrid);
#endif
Expand Down Expand Up @@ -257,6 +296,15 @@ struct StabilizationSetpoint guidance_indi_run(struct FloatVect3 *accel_sp, floa
transition_theta_offset = INT_MULT_RSHIFT((transition_percentage <<
(INT32_ANGLE_FRAC - INT32_PERCENTAGE_FRAC)) / 100, max_offset, INT32_ANGLE_FRAC);

/* Compute guidance pitch offset based on desired ground speed
* TODO check if this is really what we want
*/
float desired_speed = VECT2_NORM2(gi_speed_sp);
float guidance_pitch_offset = TRANSITION_MAX_OFFSET * desired_speed / guidance_indi_max_airspeed; // FIXME mix ground and airspeed ?
Bound(guidance_pitch_offset, TRANSITION_MAX_OFFSET, 0.f);
//float guidance_pitch_incr = gih_params.pitch_offset_gain * (guidance_pitch_offset - eulers_zxy.theta);
//printf("pitch offset %f\n", DegOfRad(guidance_pitch_offset));

// filter accel to get rid of noise and filter attitude to synchronize with accel
guidance_indi_propagate_filters();

Expand All @@ -273,11 +321,6 @@ struct StabilizationSetpoint guidance_indi_run(struct FloatVect3 *accel_sp, floa
sp_accel.z = -(radio_control.values[RADIO_THROTTLE]-4500)*8.0/9600.0;
#endif

// Calculate matrix of partial derivatives
guidance_indi_calcg_wing(&Ga);
// Invert this matrix
MAT33_INV(Ga_inv, Ga);

struct FloatVect3 accel_filt;
accel_filt.x = filt_accel_ned[0].o[0];
accel_filt.y = filt_accel_ned[1].o[0];
Expand All @@ -301,8 +344,39 @@ struct StabilizationSetpoint guidance_indi_run(struct FloatVect3 *accel_sp, floa
#endif
#endif

//Calculate roll,pitch and thrust command
MAT33_VECT3_MUL(euler_cmd, Ga_inv, a_diff);
// Calculate matrix of partial derivatives
guidance_indi_calcg_wing(Ga);
#if GUIDANCE_INDI_HYBRID_USE_WLS
// Set lower limits
du_min_gih[0] = -guidance_indi_max_bank - roll_filt.o[0]; // roll
du_min_gih[1] = RadOfDeg(guidance_indi_min_pitch) - pitch_filt.o[0]; // pitch
du_min_gih[2] = (MAX_PPRZ - actuator_state_filt_vect[0]) * g1g2[3][0] + (MAX_PPRZ - actuator_state_filt_vect[1]) * g1g2[3][1] + (MAX_PPRZ - actuator_state_filt_vect[2]) * g1g2[3][2] + (MAX_PPRZ - actuator_state_filt_vect[3]) * g1g2[3][3];

// Set upper limits limits
du_max_gih[0] = guidance_indi_max_bank - roll_filt.o[0]; //roll
du_max_gih[1] = RadOfDeg(GUIDANCE_INDI_MAX_PITCH) - pitch_filt.o[0]; // pitch
du_max_gih[2] = -(actuator_state_filt_vect[0]*g1g2[3][0] + actuator_state_filt_vect[1]*g1g2[3][1] + actuator_state_filt_vect[2]*g1g2[3][2] + actuator_state_filt_vect[3]*g1g2[3][3]);

// Set prefered states
du_pref_gih[0] = 0.; // prefered delta roll angle
du_pref_gih[1] = -pitch_filt.o[0]; // + pitch_pref_rad; // prefered delta pitch angle TODO use guidance_pitch_offset here ?
du_pref_gih[2] = du_max_gih[2];

float v_gih[3] = { a_diff.x, a_diff.y, a_diff.z };
float du_gih[3];
int num_iter UNUSED = wls_alloc_guidance(
du_gih, v_gih, du_min_gih, du_max_gih,
Bwls_gih, 0, 0, Wv_gih, Wu_gih, du_pref_gih, 100000, 10);
euler_cmd.x = du_gih[0];
euler_cmd.y = du_gih[1];
euler_cmd.z = du_gih[2];

#else // compute inverse matrix of Ga
// Invert this matrix
float_mat_inv_3d(Ga_inv, Ga);
// Calculate roll,pitch and thrust command
float_mat3_mult(&euler_cmd, Ga_inv, a_diff);
#endif

//printf("abi thrust %f\n", euler_cmd.z);
AbiSendMsgTHRUST(THRUST_INCREMENT_ID, euler_cmd.z);
Expand All @@ -320,11 +394,11 @@ struct StabilizationSetpoint guidance_indi_run(struct FloatVect3 *accel_sp, floa
Bound(airspeed_turn, 10.0f, 30.0f);

guidance_euler_cmd.phi = roll_filt.o[0] + euler_cmd.x;
guidance_euler_cmd.theta = pitch_filt.o[0] + euler_cmd.y;
guidance_euler_cmd.theta = pitch_filt.o[0] + euler_cmd.y; // + guidance_pitch_incr;

//Bound euler angles to prevent flipping
Bound(guidance_euler_cmd.phi, -guidance_indi_max_bank, guidance_indi_max_bank);
Bound(guidance_euler_cmd.theta, RadOfDeg(GUIDANCE_INDI_MIN_PITCH), RadOfDeg(GUIDANCE_INDI_MAX_PITCH));
Bound(guidance_euler_cmd.theta, RadOfDeg(guidance_indi_min_pitch), RadOfDeg(GUIDANCE_INDI_MAX_PITCH));

// Use the current roll angle to determine the corresponding heading rate of change.
float coordinated_turn_roll = eulers_zxy.phi;
Expand Down Expand Up @@ -633,7 +707,7 @@ void guidance_indi_propagate_filters(void) {
*
* @param Gmat array to write the matrix to [3x3]
*/
void guidance_indi_calcg_wing(struct FloatMat33 *Gmat) {
void guidance_indi_calcg_wing(float Gmat[3][3]) {

/*Pre-calculate sines and cosines*/
float sphi = sinf(eulers_zxy.phi);
Expand All @@ -650,22 +724,26 @@ void guidance_indi_calcg_wing(struct FloatMat33 *Gmat) {

/*Amount of lift produced by the wing*/
float pitch_lift = eulers_zxy.theta;
//Bound(pitch_lift,-M_PI_2+0.1f,0);
Bound(pitch_lift,-M_PI_2,0);
float lift = sinf(pitch_lift)*9.81;
float T = cosf(pitch_lift)*-9.81;
float spitch = sinf(pitch_lift);
float cpitch = cosf(pitch_lift);
float lift = -spitch*spitch*9.81f;
float T = -cpitch*cpitch*9.81f;

// get the derivative of the lift wrt to theta
float liftd = guidance_indi_get_liftd(stateGetAirspeed_f(), eulers_zxy.theta);

RMAT_ELMT(*Gmat, 0, 0) = cphi*ctheta*spsi*T + cphi*spsi*lift;
RMAT_ELMT(*Gmat, 1, 0) = -cphi*ctheta*cpsi*T - cphi*cpsi*lift;
RMAT_ELMT(*Gmat, 2, 0) = -sphi*ctheta*T -sphi*lift;
RMAT_ELMT(*Gmat, 0, 1) = (ctheta*cpsi - sphi*stheta*spsi)*T*GUIDANCE_INDI_PITCH_EFF_SCALING + sphi*spsi*liftd;
RMAT_ELMT(*Gmat, 1, 1) = (ctheta*spsi + sphi*stheta*cpsi)*T*GUIDANCE_INDI_PITCH_EFF_SCALING - sphi*cpsi*liftd;
RMAT_ELMT(*Gmat, 2, 1) = -cphi*stheta*T*GUIDANCE_INDI_PITCH_EFF_SCALING + cphi*liftd;
RMAT_ELMT(*Gmat, 0, 2) = stheta*cpsi + sphi*ctheta*spsi;
RMAT_ELMT(*Gmat, 1, 2) = stheta*spsi - sphi*ctheta*cpsi;
RMAT_ELMT(*Gmat, 2, 2) = cphi*ctheta;
float dragd = liftd * GUIDANCE_INDI_DRAG_OVER_LIFT;

Gmat[0][0] = cphi*ctheta*spsi*T + cphi*spsi*lift;
Gmat[1][0] = -cphi*ctheta*cpsi*T - cphi*cpsi*lift;
Gmat[2][0] = -sphi*ctheta*T -sphi*lift;
Gmat[0][1] = (ctheta*cpsi - sphi*stheta*spsi)*T*GUIDANCE_INDI_PITCH_EFF_SCALING + sphi*spsi*liftd + cpsi*dragd;
Gmat[1][1] = (ctheta*spsi + sphi*stheta*cpsi)*T*GUIDANCE_INDI_PITCH_EFF_SCALING - sphi*cpsi*liftd + spsi*dragd;
Gmat[2][1] = -cphi*stheta*T*GUIDANCE_INDI_PITCH_EFF_SCALING + cphi*liftd;
Gmat[0][2] = stheta*cpsi + sphi*ctheta*spsi;
Gmat[1][2] = stheta*spsi - sphi*ctheta*cpsi;
Gmat[2][2] = cphi*ctheta - cphi*stheta*GUIDANCE_INDI_DRAG_OVER_LIFT;
}

/**
Expand All @@ -676,17 +754,17 @@ void guidance_indi_calcg_wing(struct FloatMat33 *Gmat) {
* @return The derivative of lift w.r.t. pitch
*/
float guidance_indi_get_liftd(float airspeed, float theta) {
float liftd = 0.0;
float liftd = 0.0f;

if(airspeed < 12) {
if(airspeed < 12.f) {
/* Assume the airspeed is too low to be measured accurately
* Use scheduling based on pitch angle instead.
* You can define two interpolation segments
*/
float pitch_interp = DegOfRad(theta);
const float min_pitch = -80.0;
const float middle_pitch = -50.0;
const float max_pitch = -20.0;
const float min_pitch = -80.0f;
const float middle_pitch = -50.0f;
const float max_pitch = -20.0f;

Bound(pitch_interp, min_pitch, max_pitch);
if (pitch_interp > middle_pitch) {
Expand Down
2 changes: 2 additions & 0 deletions sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.h
View file
Expand Up @@ -66,6 +66,7 @@ struct guidance_indi_hybrid_params {
float liftd_asq;
float liftd_p80;
float liftd_p50;
float pitch_offset_gain;
};

extern struct FloatVect3 sp_accel;
Expand All @@ -76,6 +77,7 @@ extern float guidance_indi_specific_force_gain;
extern float guidance_indi_max_airspeed;
extern bool take_heading_control;
extern float guidance_indi_max_bank;
extern float guidance_indi_min_pitch;
extern bool force_forward; ///< forward flight for hybrid nav


Expand Down
377 changes: 377 additions & 0 deletions sw/airborne/firmwares/rotorcraft/guidance/wls/wls_alloc.c
View file
@@ -0,0 +1,377 @@
/*
* Copyright (C) Anton Naruta && Daniel Hoppener
* MAVLab Delft University of Technology
*
* 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, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/

/** @file wls_alloc.c
* @brief This is an active set algorithm for WLS control allocation
*
* This algorithm will find the optimal inputs to produce the least error wrt
* the control objective, taking into account the weighting matrices on the
* control objective and the control effort.
*
* The algorithm is described in:
* Prioritized Control Allocation for Quadrotors Subject to Saturation -
* E.J.J. Smeur, D.C. Höppener, C. de Wagter. In IMAV 2017
*
* written by Anton Naruta && Daniel Hoppener 2016
* MAVLab Delft University of Technology
*/

#include "wls_alloc.h"
#include <stdio.h>
#include "std.h"

#include <string.h>
#include <math.h>
#include <float.h>
#include "math/qr_solve/qr_solve.h"
#include "math/qr_solve/r8lib_min.h"

void print_final_values(int n_u, int n_v, float* u, float** B, float* v, float* umin, float* umax);
void print_in_and_outputs(int n_c, int n_free, float** A_free_ptr, float* d, float* p_free);

// provide loop feedback
#define WLS_VERBOSE FALSE

// Problem size needs to be predefined to avoid having to use VLAs
#ifndef CA_N_V_GUIDANCE
#error CA_N_V_GUIDANCE needs to be defined!
#endif

#ifndef CA_N_U_GUIDANCE
#error CA_N_U_GUIDANCE needs to be defined!
#endif

#define CA_N_C_GUIDANCE (CA_N_U_GUIDANCE+CA_N_V_GUIDANCE)

/**
* @brief Wrapper for qr solve
*
* Possible to use a different solver if needed.
* Solves a system of the form Ax = b for x.
*
* @param m number of rows
* @param n number of columns
*/
void qr_solve_wrapper_guidance(int m, int n, float** A, float* b, float* x) {
float in[m * n];
// convert A to 1d array
int k = 0;
for (int j = 0; j < n; j++) {
for (int i = 0; i < m; i++) {
in[k++] = A[i][j];
}
}
// use solver
qr_solve(m, n, in, b, x);
}

/**
* @brief active set algorithm for control allocation
*
* Takes the control objective and max and min inputs from pprz and calculates
* the inputs that will satisfy most of the control objective, subject to the
* weighting matrices Wv and Wu
*
* @param u The control output vector
* @param v The control objective
* @param umin The minimum u vector
* @param umax The maximum u vector
* @param B The control effectiveness matrix
* @param n_u Length of u
* @param n_v Lenght of v
* @param u_guess Initial value for u
* @param W_init Initial working set, if known
* @param Wv Weighting on different control objectives
* @param Wu Weighting on different controls
* @param up Preferred control vector
* @param gamma_sq Preference of satisfying control objective over desired
* control vector (sqare root of gamma)
* @param imax Max number of iterations
*
* @return Number of iterations, -1 upon failure
*/
int wls_alloc_guidance(float* u, float* v, float* umin, float* umax, float** B,
float* u_guess, float* W_init, float* Wv, float* Wu, float* up,
float gamma_sq, int imax) {
// allocate variables, use defaults where parameters are set to 0
if(!gamma_sq) gamma_sq = 100000;
if(!imax) imax = 100;

int n_c = CA_N_C_GUIDANCE;
int n_u = CA_N_U_GUIDANCE;
int n_v = CA_N_V_GUIDANCE;

float A[CA_N_C_GUIDANCE][CA_N_U_GUIDANCE];
float A_free[CA_N_C_GUIDANCE][CA_N_U_GUIDANCE];

// Create a pointer array to the rows of A_free
// such that we can pass it to a function
float * A_free_ptr[CA_N_C_GUIDANCE];
for(int i = 0; i < n_c; i++)
A_free_ptr[i] = A_free[i];

float b[CA_N_C_GUIDANCE];
float d[CA_N_C_GUIDANCE];

int free_index[CA_N_U_GUIDANCE];
int free_index_lookup[CA_N_U_GUIDANCE];
int n_free = 0;
int free_chk = -1;

int iter = 0;
float p_free[CA_N_U_GUIDANCE];
float p[CA_N_U_GUIDANCE];
float u_opt[CA_N_U_GUIDANCE];
int infeasible_index[CA_N_U_GUIDANCE] UNUSED;
int n_infeasible = 0;
float lambda[CA_N_U_GUIDANCE];
float W[CA_N_U_GUIDANCE];

// Initialize u and the working set, if provided from input
if (!u_guess) {
for (int i = 0; i < n_u; i++) {
u[i] = (umax[i] + umin[i]) * 0.5;
}
} else {
for (int i = 0; i < n_u; i++) {
u[i] = u_guess[i];
}
}
W_init ? memcpy(W, W_init, n_u * sizeof(float))
: memset(W, 0, n_u * sizeof(float));

memset(free_index_lookup, -1, n_u * sizeof(float));

// find free indices
for (int i = 0; i < n_u; i++) {
if (W[i] == 0) {
free_index_lookup[i] = n_free;
free_index[n_free++] = i;
}
}

// fill up A, A_free, b and d
for (int i = 0; i < n_v; i++) {
// If Wv is a NULL pointer, use Wv = identity
b[i] = Wv ? gamma_sq * Wv[i] * v[i] : gamma_sq * v[i];
d[i] = b[i];
for (int j = 0; j < n_u; j++) {
// If Wv is a NULL pointer, use Wv = identity
A[i][j] = Wv ? gamma_sq * Wv[i] * B[i][j] : gamma_sq * B[i][j];
d[i] -= A[i][j] * u[j];
}
}
for (int i = n_v; i < n_c; i++) {
memset(A[i], 0, n_u * sizeof(float));
A[i][i - n_v] = Wu ? Wu[i - n_v] : 1.0;
b[i] = up ? (Wu ? Wu[i-n_v] * up[i-n_v] : up[i-n_v]) : 0;
d[i] = b[i] - A[i][i - n_v] * u[i - n_v];
}

// -------------- Start loop ------------
while (iter++ < imax) {
// clear p, copy u to u_opt
memset(p, 0, n_u * sizeof(float));
memcpy(u_opt, u, n_u * sizeof(float));

// Construct a matrix with the free columns of A
if (free_chk != n_free) {
for (int i = 0; i < n_c; i++) {
for (int j = 0; j < n_free; j++) {
A_free[i][j] = A[i][free_index[j]];
}
}
free_chk = n_free;
}



if (n_free) {
// Still free variables left, calculate corresponding solution

// use a solver to find the solution to A_free*p_free = d
qr_solve_wrapper_guidance(n_c, n_free, A_free_ptr, d, p_free);

//print results current step
#if WLS_VERBOSE
print_in_and_outputs(n_c, n_free, A_free_ptr, d, p_free);
#endif

}

// Set the nonzero values of p and add to u_opt
for (int i = 0; i < n_free; i++) {
p[free_index[i]] = p_free[i];
u_opt[free_index[i]] += p_free[i];
}
// check limits
n_infeasible = 0;
for (int i = 0; i < n_u; i++) {
if (u_opt[i] >= (umax[i] + 0.00001) || u_opt[i] <= (umin[i] - 0.00001)) {
infeasible_index[n_infeasible++] = i;
}
}

// Check feasibility of the solution
if (n_infeasible == 0) {
// all variables are within limits
memcpy(u, u_opt, n_u * sizeof(float));
memset(lambda, 0, n_u * sizeof(float));

// d = d + A_free*p_free; lambda = A*d;
for (int i = 0; i < n_c; i++) {
for (int k = 0; k < n_free; k++) {
d[i] -= A_free[i][k] * p_free[k];
}
for (int k = 0; k < n_u; k++) {
lambda[k] += A[i][k] * d[i];
}
}
bool break_flag = true;

// lambda = lambda x W;
for (int i = 0; i < n_u; i++) {
lambda[i] *= W[i];
// if any lambdas are negative, keep looking for solution
if (lambda[i] < -FLT_EPSILON) {
break_flag = false;
W[i] = 0;
// add a free index
if (free_index_lookup[i] < 0) {
free_index_lookup[i] = n_free;
free_index[n_free++] = i;
}
}
}
if (break_flag) {

#if WLS_VERBOSE
print_final_values(1, n_u, n_v, u, B, v, umin, umax);
#endif

// if solution is found, return number of iterations
return iter;
}
} else {
float alpha = INFINITY;
float alpha_tmp;
int id_alpha = 0;

// find the lowest distance from the limit among the free variables
for (int i = 0; i < n_free; i++) {
int id = free_index[i];
if(fabs(p[id]) > FLT_EPSILON) {
alpha_tmp = (p[id] < 0) ? (umin[id] - u[id]) / p[id]
: (umax[id] - u[id]) / p[id];
} else {
alpha_tmp = INFINITY;
}
if (alpha_tmp < alpha) {
alpha = alpha_tmp;
id_alpha = id;
}
}

// update input u = u + alpha*p
for (int i = 0; i < n_u; i++) {
u[i] += alpha * p[i];
}
// update d = d-alpha*A*p_free
for (int i = 0; i < n_c; i++) {
for (int k = 0; k < n_free; k++) {
d[i] -= A_free[i][k] * alpha * p_free[k];
}
}
// get rid of a free index
W[id_alpha] = (p[id_alpha] > 0) ? 1.0 : -1.0;

free_index[free_index_lookup[id_alpha]] = free_index[--n_free];
free_index_lookup[free_index[free_index_lookup[id_alpha]]] =
free_index_lookup[id_alpha];
free_index_lookup[id_alpha] = -1;
}
}
// solution failed, return negative one to indicate failure
return -1;
}

#if WLS_VERBOSE
void print_in_and_outputs(int n_c, int n_free, float** A_free_ptr, float* d, float* p_free) {

printf("n_c = %d n_free = %d\n", n_c, n_free);

printf("A_free =\n");
for(int i = 0; i < n_c; i++) {
for (int j = 0; j < n_free; j++) {
printf("%f ", A_free_ptr[i][j]);
}
printf("\n");
}

printf("d = ");
for (int j = 0; j < n_c; j++) {
printf("%f ", d[j]);
}

printf("\noutput = ");
for (int j = 0; j < n_free; j++) {
printf("%f ", p_free[j]);
}
printf("\n\n");
}

void print_final_values(int n_u, int n_v, float* u, float** B, float* v, float* umin, float* umax) {
printf("n_u = %d n_v = %d\n", n_u, n_v);

printf("B =\n");
for(int i = 0; i < n_v; i++) {
for (int j = 0; j < n_u; j++) {
printf("%f ", B[i][j]);
}
printf("\n");
}

printf("v = ");
for (int j = 0; j < n_v; j++) {
printf("%f ", v[j]);
}

printf("\nu = ");
for (int j = 0; j < n_u; j++) {
printf("%f ", u[j]);
}
printf("\n");

printf("\numin = ");
for (int j = 0; j < n_u; j++) {
printf("%f ", umin[j]);
}
printf("\n");

printf("\numax = ");
for (int j = 0; j < n_u; j++) {
printf("%f ", umax[j]);
}
printf("\n\n");

}
#endif
61 changes: 61 additions & 0 deletions sw/airborne/firmwares/rotorcraft/guidance/wls/wls_alloc.h
View file
@@ -0,0 +1,61 @@
/*
* Copyright (C) Anton Naruta && Daniel Hoppener
* MAVLab Delft University of Technology
*
* 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, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/

/**
* @brief Wrapper for qr solve
*
* Possible to use a different solver if needed.
* Solves a system of the form Ax = b for x.
*
* @param m number of rows
* @param n number of columns
*/
void qr_solve_wrapper_guidance(int m, int n, float** A, float* b, float* x);

/**
* @brief active set algorithm for control allocation
*
* Takes the control objective and max and min inputs from pprz and calculates
* the inputs that will satisfy most of the control objective, subject to the
* weighting matrices Wv and Wu
*
* @param u The control output vector
* @param v The control objective
* @param umin The minimum u vector
* @param umax The maximum u vector
* @param B The control effectiveness matrix
* @param n_u Length of u
* @param n_v Lenght of v
* @param u_guess Initial value for u
* @param W_init Initial working set, if known
* @param Wv Weighting on different control objectives
* @param Wu Weighting on different controls
* @param up Preferred control vector
* @param gamma_sq Preference of satisfying control objective over desired
* control vector (sqare root of gamma)
* @param imax Max number of iterations
*
* @return Number of iterations, -1 upon failure
*/
int wls_alloc_guidance(float* u, float* v, float* umin, float* umax, float** B,
float* u_guess, float* W_init, float* Wv, float* Wu,
float* ud, float gamma, int imax);
49 changes: 49 additions & 0 deletions sw/airborne/math/pprz_algebra_float.c
View file
Expand Up @@ -813,6 +813,55 @@ void float_mat2_mult(struct FloatVect2 *vect_out, float mat[4], struct FloatVect
vect_out->y = mat[2] * vect_in.x + mat[3] * vect_in.y;
}

/**
* @brief 3x3 matrix inverse
*
* @param inv_out[3][3] inverted matrix output
* @param mat_in[3][3] matrix to be inverted
*
* @return success (0) or not invertible (1)
*/
bool float_mat_inv_3d(float inv_out[3][3], float mat_in[3][3])
{
const float m00 = mat_in[1][1]*mat_in[2][2] - mat_in[1][2]*mat_in[2][1];
const float m10 = mat_in[0][1]*mat_in[2][2] - mat_in[0][2]*mat_in[2][1];
const float m20 = mat_in[0][1]*mat_in[1][2] - mat_in[0][2]*mat_in[1][1];
const float m01 = mat_in[1][0]*mat_in[2][2] - mat_in[1][2]*mat_in[2][0];
const float m11 = mat_in[0][0]*mat_in[2][2] - mat_in[0][2]*mat_in[2][0];
const float m21 = mat_in[0][0]*mat_in[1][2] - mat_in[0][2]*mat_in[1][0];
const float m02 = mat_in[1][0]*mat_in[2][1] - mat_in[1][1]*mat_in[2][0];
const float m12 = mat_in[0][0]*mat_in[2][1] - mat_in[0][1]*mat_in[2][0];
const float m22 = mat_in[0][0]*mat_in[1][1] - mat_in[0][1]*mat_in[1][0];
const float det = mat_in[0][0]*m00 - mat_in[1][0]*m10 + mat_in[2][0]*m20;
if (fabs(det) > FLT_EPSILON) {
inv_out[0][0] = m00 / det;
inv_out[1][0] = -m01 / det;
inv_out[2][0] = m02 / det;
inv_out[0][1] = -m10 / det;
inv_out[1][1] = m11 / det;
inv_out[2][1] = -m12 / det;
inv_out[0][2] = m20 / det;
inv_out[1][2] = -m21 / det;
inv_out[2][2] = m22 / det;
return 0;
}
return 1;
}

/**
* @brief Multiply 3D matrix with vector
*
* @param vect_out output vector
* @param mat[3][3] Matrix input
* @param vect_in Vector input
*/
void float_mat3_mult(struct FloatVect3 *vect_out, float mat[3][3], struct FloatVect3 vect_in)
{
vect_out->x = mat[0][0] * vect_in.x + mat[0][1] * vect_in.y + mat[0][2] * vect_in.z;
vect_out->y = mat[1][0] * vect_in.x + mat[1][1] * vect_in.y + mat[1][2] * vect_in.z;
vect_out->z = mat[2][0] * vect_in.x + mat[2][1] * vect_in.y + mat[2][2] * vect_in.z;
}

/*
* 4x4 Matrix inverse.
* obtained from: http://rodolphe-vaillant.fr/?e=7
Expand Down
2 changes: 2 additions & 0 deletions sw/airborne/math/pprz_algebra_float.h
View file
Expand Up @@ -883,6 +883,8 @@ static inline void float_mat_diagonal_scal(float **o, float v, int n)

extern bool float_mat_inv_2d(float inv_out[4], float mat_in[4]);
extern void float_mat2_mult(struct FloatVect2 *vect_out, float mat[4], struct FloatVect2 vect_in);
extern bool float_mat_inv_3d(float inv_out[3][3], float mat_in[3][3]);
extern void float_mat3_mult(struct FloatVect3 *vect_out, float mat[3][3], struct FloatVect3 vect_in);
extern bool float_mat_inv_4d(float invOut[4][4], float mat_in[4][4]);

extern void float_vect3_bound_in_2d(struct FloatVect3 *vect3, float bound);
Expand Down
8 changes: 4 additions & 4 deletions sw/airborne/modules/ctrl/ctrl_effectiveness_scheduling.c
View file
Expand Up @@ -69,11 +69,11 @@ void ctrl_eff_scheduling_init(void)

void ctrl_eff_scheduling_periodic(void)
{
if(radio_control.values[INDI_FUNCTIONS_RC_CHANNEL] > 0) {
ctrl_eff_scheduling_periodic_b();
} else {
//if(radio_control.values[INDI_FUNCTIONS_RC_CHANNEL] > 0) {
// ctrl_eff_scheduling_periodic_b();
//} else {
ctrl_eff_scheduling_periodic_a();
}
//}

#ifdef INDI_THRUST_ON_PITCH_EFF
//State prioritization {W Roll, W pitch, W yaw, TOTAL THRUST}
Expand Down
132 changes: 132 additions & 0 deletions sw/airborne/modules/ctrl/eff_scheduling_falcon.c
View file
@@ -0,0 +1,132 @@
/*
* Copyright (C) 2023 Florian Sansou <florian.sansou@enac.fr>
* Copyright (C) 2023 Gautier Hattenberger <gautier.hattenberger.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/ctrl/eff_scheduling_falcon.c"
* Interpolation of control effectivenss matrix of the Falcon hybrid plane
*/

#include "modules/ctrl/eff_scheduling_falcon.h"
#include "firmwares/rotorcraft/stabilization/stabilization_indi.h"
#include "firmwares/rotorcraft/guidance/guidance_h.h"
#include "state.h"

#include "modules/datalink/downlink.h"

// Airspeed at which only with motor
#ifndef INDI_SCHEDULING_LOW_AIRSPEED
#define INDI_SCHEDULING_LOW_AIRSPEED 8.0f
#endif

static float g1g2_hover[INDI_OUTPUTS][INDI_NUM_ACT] = {
STABILIZATION_INDI_G1_ROLL,
STABILIZATION_INDI_G1_PITCH,
STABILIZATION_INDI_G1_YAW,
STABILIZATION_INDI_G1_THRUST
};

void ctrl_eff_scheduling_init(void)
{
for (int8_t i = 0; i < INDI_OUTPUTS; i++) {
for (int8_t j = 0; j < INDI_NUM_ACT; j++) {
g1g2[i][j] = g1g2_hover[i][j] / INDI_G_SCALING;
}
}
}

void ctrl_eff_scheduling_periodic(void)
{
// calculate squared airspeed
float airspeed = stateGetAirspeed_f();

if (airspeed > INDI_SCHEDULING_LOW_AIRSPEED) {
airspeed -= INDI_SCHEDULING_LOW_AIRSPEED; //offset for start eff at zero!
struct FloatEulers eulers_zxy;
float_eulers_of_quat_zxy(&eulers_zxy, stateGetNedToBodyQuat_f());

float pitch_ratio = 0.0f;
if (eulers_zxy.theta > -M_PI_4) {
pitch_ratio = 0.0f;
}
else {
pitch_ratio = fabsf(1.0f - eulers_zxy.theta/(float)(-M_PI_4));
}
Bound(pitch_ratio, 0.0f, 1.0f);


Bound(airspeed, 0.0f, 30.0f);
float airspeed2 = airspeed*airspeed;

// not effect of elevon on body roll axis
g1g2[0][4] = 0;
g1g2[0][5] = 0;

//g1g2[1][0] = 0;
//g1g2[1][1] = 0;
//g1g2[1][2] = 0;
//g1g2[1][3] = 0;
//g1g2[1][0] = g1g2_hover[1][0] / INDI_G_SCALING/10.;
//g1g2[1][1] = g1g2_hover[1][1] / INDI_G_SCALING/10.;
//g1g2[1][2] = g1g2_hover[1][2] / INDI_G_SCALING/10.;
//g1g2[1][3] = g1g2_hover[1][3] / INDI_G_SCALING/10.;
float pitch_eff = pitch_ratio * EFF_PITCH_A * airspeed2;
g1g2[1][4] = pitch_eff / 1000; // elevon_right
g1g2[1][5] = -pitch_eff / 1000; // elevon_left

float yaw_eff = pitch_ratio * EFF_YAW_A * airspeed2;
g1g2[2][4] = -yaw_eff / 1000; // elevon_right
g1g2[2][5] = -yaw_eff / 1000; // elevon_left

// No thrust generated by elevon, maybe take drag in accout for the future ?
g1g2[3][4] = 0;
g1g2[3][5] = 0;
}
else {
//Come back to motor control
g1g2[0][4] = 0; // elevon_left
g1g2[0][5] = 0; // elevon_right

//g1g2[1][0] = g1g2_hover[1][0] / INDI_G_SCALING;
//g1g2[1][1] = g1g2_hover[1][1] / INDI_G_SCALING;
//g1g2[1][2] = g1g2_hover[1][2] / INDI_G_SCALING;
//g1g2[1][3] = g1g2_hover[1][3] / INDI_G_SCALING;
g1g2[1][4] = 0; // elevon_left
g1g2[1][5] = 0; // elevon_right

g1g2[2][4] = 0; // elevon_left
g1g2[2][5] = 0; // elevon_right

g1g2[3][4] = 0; // elevon_left
g1g2[3][5] = 0; // elevon_right
}
}

extern void ctrl_eff_scheduling_report(void)
{
//float f[4] = { g1g2_forward[0][0], g1g2_forward[1][0], g1g2_forward[2][0], g1g2_forward[3][0]};
//g1g2[1][0], g1g2[1][1], g1g2[1][2], g1g2[1][3], g1g2[1][4], g1g2[1][5],
//g1g2[2][0], g1g2[2][1], g1g2[2][2], g1g2[2][3], g1g2[2][4], g1g2[2][5],
//g1g2[3][0], g1g2[3][1], g1g2[3][2], g1g2[3][3], g1g2[3][4], g1g2[3][5]

//float f[6] = { g1g2[0][0], g1g2[0][1], g1g2[0][2], g1g2[0][3], g1g2[0][4], g1g2[0][5] };
float f[6] = { g1g2[1][4], g1g2[1][5], g1g2[2][4], g1g2[2][5], g1g2[1][0], g1g2[2][0] };
DOWNLINK_SEND_PAYLOAD_FLOAT(DefaultChannel, DefaultDevice, 6, f);
}

33 changes: 33 additions & 0 deletions sw/airborne/modules/ctrl/eff_scheduling_falcon.h
View file
@@ -0,0 +1,33 @@
/*
* Copyright (C) 2023 Florian Sansou <florian.sansou@enac.fr>
* Copyright (C) 2023 Gautier Hattenberger <gautier.hattenberger.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/ctrl/eff_scheduling_falcon.h"
* Interpolation of control effectivenss matrix of the Falcon hybrid plane
*/

#ifndef EFF_SCHEDULING_FALCON_H
#define EFF_SCHEDULING_FALCON_H

extern void ctrl_eff_scheduling_init(void);
extern void ctrl_eff_scheduling_periodic(void);
extern void ctrl_eff_scheduling_report(void);

#endif // EFF_SCHEDULING_FALCON_H
3 changes: 2 additions & 1 deletion sw/airborne/modules/digital_cam/dc.c
View file
Expand Up @@ -190,7 +190,8 @@ void dc_send_command_common(uint8_t cmd __attribute__((unused)))
{
#if DC_SHOT_EXTRA_DL
uint8_t tab[] = { cmd };
DOWNLINK_SEND_PAYLOAD_COMMAND(extra_pprz_tp, EXTRA_DOWNLINK_DEVICE, 0, 1, tab);
uint8_t dst_id = 0;
DOWNLINK_SEND_PAYLOAD_COMMAND(extra_pprz_tp, EXTRA_DOWNLINK_DEVICE, &dst_id, 1, tab);
#endif
}

Expand Down
6 changes: 6 additions & 0 deletions sw/airborne/modules/imu/filter_1euro_imu.c
View file
Expand Up @@ -29,6 +29,7 @@
#include "math/pprz_algebra_float.h"
#include "modules/core/abi.h"
#include "generated/airframe.h"
#include "modules/imu/imu.h"

/** Enable by default */
#ifndef FILTER_1EURO_ENABLED
Expand Down Expand Up @@ -208,6 +209,11 @@ void filter_1euro_imu_init(void)
AbiBindMsgIMU_GYRO(IMU_F1E_BIND_ID, &gyro_ev, gyro_cb);
AbiBindMsgIMU_ACCEL(IMU_F1E_BIND_ID, &accel_ev, accel_cb);
AbiBindMsgIMU_MAG(IMU_F1E_BIND_ID, &mag_ev, mag_cb);

// force the creation of 1euro_imu in the imu structure
imu_get_gyro(IMU_F1E_ID, true);
imu_get_accel(IMU_F1E_ID, true);
imu_get_mag(IMU_F1E_ID, true);
}

/**
Expand Down
16 changes: 15 additions & 1 deletion sw/airborne/modules/ins/ins_ekf2.cpp
View file
Expand Up @@ -522,7 +522,7 @@ void ins_ekf2_init(void)

/* Initialize the origin from flight plan */
#if USE_INS_NAV_INIT
if(ekf.setEkfGlobalOrigin(NAV_LAT0*1e-7, NAV_LON0*1e-7, (NAV_ALT0 + NAV_MSL0)*1e-3))
if (ekf.setEkfGlobalOrigin(NAV_LAT0*1e-7, NAV_LON0*1e-7, (NAV_ALT0 + NAV_MSL0)*1e-3))
{
struct LlaCoor_i llh_nav0; /* Height above the ellipsoid */
llh_nav0.lat = NAV_LAT0;
Expand Down Expand Up @@ -565,6 +565,20 @@ void ins_ekf2_init(void)
AbiBindMsgOPTICAL_FLOW(INS_EKF2_OF_ID, &optical_flow_ev, optical_flow_cb);
}

void ins_reset_local_origin(void)
{
#if USE_GPS
if (GpsFixValid()) {
struct LlaCoor_i lla_pos = lla_int_from_gps(&gps);
if (ekf.setEkfGlobalOrigin(lla_pos.lat*1e-7, lla_pos.lon*1e-7, gps.hmsl*1e-3)) {
ltp_def_from_lla_i(&ekf2.ltp_def, &lla_pos);
ekf2.ltp_def.hmsl = gps.hmsl;
stateSetLocalOrigin_i(&ekf2.ltp_def);
}
}
#endif
}

/* Update the INS state */
void ins_ekf2_update(void)
{
Expand Down
2 changes: 1 addition & 1 deletion sw/airborne/modules/loggers/logger_control_effectiveness.c
View file
Expand Up @@ -129,7 +129,7 @@ void logger_control_effectiveness_periodic(void)
// log actuators
#if LOGGER_CONTROL_EFFECTIVENESS_ACTUATORS
for (unsigned int i = 0; i < ACTUATORS_NB; i++) {
sdLogWriteLog(pprzLogFile, ",%d", actuators[i]);
sdLogWriteLog(pprzLogFile, ",%d", actuators_pprz[i]);
}
#endif

Expand Down