Skip to content

Commit

Permalink
GVF firmware adjusted to work with GVF Viewer (#2893)
Browse files Browse the repository at this point in the history
  • Loading branch information
@jesusBV20
jesusBV20 committed Aug 31, 2022
1 parent 962930b commit 9098b7c
Show file tree
Hide file tree
Showing 18 changed files with 349 additions and 198 deletions.
8 changes: 6 additions & 2 deletions conf/airframes/UCM/rover_steering.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -44,8 +44,12 @@
<configure name="USE_MAGNETOMETER" value="0"/>
</module-->

<module name="nav" type="rover_base"/>
<module name="gvf" type="module"/>
<module name="nav" type="rover_base">
<define name="ROVER_BASE_SEND_TRAJECTORY" value="FALSE"/>
</module>
<module name="gvf" type="module">
<define name="GVF_OCAML_GCS" value="FALSE"/>
</module>

<module name="guidance" type="rover_steering">
<define name="MAX_DELTA" value="15.0"/>
Expand Down
6 changes: 3 additions & 3 deletions conf/autopilot/rover_steering.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,9 +10,9 @@
<include name="autopilot_rc_helpers.h"/>
<include name="modules/gps/gps.h"/>
<include name="modules/actuators/actuators.h"/>
<include name="navigation.h"/>
<include name="modules/radio_control/radio_control.h"/>
<include name="guidance/rover_guidance_steering.h"/>
<include name="modules/guidance/gvf/gvf.h"/>
<include name="navigation.h"/>
<include name="state.h"/>
<define name="RCLost()" value="(radio_control.status == RC_REALLY_LOST)"/>
</includes>
Expand Down Expand Up @@ -77,7 +77,7 @@
</control>
<control>
<call fun="rover_guidance_steering_speed_ctrl()"/>
<call fun="rover_guidance_steering_heading_ctrl()"/>
<call fun="rover_guidance_steering_heading_ctrl(gvf_control.omega)"/>

<call fun="commands[COMMAND_STEERING] = GetCmdFromDelta(guidance_control.cmd.delta);"/>
<call fun="commands[COMMAND_THROTTLE] = GetCmdFromThrottle(guidance_control.throttle);"/>
Expand Down
119 changes: 119 additions & 0 deletions conf/flight_plans/UCM/fixedwing_gvfMission.xml
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,119 @@
<!DOCTYPE flight_plan SYSTEM "../flight_plan.dtd">

<flight_plan alt="750" ground_alt="650" lat0="40.450631" lon0="-3.726058" max_dist_from_home="1500" name="Rover Steering" security_height="25" home_mode_height="200" qfu="90" geofence_sector="FlightArea" geofence_max_alt="3000" geofence_max_height="1000">
<header>

</header>
<waypoints>
<waypoint alt="700.0" name="HOME" x="0.0" y="0.0"/>
<waypoint alt="700.0" name="STDBY" x="0" y="-4"/>
<waypoint alt="700.0" name="ELLIPSE" x="-30" y="-30"/>
<waypoint alt="700.0" name="CLIMB" x="-30" y="-40"/>
<waypoint alt="700.0" name="P1" x="-20" y="-40"/>
<waypoint alt="700.0" name="P2" x="20" y="-40"/>

<!-- Sectors waypoints -->
<waypoint name="_S1" x="3" y="4"/>
<waypoint name="_S2" x="3" y="-4"/>
<waypoint name="_S3" x="-3" y="-4"/>
<waypoint name="_S4" x="-3" y="4"/>
<waypoint name="_N1" x="1000" y="1000"/>
<waypoint name="_N2" x="1000" y="-1000"/>
<waypoint name="_N3" x="-1000" y="-1000"/>
<waypoint name="_N4" x="-1000" y="1000"/>
</waypoints>

<sectors>
<sector name="FlightArea" color="red">
<corner name="_N1"/>
<corner name="_N2"/>
<corner name="_N3"/>
<corner name="_N4"/>
</sector>

<!--sector name="Survey" color="green">
<corner name="_S1"/>
<corner name="_S2"/>
<corner name="_S3"/>
<corner name="_S4"/>
</sector-->
</sectors>

<variables>
<variable var="a_stb" init="60.0" min="1.0" max="150.0" step="1.0"/>
<variable var="b_stb" init="60.0" min="1.0" max="150.0" step="1.0"/>
</variables>

<modules>
<module name="gvf_module">
<define name="GVF_OCAML_GCS" value="FALSE"/>
</module>
<module name="gvf_parametric"/>
</modules>

<blocks>
<block name="Wait GPS" strip_icon="gps.png">
<set value="1" var="autopilot.kill_throttle"/>
<while cond="!GpsFixValid()"/>
</block>

<block name="Geo init" strip_icon="googleearth.png">
<call_once fun="NavSetGroundReferenceHere()"/>
</block>

<block name="Holding point">
<set value="1" var="autopilot.kill_throttle"/>
<attitude roll="0" throttle="0" vmode="throttle"/>
</block>

<!-- need this for simulation, manual takeoff for real testing -->
<block key="t" name="Takeoff" strip_button="Takeoff (wp CLIMB)" strip_icon="takeoff.png" group="home">
<exception cond="GetPosAlt() @GT (ground_alt+100)" deroute="Standby"/>
<set value="0" var="autopilot.kill_throttle"/>
<set value="1" var="autopilot.launch"/>
<set value="0" var="autopilot.flight_time"/>
<go from="HOME" throttle="1.0" vmode="throttle" wp="CLIMB" pitch="15"/>
</block>

<block key="s" name="Standby" strip_button="Standby" strip_icon="home.png" group="home">
<call fun="gvf_ellipse_wp(WP_ELLIPSE, a_stb, b_stb, gvf_ellipse_par.alpha)"/>
</block>

<block name="line_to_HOME" strip_icon="home_drop.png">
<call fun="gvf_segment_XY1_XY2(GetPosX(), GetPosY(), 0.f, 0.f)"/>
</block>

<block name="ellipse_wp" strip_icon="oval.png">
<call fun="gvf_ellipse_wp(WP_ELLIPSE, gvf_ellipse_par.a, gvf_ellipse_par.b, gvf_ellipse_par.alpha)"/>
</block>

<block name="gvf_ellipse_XY" strip_icon="oval.png">
<call fun="gvf_ellipse_XY(10.1, 10.1, gvf_ellipse_par.a, gvf_ellipse_par.b, gvf_ellipse_par.alpha)"/>
</block>

<block name="line_P1_P2" strip_icon="line.png">
<call fun="gvf_line_wp1_wp2(WP_P1, WP_P2)"/>
</block>

<block name="segment_turn_P1_P2" strip_icon="line.png">
<call fun="gvf_segment_loop_wp1_wp2(WP_P1, WP_P2, gvf_segment_par.d1, gvf_segment_par.d2)"/>
</block>

<block name="sin_p1_p2" strip_icon="line_drop.png">
<call fun="gvf_sin_wp1_wp2(WP_P1, WP_P2, gvf_sin_par.w, gvf_sin_par.off, gvf_sin_par.A)"/>
</block>

<block name="2D_trefoil_wp" strip_icon="eight.png">
<call fun="gvf_parametric_2D_trefoil_wp(WP_P1, gvf_parametric_2d_trefoil_par.w1, gvf_parametric_2d_trefoil_par.w2, gvf_parametric_2d_trefoil_par.ratio, gvf_parametric_2d_trefoil_par.r, gvf_parametric_2d_trefoil_par.alpha)"/>
</block>

<block name="3D_ellipse_wp" strip_icon="oval.png">
<call fun="gvf_parametric_3D_ellipse_wp(WP_ELLIPSE, gvf_parametric_3d_ellipse_par.r, gvf_parametric_3d_ellipse_par.zl, gvf_parametric_3d_ellipse_par.zh, gvf_parametric_3d_ellipse_par.alpha)"/>
</block>

<block name="3D_lissajous_wp" strip_icon="eight.png">
<call fun="gvf_parametric_3D_lissajous_wp_center(WP_P2, 100, gvf_parametric_3d_lissajous_par.cx, gvf_parametric_3d_lissajous_par.cy, gvf_parametric_3d_lissajous_par.cz, gvf_parametric_3d_lissajous_par.wx, gvf_parametric_3d_lissajous_par.wy, gvf_parametric_3d_lissajous_par.wz, gvf_parametric_3d_lissajous_par.dx, gvf_parametric_3d_lissajous_par.dy, gvf_parametric_3d_lissajous_par.dz, gvf_parametric_3d_lissajous_par.alpha)"/>
</block>

</blocks>
</flight_plan>
47 changes: 23 additions & 24 deletions conf/flight_plans/UCM/steering_rover_gvfMission.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,10 +17,10 @@
<waypoint name="_S2" x="3" y="-4"/>
<waypoint name="_S3" x="-3" y="-4"/>
<waypoint name="_S4" x="-3" y="4"/>
<waypoint name="_N1" x="40" y="40"/>
<waypoint name="_N2" x="40" y="-40"/>
<waypoint name="_N3" x="-40" y="-40"/>
<waypoint name="_N4" x="-40" y="40"/>
<waypoint name="_N1" x="100" y="100"/>
<waypoint name="_N2" x="100" y="-100"/>
<waypoint name="_N3" x="-100" y="-100"/>
<waypoint name="_N4" x="-100" y="100"/>
</waypoints>

<sectors>
Expand All @@ -40,56 +40,55 @@
</sectors>

<variables>
<variable var="a" init="5.0" min="1.0" max="10.0" step="1.0"/>
<variable var="b" init="5.0" min="1.0" max="10.0" step="1.0"/>
<variable var="a_stb" init="5.0" min="1.0" max="20.0" step="1.0"/>
<variable var="b_stb" init="5.0" min="1.0" max="20.0" step="1.0"/>
</variables>

<modules>
<module name="gvf_module"/>
</modules>

<blocks>
<block name="Wait GPS">
<block name="Wait GPS" strip_icon="gps.png">
<call_once fun="NavKillThrottle()"/>
<while cond="!GpsFixValid()"/>
</block>

<block name="Geo init">
<block name="Geo init" strip_icon="googleearth.png">
<while cond="LessThan(NavBlockTime(), 10)"/>
<call_once fun="NavSetGroundReferenceHere()"/>
<call_once fun="NavSetAltitudeReferenceHere()"/>
</block>

<block name="Start Engine">
<block name="Start Engine" strip_icon="on.png">
<while cond="LessThan(NavBlockTime(), 1)"/>
<call_once fun="autopilot_set_motors_on(TRUE)"/>
</block>

<block name="Standby" strip_button="Standby" strip_icon="home.png">
<call fun="gvf_ellipse_wp(WP_STDBY, 3, 3, 0)"/>
<call fun="gvf_ellipse_wp(WP_STDBY, a_stb, b_stb, 0)"/>
</block>

<block name="ellipse_wp">
<call fun="gvf_ellipse_wp(WP_ELLIPSE, a, b, gvf_ellipse_par.alpha)"/>
<block name="line_to_HOME" strip_icon="home_drop.png">
<call fun="gvf_segment_XY1_XY2(GetPosX(), GetPosY(), 0.f, 0.f)"/>
<exception cond="! InsideNet(GetPosX(), GetPosY())" deroute="Standby"/>
</block>

<block name="line_P1_P2_turn">
<call fun="gvf_segment_loop_wp1_wp2(WP_P1, WP_P2, gvf_segment_par.d1, gvf_segment_par.d2)"/>
<block name="ellipse_wp" strip_icon="oval.png">
<call fun="gvf_ellipse_wp(WP_ELLIPSE, gvf_ellipse_par.a, gvf_ellipse_par.b, gvf_ellipse_par.alpha)"/>
<exception cond="! InsideNet(GetPosX(), GetPosY())" deroute="Standby"/>
</block>
<block name="line_to_HOME">
<call fun="gvf_segment_XY1_XY2(GetPosX(), GetPosY(), 0.f, 0.f)"/>

<block name="line_P1_P2" strip_icon="line.png">
<call fun="gvf_line_wp1_wp2(WP_P1, WP_P2)"/>
<exception cond="! InsideNet(GetPosX(), GetPosY())" deroute="Standby"/>
</block>

<block name="sin_p1_p2">
<call fun="gvf_sin_wp1_wp2(WP_P1, WP_P2, 10*gvf_sin_par.w, gvf_sin_par.off, gvf_sin_par.A)"/>
<block name="segment_turn_P1_P2" strip_icon="line.png">
<call fun="gvf_segment_loop_wp1_wp2(WP_P1, WP_P2, gvf_segment_par.d1, gvf_segment_par.d2)"/>
<exception cond="! InsideNet(GetPosX(), GetPosY())" deroute="Standby"/>
</block>


<block name="sin_p1_p2" strip_icon="line_drop.png">
<call fun="gvf_sin_wp1_wp2(WP_P1, WP_P2, gvf_sin_par.w, gvf_sin_par.off, gvf_sin_par.A)"/>
<exception cond="! InsideNet(GetPosX(), GetPosY())" deroute="Standby"/>
</block>

</blocks>
</flight_plan>
85 changes: 0 additions & 85 deletions conf/flight_plans/UCM/test_ucm_fisicas.xml
View file

This file was deleted.

8 changes: 3 additions & 5 deletions conf/telemetry/default_fixedwing_gvf.xml → conf/telemetry/GVF/gvf_default_fixedwing.xml
View file
Original file line number Diff line number Diff line change
@@ -1,5 +1,5 @@
<?xml version="1.0"?>
<!DOCTYPE telemetry SYSTEM "telemetry.dtd">
<!DOCTYPE telemetry SYSTEM "../telemetry.dtd">
<telemetry>
<process name="Ap">
<mode name="default">
Expand All @@ -12,9 +12,7 @@
<message name="ESTIMATOR" period="0.5"/>
<message name="ENERGY" period="1.1"/>
<message name="WP_MOVED" period="0.5"/>
<message name="CIRCLE" period="1.05"/>
<message name="DESIRED" period="1.05"/>
<message name="SEGMENT" period="1.2"/>
<message name="CALIBRATION" period="2.1"/>
<message name="NAVIGATION_REF" period="9."/>
<message name="PPRZ_MODE" period="5."/>
Expand All @@ -31,8 +29,8 @@
<message name="FBW_STATUS" period="2"/>
<message name="AIR_DATA" period="1.3"/>
<message name="VECTORNAV_INFO" period="0.5"/>
<message name="GVF" period="1"/>
<message name="GVF_PARAMETRIC" period="1"/>
<message name="GVF" period="3."/>
<message name="GVF_PARAMETRIC" period="3."/>
</mode>
<mode name="minimal">
<message name="ALIVE" period="5"/>
Expand Down

0 comments on commit 9098b7c

Please sign in to comment.