Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-02 05:17:03 +08:00
Code Issues Actions 24 Packages Projects Releases Wiki Activity

[rover] improve support for ROVER firmware (#3530)
Issues due date / Add labels to issues (push) Has been cancelled
Details
Doxygen / build (push) Has been cancelled
Details

Browse Source 
- possibility to use basic PID with steering rover
- fix navigation
- add support for mission mode
- fix simulator (orientation was wrong, leading to opposite steering
  between sim and reality)
- add support of 2 wheels rover in simulation
- add and update config files
This commit is contained in:
Gautier Hattenberger
2025-09-30 08:54:06 +02:00
committed by GitHub
parent e9a8673f97
commit 41048101d0
18 changed files with 1011 additions and 218 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/ENAC/rover/rover_2_wheels.xml
+8 -5
View File
@@ -1,4 +1,4 @@
<!DOCTYPE airframe SYSTEM "../airframe.dtd"> <!DOCTYPE airframe SYSTEM "../../airframe.dtd">
<airframe name="Rover"> <airframe name="Rover">
@@ -16,6 +16,11 @@
<target name="ap" board="chimera_1.0"> <target name="ap" board="chimera_1.0">
</target> </target>
<target name="nps" board="pc">
<module name="fdm" type="rover"/> <!-- NPS_FDM rover physics!! -->
<define name="NPS_BYPASS_AHRS" value="TRUE"/>
<define name="NPS_BYPASS_INS" value="TRUE"/>
</target>
<module name="radio_control" type="datalink"> <module name="radio_control" type="datalink">
<!--define name="RADIO_KILL_SWITCH" value="RADIO_GAIN1"/--> <!--define name="RADIO_KILL_SWITCH" value="RADIO_GAIN1"/-->
@@ -98,6 +103,7 @@
<define name="MODE_MANUAL" value="AP_MODE_DIRECT"/> <!-- for compilation --> <define name="MODE_MANUAL" value="AP_MODE_DIRECT"/> <!-- for compilation -->
<define name="MODE_AUTO1" value="AP_MODE_DIRECT"/> <!-- for compilation --> <define name="MODE_AUTO1" value="AP_MODE_DIRECT"/> <!-- for compilation -->
<define name="MODE_AUTO2" value="AP_MODE_NAV"/> <define name="MODE_AUTO2" value="AP_MODE_NAV"/>
<define name="MODE_STARTUP" value="AP_MODE_NAV"/>
</section> </section>
<section name="BAT"> <section name="BAT">
@@ -111,10 +117,7 @@
</section> </section>
<section name="GCS"> <section name="GCS">
<define name="ALT_SHIFT_PLUS_PLUS" value="5"/> <define name="AC_ICON" value="rover"/>
<define name="ALT_SHIFT_PLUS" value="1"/>
<define name="ALT_SHIFT_MINUS" value="-1"/>
<define name="AC_ICON" value="quadrotor_x"/>
</section> </section>
</airframe> </airframe>
conf/airframes/ENAC/rover/rover_e_maxx.xml
+147
View File
@@ -0,0 +1,147 @@
<!DOCTYPE airframe SYSTEM "../../airframe.dtd">
<airframe name="Rover E Maxx">
<firmware name="rover">
<autopilot name="rover_steering_pid"/>
<configure name="PERIODIC_FREQUENCY" value="150"/>
<target name="ap" board="tawaki_1.0">
<module name="radio_control" type="sbus"/>
</target>
<target name="nps" board="pc">
<module name="radio_control" type="datalink"/>
<module name="fdm" type="rover"/>
<define name="NPS_BYPASS_AHRS" value="TRUE"/>
<define name="NPS_BYPASS_INS" value="TRUE"/>
<!-- Multiple agents simulation -->
<!--configure name="MODEM_PORT_OUT" value="4246"/>
<configure name="MODEM_PORT_IN" value="4247"/-->
</target>
<module name="actuators" type="pwm"/>
<module name="telemetry" type="xbee_api"/>
<module name="actuators" type="sbus">
<configure name="ACTUATORS_SBUS_DEV" value="UART4"/>
</module>
<module name="power_switch" >
<define name="POWER_SWITCH_GPIO" value="GPIOA,GPIO1"/>
</module>
<module name="gimbal" type="caddx_gm3"/>
<module name="joystick"/>
<module name="board_tawaki" />
<module name="gps" type="ublox">
<configure name="GPS_BAUD" value="B115200"/>
<define name="GPS_POS_BROADCAST"/>
</module>
<module name="ins"/>
<module name="ahrs" type="int_cmpl_quat">
<define name="AHRS_HEADING_UPDATE_GPS_MIN_SPEED" value="0.1"/>
<define name="AHRS_USE_GPS_HEADING" value="TRUE"/>
<configure name="USE_MAGNETOMETER" value="0"/>
</module>
</firmware>
<!-- COMMANDS SECTION ..................................................................... -->
<servos driver="Pwm">
<!-- "no" 1 and 2 but no 0 because our board don't have S0 actuator entry... -->
<servo name="MOTOR_THROTTLE" no="1" min="1410" neutral="1510" max="1730"/>
<servo name="MOTOR_STEERING" no="2" min="942" neutral="1436" max="1944"/>
</servos>
<!-- gimbal -->
<servos driver="Sbus">
<servo name="GIMBAL_CADDX_MODE" no="0" min="172" neutral="992" max="1811"/>
<servo name="GIMBAL_CADDX_ROLL" no="1" min="800" neutral="1005" max="1265"/>
<servo name="GIMBAL_CADDX_TILT" no="2" min="655" neutral="1100" max="1538"/>
<servo name="GIMBAL_CADDX_PAN" no="3" min="428" neutral="1019" max="1610"/>
<servo name="GIMBAL_CADDX_SENS" no="4" min="172" neutral="992" max="1811"/>
</servos>
<!-- Low level commands (PWM signal) -->
<commands>
<axis name="THROTTLE" failsafe_value="0"/>
<axis name="STEERING" failsafe_value="0"/>
<axis name="CAM_PAN" failsafe_value="0"/>
<axis name="CAM_TILT" failsafe_value="0"/>
</commands>
<!-- When SetCommandsFromRC -->
<rc_commands>
<set command="THROTTLE" value="@THROTTLE"/>
<set command="STEERING" value="@ROLL"/>
</rc_commands>
<!-- When SetActuatorsFromCommands-->
<command_laws>
<set servo="MOTOR_THROTTLE" value="@THROTTLE"/>
<set servo="MOTOR_STEERING" value="@STEERING"/>
<set servo="GIMBAL_CADDX_PAN" value="@CAM_PAN"/>
<set servo="GIMBAL_CADDX_TILT" value="@CAM_TILT"/>
</command_laws>
<!-- SECTIONS: General config ............................................................. -->
<section name="IMU" prefix="IMU_">
<define name="BODY_TO_IMU_PHI" value="0." unit="deg"/>
<define name="BODY_TO_IMU_THETA" value="0." unit="deg"/>
<define name="BODY_TO_IMU_PSI" value="90." unit="deg"/>
</section>
<section name="AHRS" prefix="AHRS_">
<!-- values used if no GPS fix (i.e NPS), on 3D fix is update by geo_mag module if loaded -->
<!-- Toulouse -->
<define name="H_X" value="0.513081"/>
<define name="H_Y" value="-0.00242783"/>
<define name="H_Z" value="0.858336"/>
<define name="USE_GPS" value="TRUE"/>
</section>
<section name="ROVER_GUIDANCE_STEERING">
<define name="MAX_DELTA" value="30.0"/>
<define name="DRIVE_SHAFT_DISTANCE" value="0.5"/>
<define name="SR_MEASURED_KF" value="1920"/>
<define name="ROVER_GUIDANCE_MAX_SPEED" value="5.0"/>
<define name="ROVER_GUIDANCE_POS_KP" value="0.6"/>
<define name="ROVER_GUIDANCE_HEADING_KP" value="0.8"/>
<define name="ROVER_GUIDANCE_SPEED_KP" value="10.0"/>
<define name="ROVER_GUIDANCE_SPEED_KI" value="10.0"/>
<define name="ROVER_GUIDANCE_PROXIMITY_DISTANCE" value="3.0"/>
</section>
<section name="AUTOPILOT">
<define name="MODE_MANUAL" value="AP_MODE_KILL"/>
<define name="MODE_AUTO1" value="AP_MODE_DIRECT"/>
<define name="MODE_AUTO2" value="AP_MODE_NAV"/>
<define name="MODE_STARTUP" value="AP_MODE_NAV"/>
</section>
<section name="SIMULATOR" prefix="NPS_">
<define name="ROVER_FRICTION" value="0.5"/>
<define name="ROVER_ACCELERATION" value="2.5"/>
<define name="USE_COMMANDS" value="TRUE"/>
</section>
<section name="BAT">
<define name="CATASTROPHIC_BAT_LEVEL" value="12.3" unit="V"/>
<define name="CRITIC_BAT_LEVEL" value="13" unit="V"/>
<define name="LOW_BAT_LEVEL" value="13.5" unit="V"/>
<define name="MAX_BAT_LEVEL" value="17" unit="V"/>
<define name="BAT_NB_CELLS" value="4"/>
</section>
<section name="GCS">
<define name="AC_ICON" value="rover"/>
</section>
</airframe>
conf/autopilot/rover.xml
+10 -8
View File
@@ -4,16 +4,18 @@
<state_machine name="ap" freq="PERIODIC_FREQUENCY" gcs_mode="true" settings_mode="true" settings_handler="autopilot_generated|SetModeHandler"> <state_machine name="ap" freq="PERIODIC_FREQUENCY" gcs_mode="true" settings_mode="true" settings_handler="autopilot_generated|SetModeHandler">
<modules>
<module name="nav" type="rover_base"/>
<module name="guidance" type="rover"/>
</modules>
<includes> <includes>
<include name="generated/airframe.h"/> <include name="generated/airframe.h"/>
<include name="autopilot.h"/> <include name="autopilot.h"/>
<include name="autopilot_rc_helpers.h"/> <include name="autopilot_rc_helpers.h"/>
<include name="modules/gps/gps.h"/> <include name="modules/gps/gps.h"/>
<include name="modules/actuators/actuators.h"/> <include name="modules/actuators/actuators.h"/>
<include name="navigation.h"/>
<include name="guidance/rover_guidance.h"/>
<include name="modules/radio_control/radio_control.h"/> <include name="modules/radio_control/radio_control.h"/>
<define name="RCLost()" value="(radio_control.status == RC_REALLY_LOST)"/>
</includes> </includes>
<settings> <settings>
@@ -33,7 +35,7 @@
<control> <control>
<call fun="SetCommandsFromRC(commands, radio_control.values)"/> <call fun="SetCommandsFromRC(commands, radio_control.values)"/>
</control> </control>
<exception cond="RCLost()" deroute="KILL"/> <exception cond="RadioControlIsLost()" deroute="KILL"/>
</mode> </mode>
<mode name="NAV"> <mode name="NAV">
@@ -83,8 +85,8 @@
<select cond="nav.mode == NAV_MODE_MANUAL"/> <select cond="nav.mode == NAV_MODE_MANUAL"/>
<control> <control>
<!-- copy manual nav controls to commands --> <!-- copy manual nav controls to commands -->
<call fun="commands[COMMAND_SPEED] = TRIM_PPRZ(nav.speed * MAX_PPRZ)"/> <call fun="command_set(COMMAND_SPEED, TRIM_PPRZ(nav.speed * MAX_PPRZ))"/>
<call fun="commands[COMMAND_TURN] = TRIM_PPRZ(nav.turn * MAX_PPRZ)"/> <call fun="command_set(COMMAND_TURN, TRIM_PPRZ(nav.turn * MAX_PPRZ))"/>
<call fun="autopilot.throttle = commands[COMMAND_SPEED]"/> <call fun="autopilot.throttle = commands[COMMAND_SPEED]"/>
</control> </control>
</mode> </mode>
@@ -94,8 +96,8 @@
<control> <control>
<call fun="VECT2_COPY(rover_guidance.sp.pos, nav.carrot)"/> <call fun="VECT2_COPY(rover_guidance.sp.pos, nav.carrot)"/>
<call fun="rover_guidance_run(&nav.heading)"/> <call fun="rover_guidance_run(&nav.heading)"/>
<call fun="commands[COMMAND_SPEED] = TRIM_PPRZ(rover_guidance.cmd.motor_speed)"/> <call fun="command_set(COMMAND_SPEED, TRIM_PPRZ(rover_guidance.cmd.motor_speed))"/>
<call fun="commands[COMMAND_TURN] = TRIM_PPRZ(rover_guidance.cmd.motor_turn)"/> <call fun="command_set(COMMAND_TURN, TRIM_PPRZ(rover_guidance.cmd.motor_turn))"/>
<call fun="autopilot.throttle = commands[COMMAND_SPEED]"/> <call fun="autopilot.throttle = commands[COMMAND_SPEED]"/>
</control> </control>
</mode> </mode>
conf/autopilot/rover_steering.xml
+7 -3
View File
@@ -2,7 +2,13 @@
<autopilot name="Steering Rover Autopilot"> <autopilot name="Steering Rover Autopilot">
<state_machine name="ap" freq="PERIODIC_FREQUENCY" gcs_mode="true" settings_mode="true"> <state_machine name="ap" freq="PERIODIC_FREQUENCY" gcs_mode="true" settings_mode="true" settings_handler="autopilot_generated|SetModeHandler">
<modules>
<module name="nav" type="rover_base"/>
<module name="guidance" type="rover_steering"/>
<module name="gvf_common"/>
</modules>
<includes> <includes>
<include name="generated/airframe.h"/> <include name="generated/airframe.h"/>
@@ -11,8 +17,6 @@
<include name="modules/gps/gps.h"/> <include name="modules/gps/gps.h"/>
<include name="modules/actuators/actuators.h"/> <include name="modules/actuators/actuators.h"/>
<include name="modules/radio_control/radio_control.h"/> <include name="modules/radio_control/radio_control.h"/>
<include name="modules/guidance/gvf_common.h"/>
<include name="navigation.h"/>
<include name="state.h"/> <include name="state.h"/>
<define name="RCLost()" value="(radio_control.status == RC_REALLY_LOST)"/> <define name="RCLost()" value="(radio_control.status == RC_REALLY_LOST)"/>
</includes> </includes>
conf/autopilot/rover_steering_cruise.xml
+7 -3
View File
@@ -2,7 +2,13 @@
<autopilot name="Steering Rover Autopilot"> <autopilot name="Steering Rover Autopilot">
<state_machine name="ap" freq="PERIODIC_FREQUENCY" gcs_mode="true" settings_mode="true"> <state_machine name="ap" freq="PERIODIC_FREQUENCY" gcs_mode="true" settings_mode="true" settings_handler="autopilot_generated|SetModeHandler">
<modules>
<module name="nav" type="rover_base"/>
<module name="guidance" type="rover_steering"/>
<module name="gvf_common"/>
</modules>
<includes> <includes>
<include name="generated/airframe.h"/> <include name="generated/airframe.h"/>
@@ -11,8 +17,6 @@
<include name="modules/gps/gps.h"/> <include name="modules/gps/gps.h"/>
<include name="modules/actuators/actuators.h"/> <include name="modules/actuators/actuators.h"/>
<include name="modules/radio_control/radio_control.h"/> <include name="modules/radio_control/radio_control.h"/>
<include name="modules/guidance/gvf_common.h"/>
<include name="navigation.h"/>
<include name="state.h"/> <include name="state.h"/>
<define name="RCLost()" value="(radio_control.status == RC_REALLY_LOST)"/> <!-- TODO: define it in nav.h?? --> <define name="RCLost()" value="(radio_control.status == RC_REALLY_LOST)"/> <!-- TODO: define it in nav.h?? -->
conf/autopilot/rover_steering_pid.xml
+129
View File
@@ -0,0 +1,129 @@
<!DOCTYPE autopilot SYSTEM "autopilot.dtd">
<autopilot name="Steering Rover Autopilot">
<state_machine name="ap" freq="PERIODIC_FREQUENCY" gcs_mode="true" settings_mode="true" settings_handler="autopilot_generated|SetModeHandler">
<modules>
<module name="nav" type="rover_base"/>
<module name="guidance" type="rover_steering"/>
</modules>
<includes>
<include name="generated/airframe.h"/>
<include name="autopilot.h"/>
<include name="autopilot_rc_helpers.h"/>
<include name="modules/gps/gps.h"/>
<include name="modules/actuators/actuators.h"/>
<include name="modules/radio_control/radio_control.h"/>
</includes>
<settings>
<dl_setting var="autopilot.kill_throttle" min="0" step="1" max="1" module="autopilot" values="Resurrect|Kill" handler="KillThrottle"/>
</settings>
<mode_selection>
<mode_select cond="RCMode0()" mode="MODE_MANUAL"/>
<mode_select cond="RCMode1()" mode="MODE_AUTO1"/>
<mode_select cond="RCMode2()" mode="MODE_AUTO2" exception="HOME"/>
</mode_selection>
<exceptions>
<exception cond="nav.too_far_from_home" deroute="HOME"/>
<exception cond="kill_switch_is_on()" deroute="KILL"/>
</exceptions>
<mode name="DIRECT" shortname="MANUAL">
<control freq="NAVIGATION_FREQUENCY"> <!-- Only for display -->
<call fun="nav_periodic_task()"/>
</control>
<control>
<call fun="SetCommandsFromRC(commands, radio_control.values)"/>
</control>
<exception cond="RadioControlIsLost()" deroute="KILL"/>
</mode>
<!-- Cruiser: Speed ctrl (from state speed on enter) + Heading from RC -->
<mode name="CRUISER" shortname="AUTO1">
<on_enter>
<call fun="guidance_control.cmd.speed = stateGetHorizontalSpeedNorm_f()"/>
</on_enter>
<control freq="NAVIGATION_FREQUENCY">
<call fun="nav_periodic_task()"/>
</control>
<control>
<call fun="rover_guidance_steering_speed_ctrl()"/>
<call fun="SetCommandsFromRC(commands, radio_control.values)"/>
<call fun="command_set(COMMAND_THROTTLE, GetCmdFromThrottle(guidance_control.throttle))"/>
<call fun="autopilot.throttle = commands[COMMAND_THROTTLE]"/>
</control>
<exception cond="RadioControlIsLost()" deroute="KILL"/>
</mode>
<mode name="NAV">
<control freq="NAVIGATION_FREQUENCY">
<call fun="nav_periodic_task()"/>
</control>
<control>
<call fun="rover_guidance_steering_periodic()"/>
</control>
<exception cond="GpsIsLost() && autopilot_in_flight()" deroute="KILL"/>
</mode>
<mode name="HOME">
<control freq="NAVIGATION_FREQUENCY">
<call fun="nav_home()"/>
</control>
<control>
<call fun="rover_guidance_steering_periodic()"/>
</control>
<exception cond="GpsIsLost()" deroute="KILL"/>
</mode>
<mode name="KILL">
<select cond="kill_switch_is_on()"/>
<on_enter>
<call fun="autopilot_set_in_flight(false)"/>
<call fun="autopilot_set_motors_on(false)"/>
</on_enter>
<control>
<call fun="rover_guidance_steering_kill()"/>
<call fun="SetCommands(commands_failsafe)"/>
<call fun="autopilot.throttle = commands[COMMAND_THROTTLE]"/>
</control>
</mode>
</state_machine>
<state_machine name="guidance" freq="PERIODIC_FREQUENCY">
<includes>
<include name="navigation.h"/>
</includes>
<mode name="NAV_DIRECT">
<select cond="nav.mode == NAV_MODE_MANUAL"/>
<control>
<!-- copy manual nav controls to commands -->
<call fun="command_set(COMMAND_THROTTLE, TRIM_PPRZ(nav.speed * MAX_PPRZ))"/>
<call fun="command_set(COMMAND_STEERING, TRIM_PPRZ(nav.turn * MAX_PPRZ))"/>
<call fun="autopilot.throttle = commands[COMMAND_THROTTLE]"/>
</control>
</mode>
<mode name="NAV_AUTO">
<select cond="nav.mode != NAV_MODE_MANUAL"/>
<control>
<call fun="rover_guidance_steering_setpoints(nav.carrot, &nav.heading)"/>
<call fun="rover_guidance_steering_speed_ctrl()"/>
<call fun="rover_guidance_steering_heading_ctrl(guidance_control.omega_sp)"/>
<call fun="command_set(COMMAND_THROTTLE, TRIM_UPPRZ(GetCmdFromThrottle(guidance_control.throttle)))"/> <!-- no reverse gear in nav -->
<call fun="command_set(COMMAND_STEERING, GetCmdFromDelta(guidance_control.cmd.delta))"/>
<call fun="autopilot.throttle = commands[COMMAND_THROTTLE]"/>
</control>
</mode>
</state_machine>
</autopilot>
conf/flight_plans/ENAC/rover_enac.xml
+78
View File
@@ -0,0 +1,78 @@
<?xml version="1.0"?>
<!DOCTYPE flight_plan SYSTEM "../flight_plan.dtd">
<flight_plan alt="157" ground_alt="147" lat0="43.5634" lon0="1.48448" max_dist_from_home="80" name="Enac test rover" security_height="1" wp_frame="ltp">
<header/>
<waypoints>
<waypoint name="HOME" x="0." y="0."/>
<waypoint name="STDBY" x="-14.974" y="18.525"/>
<waypoint name="p1" x="-38.2" y="18.5"/>
<waypoint name="p2" x="-15.3" y="-14.3"/>
<waypoint name="p3" x="17.5" y="8.6"/>
<waypoint name="p4" x="-5.5" y="41.4"/>
<waypoint name="_FLY1" x="-6.8" y="59.0"/>
<waypoint name="_FLY2" x="53.6" y="-25.7"/>
<waypoint name="_FLY3" x="5.3" y="-55.8"/>
<waypoint name="_FLY4" x="-54.3" y="25.9"/>
<waypoint name="_KILL1" x="-6.3" y="68.6"/>
<waypoint name="_KILL2" x="62.8" y="-29.8"/>
<waypoint name="_KILL3" x="4.4" y="-65.8"/>
<waypoint name="_KILL4" x="-67.4" y="29.7"/>
</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="mission" type="rover"/>
</modules>
<exceptions>
<exception cond="!InsideFlight_Area(GetPosX(), GetPosY()) @AND (nav_block @GT IndexOfBlock('Holding point'))" 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)"/>
</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="Standby" pre_call="if(!InsideKill(GetPosX(), GetPosY())) NavKillThrottle();" strip_button="Standby" strip_icon="home.png">
<stay wp="STDBY"/>
</block>
<block name="Mission" strip_button="Mission">
<call fun="mission_run()"/>
<deroute block="Standby"/>
</block>
<block name="Square">
<go wp="p1"/>
<path wpts="p1,p2,p3,p4,p1"/>
<stay wp="p1"/>
</block>
<block name="Circle">
<circle radius="nav.radius" wp="STDBY"/>
</block>
<block name="Oval">
<oval p1="STDBY" p2="HOME" radius="nav.radius"/>
</block>
</blocks>
</flight_plan>
conf/modules/guidance_rover.xml
+1 -1
View File
@@ -26,7 +26,7 @@
<file name="rover_guidance.h"/> <file name="rover_guidance.h"/>
</header> </header>
<init fun="rover_guidance_init()"/> <init fun="rover_guidance_init()"/>
<makefile target="ap" firmware="rover"> <makefile target="ap|nps" firmware="rover">
<file name="rover_guidance.c" dir="$(SRC_FIRMWARE)/guidance"/> <file name="rover_guidance.c" dir="$(SRC_FIRMWARE)/guidance"/>
</makefile> </makefile>
</module> </module>
conf/modules/guidance_rover_steering.xml
+7 -3
View File
@@ -15,6 +15,10 @@
<settings name="SR Guidance"> <settings name="SR Guidance">
<dl_settings> <dl_settings>
<dl_settings NAME="SR Guidance"> <dl_settings NAME="SR Guidance">
<dl_settings NAME="Guidance control">
<dl_setting MAX="10.0" MIN="0.0" STEP="0.01" VAR="guidance_control.pos_kp" shortname="pos_kp"/>
<dl_setting MAX="10.0" MIN="0.0" STEP="0.01" VAR="guidance_control.heading_kp" shortname="heading_kp"/>
</dl_settings>
<dl_settings NAME="Steering control"> <dl_settings NAME="Steering control">
<!--TODO: Set MIN_DELTA/MAX_DELTA config limits--> <!--TODO: Set MIN_DELTA/MAX_DELTA config limits-->
<dl_setting MAX="15.0" MIN="-15.0" STEP="0.1" VAR="guidance_control.cmd.delta" shortname="sr_delta" param="SR_GUIDANCE_DELTA"/> <dl_setting MAX="15.0" MIN="-15.0" STEP="0.1" VAR="guidance_control.cmd.delta" shortname="sr_delta" param="SR_GUIDANCE_DELTA"/>
@@ -22,15 +26,15 @@
<dl_settings NAME="Speed control"> <dl_settings NAME="Speed control">
<dl_setting MAX="10.0" MIN="-10.0" STEP="0.1" VAR="guidance_control.cmd.speed" shortname="sr_speed" param="SR_GUIDANCE_SPEED"/> <dl_setting MAX="10.0" MIN="-10.0" STEP="0.1" VAR="guidance_control.cmd.speed" shortname="sr_speed" param="SR_GUIDANCE_SPEED"/>
<dl_setting MAX="2000.0" MIN="0.0" STEP="1" VAR="guidance_control.kf" shortname="kf"/> <dl_setting MAX="2000.0" MIN="0.0" STEP="1" VAR="guidance_control.kf" shortname="kf"/>
<dl_setting MAX="1000.0" MIN="0.0" STEP="1" VAR="guidance_control.kp" shortname="kp"/> <dl_setting MAX="1000.0" MIN="0.0" STEP="1" VAR="guidance_control.kp" shortname="kp" handler="set_speed_pgain" module="guidance/rover_guidance_steering"/>
<dl_setting MAX="1000.0" MIN="0.0" STEP="1" VAR="guidance_control.ki" shortname="ki"/> <dl_setting MAX="1000.0" MIN="0.0" STEP="1" VAR="guidance_control.ki" shortname="ki" handler="set_speed_igain" module="guidance/rover_guidance_steering"/>
</dl_settings> </dl_settings>
</dl_settings> </dl_settings>
</dl_settings> </dl_settings>
</settings> </settings>
<dep> <dep>
<depends>@navigation,gvf_common</depends> <depends>@navigation</depends>
<provides>guidance,commands</provides> <provides>guidance,commands</provides>
</dep> </dep>
conf/modules/mission_rover.xml
+22
View File
@@ -0,0 +1,22 @@
<!DOCTYPE module SYSTEM "module.dtd">
<module name="mission_rover" dir="mission" task="control">
<doc>
<description>
Specific interface for mission control of rover.
</description>
</doc>
<dep>
<depends>mission_common</depends>
<provides>mission</provides>
</dep>
<makefile>
<file name="mission_rover_nav.c"/>
<test firmware="rover">
<define name="USE_GENERATED_AUTOPILOT"/>
<define name="DOWNLINK_TRANSPORT" value="pprz_tp"/>
<define name="DOWNLINK_DEVICE" value="uart0"/>
<define name="USE_UART0"/>
</test>
</makefile>
</module>
conf/radios/FrSky_X-Lite_car.xml
+13
View File
@@ -0,0 +1,13 @@
<?xml version="1.0"?>
<!DOCTYPE radio SYSTEM "radio.dtd">
<radio name="FrSky X-Lite with XM+ SBUS receiver" data_min="900" data_max="2100" sync_min ="5000" sync_max ="15000" pulse_type="POSITIVE">
<channel function="ROLL" min="990" neutral="1500" max="2011" average="0"/>
<channel function="PITCH" min="990" neutral="1500" max="2011" average="0" reverse="1"/>
<channel function="THROTTLE" min="990" neutral="1500" max="2011" average="0"/>
<channel function="YAW" min="990" neutral="1500" max="2011" average="0"/>
<channel function="MODE" min="990" neutral="1500" max="2011" average="1" reverse="1"/>
<channel function="GAIN1" min="990" neutral="1500" max="2011" average="1" reverse="1"/>
<channel function="GAIN2" min="990" neutral="1500" max="2011" average="1"/>
<channel function="GAIN3" min="990" neutral="1500" max="2011" average="1"/>
</radio>
sw/airborne/firmwares/rover/guidance/rover_guidance_steering.c
+96 -40
View File
@@ -1,6 +1,7 @@
/* /*
* Copyright (C) 2021 Jesús Bautista <jesusbautistavillar@gmail.com> * Copyright (C) 2021 Jesús Bautista <jesusbautistavillar@gmail.com>
* Hector García <noeth3r@gmail.com> * Hector García <noeth3r@gmail.com>
* 2025 Gautier Hattenberger <gautier.hattenberger@gmail.com>
* *
* This file is part of paparazzi. * This file is part of paparazzi.
* *
@@ -25,90 +26,131 @@
#include "firmwares/rover/guidance/rover_guidance_steering.h" #include "firmwares/rover/guidance/rover_guidance_steering.h"
#include "generated/airframe.h" #include "generated/airframe.h"
#include "generated/autopilot_core_guidance.h"
#include "modules/actuators/actuators_default.h"
#include "modules/radio_control/radio_control.h"
#include "autopilot.h" #include "autopilot.h"
#include "navigation.h"
#include "state.h" #include "state.h"
#include "filters/pid.h" // Used for p+i speed controller #include "filters/pid.h" // Used for p+i speed controller
#include <math.h> #ifndef ROVER_GUIDANCE_POS_KP
#include <stdio.h> #define ROVER_GUIDANCE_POS_KP 1.f
#endif
#ifndef ROVER_GUIDANCE_HEADING_KP
#define ROVER_GUIDANCE_HEADING_KP 1.f
#endif
#ifndef ROVER_GUIDANCE_SPEED_KP
#define ROVER_GUIDANCE_SPEED_KP 10.f
#endif
#ifndef ROVER_GUIDANCE_SPEED_KI
#define ROVER_GUIDANCE_SPEED_KI 100.f
#endif
#ifndef ROVER_GUIDANCE_MAX_POS_ERR
#define ROVER_GUIDANCE_MAX_POS_ERR 10.f // max position error
#endif
#ifndef ROVER_GUIDANCE_MAX_SPEED
#define ROVER_GUIDANCE_MAX_SPEED 10.f
#endif
#ifndef ROVER_GUIDANCE_PROXIMITY_DISTANCE
#define ROVER_GUIDANCE_PROXIMITY_DISTANCE 2.f // proximity distance TODO improve with hysteresis ?
#endif
// Guidance control main variables // Guidance control main variables
rover_ctrl guidance_control; rover_ctrl guidance_control;
static struct PID_f rover_pid; static struct PID_f rover_pid;
static float time_step; static float time_step;
static float last_speed_cmd;
static uint8_t last_ap_mode;
/** INIT function **/ /** INIT function **/
void rover_guidance_steering_init(void) void rover_guidance_steering_init(void)
{ {
guidance_control.cmd.delta = 0.0; guidance_control.cmd.delta = 0.f;
guidance_control.cmd.speed = 0.0; guidance_control.cmd.speed = 0.f;
guidance_control.throttle = 0.0; guidance_control.throttle = 0.f;
last_speed_cmd = 0.0; guidance_control.pos_kp = ROVER_GUIDANCE_POS_KP;
last_ap_mode = AP_MODE_KILL; guidance_control.heading_kp = ROVER_GUIDANCE_HEADING_KP;
guidance_control.speed_error = 0.f;
guidance_control.speed_error = 0.0;
guidance_control.kf = SR_MEASURED_KF; guidance_control.kf = SR_MEASURED_KF;
guidance_control.kp = 10; guidance_control.kp = ROVER_GUIDANCE_SPEED_KP;
guidance_control.ki = 100; guidance_control.ki = ROVER_GUIDANCE_SPEED_KI;
init_pid_f(&rover_pid, guidance_control.kp, 0.f, guidance_control.ki, MAX_PPRZ*0.2); init_pid_f(&rover_pid, guidance_control.kp, 0.f, guidance_control.ki, MAX_PPRZ*0.2f);
// Based on autopilot state machine frequency // Based on autopilot state machine frequency
time_step = 1.f/PERIODIC_FREQUENCY; time_step = 1.f/PERIODIC_FREQUENCY;
} }
void rover_guidance_steering_periodic(void)
{
// from code generation
autopilot_core_guidance_periodic_task();
}
/** CTRL functions **/ /** CTRL functions **/
// Steering control (GVF) // Steering control (GVF)
void rover_guidance_steering_heading_ctrl(float omega) //GVF give us this omega void rover_guidance_steering_heading_ctrl(float omega) //GVF give us this omega
{ {
float delta = 0.0;
// Speed is bounded to avoid GPS noise while driving at small velocity // Speed is bounded to avoid GPS noise while driving at small velocity
float speed = BoundSpeed(stateGetHorizontalSpeedNorm_f()); float speed = BoundSpeed(stateGetHorizontalSpeedNorm_f());
// Compute steering angle
if (fabs(omega)>0.0) { float delta = DegOfRad(atanf(omega*DRIVE_SHAFT_DISTANCE/speed));
delta = DegOfRad(-atanf(omega*DRIVE_SHAFT_DISTANCE/speed));
}
guidance_control.cmd.delta = BoundDelta(delta); guidance_control.cmd.delta = BoundDelta(delta);
} }
// Speed control (feed feed forward + propotional + integral controler) (PID) // Speed control (feed feed forward + propotional + integral controler) (PID)
void rover_guidance_steering_speed_ctrl(void) void rover_guidance_steering_speed_ctrl(void)
{ {
// - Looking for setting update // Updating PID
if (guidance_control.kp != rover_pid.g[0] || guidance_control.ki != rover_pid.g[2]) {
set_gains_pid_f(&rover_pid, guidance_control.kp, 0.f, guidance_control.ki);
}
if (guidance_control.cmd.speed != last_speed_cmd) {
last_speed_cmd = guidance_control.cmd.speed;
//reset_pid_f(&rover_pid);
}
// - Updating PID
guidance_control.speed_error = (guidance_control.cmd.speed - stateGetHorizontalSpeedNorm_f()); guidance_control.speed_error = (guidance_control.cmd.speed - stateGetHorizontalSpeedNorm_f());
update_pid_f(&rover_pid, guidance_control.speed_error, time_step); update_pid_f(&rover_pid, guidance_control.speed_error, time_step);
guidance_control.throttle = BoundThrottle(guidance_control.kf*guidance_control.cmd.speed + get_pid_f(&rover_pid)); guidance_control.throttle = BoundThrottle(guidance_control.kf*guidance_control.cmd.speed + get_pid_f(&rover_pid));
} }
void rover_guidance_steering_setpoints(struct EnuCoor_f pos_sp, float *heading_sp)
{
// compute position error
struct FloatVect2 pos_2d;
VECT2_COPY(pos_2d, pos_sp);
struct FloatVect2 pos_err;
VECT2_DIFF(pos_err, pos_2d, *stateGetPositionEnu_f());
float pos_err_norm = float_vect2_norm(&pos_err);
BoundAbs(pos_err_norm, ROVER_GUIDANCE_MAX_POS_ERR);
// speed and heading update when far enough
if (pos_err_norm > ROVER_GUIDANCE_PROXIMITY_DISTANCE) {
// compute speed error
guidance_control.cmd.speed = guidance_control.pos_kp * pos_err_norm;
Bound(guidance_control.cmd.speed, 0.f, ROVER_GUIDANCE_MAX_SPEED);
// if not close to WP, compute desired heading
guidance_control.heading_sp = atan2f(pos_err.x, pos_err.y);
// update nav sp
*heading_sp = guidance_control.heading_sp;
// angular error
float heading_err = guidance_control.heading_sp - stateGetNedToBodyEulers_f()->psi;
NormRadAngle(heading_err);
// compute omega setpoint
guidance_control.omega_sp = guidance_control.heading_kp * heading_err;
}
else {
guidance_control.cmd.speed = 0.f;
guidance_control.heading_sp = *heading_sp;
guidance_control.omega_sp = 0.f;
}
}
/** PID RESET function**/ /** PID RESET function**/
void rover_guidance_steering_pid_reset(void) void rover_guidance_steering_pid_reset(void)
{ {
// Reset speed PID reset_pid_f(&rover_pid);
if (rover_pid.sum != 0) {
reset_pid_f(&rover_pid);
}
} }
void rover_guidance_steering_kill(void) void rover_guidance_steering_kill(void)
@@ -116,3 +158,17 @@ void rover_guidance_steering_kill(void)
guidance_control.cmd.delta = 0.0; guidance_control.cmd.delta = 0.0;
guidance_control.cmd.speed = 0.0; guidance_control.cmd.speed = 0.0;
} }
void rover_guidance_steering_set_speed_pgain(float pgain)
{
guidance_control.kp = pgain;
set_gains_pid_f(&rover_pid, guidance_control.kp, 0.f, guidance_control.ki);
}
void rover_guidance_steering_set_speed_igain(float igain)
{
guidance_control.ki = igain;
set_gains_pid_f(&rover_pid, guidance_control.kp, 0.f, guidance_control.ki);
rover_pid.sum = 0.f;
}
sw/airborne/firmwares/rover/guidance/rover_guidance_steering.h
+18 -8
View File
@@ -1,6 +1,7 @@
/* /*
* Copyright (C) 2021 Jesús Bautista <jesusbautistavillar@gmail.com> * Copyright (C) 2021 Jesús Bautista <jesusbautistavillar@gmail.com>
* Hector García <noeth3r@gmail.com> * Hector García <noeth3r@gmail.com>
* 2025 Gautier Hattenberger <gautier.hattenberger@gmail.com>
* *
* This file is part of paparazzi. * This file is part of paparazzi.
* *
@@ -30,6 +31,7 @@
#include "std.h" #include "std.h"
#include <math.h> #include <math.h>
#include "math/pprz_geodetic_float.h"
#include "generated/airframe.h" #include "generated/airframe.h"
@@ -56,10 +58,10 @@
// MIN_DELTA, MAX_DELTA: Min and max wheels turning angle (deg) // MIN_DELTA, MAX_DELTA: Min and max wheels turning angle (deg)
// You should measure this angle if you want to have an // You should measure this angle if you want to have an
// efficient control in your steering // efficient control in your steering
#ifndef MAX_DELTA #ifndef MAX_DELTA
#define MAX_DELTA 15.0 #define MAX_DELTA 15.0
#endif #endif
#ifndef MIN_DELTA #ifndef MIN_DELTA
#define MIN_DELTA MAX_DELTA #define MIN_DELTA MAX_DELTA
#endif #endif
@@ -70,15 +72,15 @@
#ifndef MAX_CMD_SHUT #ifndef MAX_CMD_SHUT
#define MAX_CMD_SHUT 0 #define MAX_CMD_SHUT 0
#endif #endif
#ifndef MIN_CMD_SHUT #ifndef MIN_CMD_SHUT
#define MIN_CMD_SHUT 0 #define MIN_CMD_SHUT 0
#endif #endif
// MIN_SPEED, MAX_SPEED: Min and max state speed (m/s) // MIN_SPEED, MAX_SPEED: Min and max state speed (m/s)
#ifndef MAX_SPEED #ifndef MAX_SPEED
#define MAX_SPEED 999.0 //We don't really use that variable #define MAX_SPEED 999.0 //We don't really use that variable
#endif #endif
#ifndef MIN_SPEED #ifndef MIN_SPEED
#define MIN_SPEED 0.2 //But this one is mandatory because we have #define MIN_SPEED 0.2 //But this one is mandatory because we have
#endif //to deal with GPS noise (and 1/v in guidance control). #endif //to deal with GPS noise (and 1/v in guidance control).
@@ -106,7 +108,11 @@ typedef struct {
typedef struct { typedef struct {
sr_cmd_t cmd; sr_cmd_t cmd;
float throttle; float throttle;
float heading_sp; ///< heading setpoint
float omega_sp; ///< omega setpoint
float pos_kp;
float heading_kp;
float speed_error; float speed_error;
float kf; float kf;
float kp; float kp;
@@ -117,11 +123,15 @@ extern rover_ctrl guidance_control;
/** Steering rover guidance EXT FUNCTIONS **/ /** Steering rover guidance EXT FUNCTIONS **/
extern void rover_guidance_steering_init(void); extern void rover_guidance_steering_init(void);
extern void rover_guidance_steering_periodic(void); // call state machine
extern void rover_guidance_steering_heading_ctrl(float omega); extern void rover_guidance_steering_heading_ctrl(float omega);
extern void rover_guidance_steering_speed_ctrl(void); extern void rover_guidance_steering_speed_ctrl(void);
extern void rover_guidance_steering_setpoints(struct EnuCoor_f pos_sp, float *heading_sp);
extern void rover_guidance_steering_pid_reset(void); extern void rover_guidance_steering_pid_reset(void);
extern void rover_guidance_steering_kill(void); extern void rover_guidance_steering_kill(void);
extern void rover_guidance_steering_set_speed_pgain(float pgain);
extern void rover_guidance_steering_set_speed_igain(float igain);
/** MACROS **/ /** MACROS **/
// Bound delta // Bound delta
@@ -138,8 +148,8 @@ extern void rover_guidance_steering_kill(void);
#define BoundThrottle(throttle) TRIM_PPRZ((int)throttle) #define BoundThrottle(throttle) TRIM_PPRZ((int)throttle)
// Set low level commands from high level commands // Set low level commands from high level commands
#define GetCmdFromDelta(delta) (delta >= 0 ? -delta/MAX_DELTA * (MAX_PPRZ - (int)MAX_CMD_SHUT) : \ #define GetCmdFromDelta(delta) (delta >= 0 ? delta/MAX_DELTA * (MAX_PPRZ - (int)MAX_CMD_SHUT) : \
-delta/MIN_DELTA * (MAX_PPRZ - (int)MIN_CMD_SHUT)) delta/MIN_DELTA * (MAX_PPRZ - (int)MIN_CMD_SHUT))
// This macro is for NAV state // This macro is for NAV state
#define GetCmdFromThrottle(throttle) (autopilot_throttle_killed() ? 0 : TRIM_PPRZ((int)throttle)) #define GetCmdFromThrottle(throttle) (autopilot_throttle_killed() ? 0 : TRIM_PPRZ((int)throttle))
sw/airborne/firmwares/rover/navigation.c
+41 -40
View File
@@ -134,41 +134,39 @@ void nav_parse_MOVE_WP(uint8_t *buf)
bool nav_check_wp_time(struct EnuCoor_f *wp, float stay_time) bool nav_check_wp_time(struct EnuCoor_f *wp, float stay_time)
{ {
(void) wp; uint16_t time_at_wp;
(void) stay_time; float dist_to_point;
return true; static uint16_t wp_entry_time = 0;
// uint16_t time_at_wp; static bool wp_reached = false;
// float dist_to_point; static struct EnuCoor_i wp_last = { 0, 0, 0 };
// static uint16_t wp_entry_time = 0; struct EnuCoor_i wp_i;
// static bool wp_reached = false; struct FloatVect2 diff;
// static struct EnuCoor_i wp_last = { 0, 0, 0 };
// struct Int32Vect2 diff; ENU_BFP_OF_REAL(wp_i, *wp);
// if ((wp_last.x != wp_i.x) || (wp_last.y != wp_i.y)) {
// if ((wp_last.x != wp->x) || (wp_last.y != wp->y)) { wp_reached = false;
// wp_reached = false; wp_last = wp_i;
// wp_last = *wp; }
// }
// VECT2_DIFF(diff, *wp, *stateGetPositionEnu_f());
// VECT2_DIFF(diff, *wp, *stateGetPositionEnu_i()); dist_to_point = float_vect2_norm(&diff);
// struct FloatVect2 diff_f = {POS_FLOAT_OF_BFP(diff.x), POS_FLOAT_OF_BFP(diff.y)}; if (dist_to_point < ARRIVED_AT_WAYPOINT) {
// dist_to_point = float_vect2_norm(&diff_f); if (!wp_reached) {
// if (dist_to_point < ARRIVED_AT_WAYPOINT) { wp_reached = true;
// if (!wp_reached) { wp_entry_time = autopilot.flight_time;
// wp_reached = true; time_at_wp = 0;
// wp_entry_time = autopilot.flight_time; } else {
// time_at_wp = 0; time_at_wp = autopilot.flight_time - wp_entry_time;
// } else { }
// time_at_wp = autopilot.flight_time - wp_entry_time; } else {
// } time_at_wp = 0;
// } else { wp_reached = false;
// time_at_wp = 0; }
// wp_reached = false; if (time_at_wp > stay_time) {
// } INT_VECT3_ZERO(wp_last);
// if (time_at_wp > stay_time) { return true;
// INT_VECT3_ZERO(wp_last); }
// return true; return false;
// }
// return false;
} }
@@ -190,15 +188,15 @@ void nav_init_stage(void)
{ {
VECT3_COPY(nav.last_pos, *stateGetPositionEnu_f()); VECT3_COPY(nav.last_pos, *stateGetPositionEnu_f());
stage_time = 0; stage_time = 0;
//nav_circle_radians = 0; FIXME if (nav.nav_stage_init) {
nav.nav_stage_init();
}
} }
void nav_periodic_task(void) void nav_periodic_task(void)
{ {
RunOnceEvery(NAVIGATION_FREQUENCY, { stage_time++; block_time++; }); RunOnceEvery(NAVIGATION_FREQUENCY, { stage_time++; block_time++; });
//nav.dist2_to_wp = 0; FIXME
/* from flight_plan.h */ /* from flight_plan.h */
auto_nav(); auto_nav();
@@ -217,8 +215,6 @@ void nav_home(void)
nav.mode = NAV_MODE_WAYPOINT; nav.mode = NAV_MODE_WAYPOINT;
VECT3_COPY(nav.target, waypoints[WP_HOME].enu_f); VECT3_COPY(nav.target, waypoints[WP_HOME].enu_f);
//nav.dist2_to_wp = nav.dist2_to_home; FIXME
/* run carrot loop */ /* run carrot loop */
nav_run(); nav_run();
} }
@@ -307,6 +303,11 @@ void nav_set_failsafe(void)
/** Register functions /** Register functions
*/ */
void nav_register_stage_init(nav_rover_stage_init nav_stage_init)
{
nav.nav_stage_init = nav_stage_init;
}
void nav_register_goto_wp(nav_rover_goto nav_goto, nav_rover_route nav_route, nav_rover_approaching nav_approaching) void nav_register_goto_wp(nav_rover_goto nav_goto, nav_rover_route nav_route, nav_rover_approaching nav_approaching)
{ {
nav.nav_goto = nav_goto; nav.nav_goto = nav_goto;
sw/airborne/firmwares/rover/navigation.h
+7 -3
View File
@@ -40,7 +40,7 @@
#endif #endif
#ifndef CARROT_DIST #ifndef CARROT_DIST
#define CARROT_DIST 2.f #define CARROT_DIST 5.f
#endif #endif
/** default navigation frequency */ /** default navigation frequency */
@@ -75,6 +75,7 @@
#define NAV_MODE_HEADING 3 #define NAV_MODE_HEADING 3
#define NAV_MODE_MANUAL 4 #define NAV_MODE_MANUAL 4
typedef void (*nav_rover_stage_init)(void);
typedef void (*nav_rover_goto)(struct EnuCoor_f *wp); typedef void (*nav_rover_goto)(struct EnuCoor_f *wp);
typedef void (*nav_rover_route)(struct EnuCoor_f *wp_start, struct EnuCoor_f *wp_end); typedef void (*nav_rover_route)(struct EnuCoor_f *wp_start, struct EnuCoor_f *wp_end);
typedef bool (*nav_rover_approaching)(struct EnuCoor_f *wp_to, struct EnuCoor_f *wp_from, float approaching_time); typedef bool (*nav_rover_approaching)(struct EnuCoor_f *wp_to, struct EnuCoor_f *wp_from, float approaching_time);
@@ -106,6 +107,7 @@ struct RoverNavigation {
struct EnuCoor_f last_pos; ///< last stage position struct EnuCoor_f last_pos; ///< last stage position
// pointers to basic nav functions // pointers to basic nav functions
nav_rover_stage_init nav_stage_init;
nav_rover_goto nav_goto; nav_rover_goto nav_goto;
nav_rover_route nav_route; nav_rover_route nav_route;
nav_rover_approaching nav_approaching; nav_rover_approaching nav_approaching;
@@ -118,6 +120,7 @@ extern struct RoverNavigation nav;
/** Registering functions /** Registering functions
*/ */
extern void nav_register_stage_init(nav_rover_stage_init nav_stage_init);
extern void nav_register_goto_wp(nav_rover_goto nav_goto, extern void nav_register_goto_wp(nav_rover_goto nav_goto,
nav_rover_route nav_route, nav_rover_route nav_route,
nav_rover_approaching nav_approaching); nav_rover_approaching nav_approaching);
@@ -243,8 +246,9 @@ bool nav_check_wp_time(struct EnuCoor_f *wp, float stay_time);
static inline void NavGotoWaypoint(uint8_t wp) static inline void NavGotoWaypoint(uint8_t wp)
{ {
nav.mode = NAV_MODE_WAYPOINT; nav.mode = NAV_MODE_WAYPOINT;
VECT3_COPY(nav.target, waypoints[wp].enu_f); if (nav.nav_goto) {
//nav.dist2_to_wp = get_dist2_to_waypoint(wp); FIXME nav.nav_goto(&waypoints[wp].enu_f);
}
} }
/*********** Navigation along a line *************************************/ /*********** Navigation along a line *************************************/
sw/airborne/modules/mission/mission_rover_nav.c
+255
View File
@@ -0,0 +1,255 @@
/** @file modules/mission/mission_rover_nav.c
* @brief mission navigation for rovers
*
* Implement specific navigation routines for the mission control
* of a rover.
*/
#include <stdio.h>
#include "modules/mission/mission_common.h"
#include "autopilot.h"
#include "firmwares/rover/navigation.h"
#include "generated/flight_plan.h"
// Buffer zone in [m] before MAX_DIST_FROM_HOME
#define BUFFER_ZONE_DIST 10
/// Utility function: converts lla (int) to local point (float)
bool mission_point_of_lla(struct EnuCoor_f *point, struct LlaCoor_i *lla)
{
// return FALSE if there is no valid local coordinate system
if (!state.ned_initialized_i) {
return false;
}
// For a rover, GPS z altitude is ignored. The altitude is forced to 0.
lla->alt = 0;
// Compute ENU components from LLA with respect to ltp origin
struct EnuCoor_i tmp_enu_point_i;
enu_of_lla_point_i(&tmp_enu_point_i, stateGetNedOrigin_i(), lla);
struct EnuCoor_f tmp_enu_point_f;
// result of enu_of_lla_point_i is in cm, convert to float in m
VECT3_SMUL(tmp_enu_point_f, tmp_enu_point_i, 0.01f);
// Bound the new waypoint with max distance from home
struct FloatVect2 home;
home.x = waypoint_get_x(WP_HOME);
home.y = waypoint_get_y(WP_HOME);
struct FloatVect2 vect_from_home;
VECT2_DIFF(vect_from_home, tmp_enu_point_f, home);
// Saturate the mission wp not to overflow max_dist_from_home
// including a buffer zone before limits
float dist_to_home = float_vect2_norm(&vect_from_home);
dist_to_home += BUFFER_ZONE_DIST;
if (dist_to_home > MAX_DIST_FROM_HOME) {
VECT2_SMUL(vect_from_home, vect_from_home, (MAX_DIST_FROM_HOME / dist_to_home));
}
// set new point (2D)
point->x = home.x + vect_from_home.x;
point->y = home.y + vect_from_home.y;
point->z = 0.0f; // to be sure the altitude is at 0
return true;
}
// navigation time step
static const float dt_navigation = 1.0f / ((float)NAVIGATION_FREQUENCY);
// last_mission_wp, last target wp from mission elements, not used actively and kept for future implementations
struct EnuCoor_f last_mission_wp = { 0.f, 0.f, 0.f };
/** Navigation function to a single waypoint (2D)
*/
static inline bool mission_nav_wp(struct _mission_element *el)
{
struct EnuCoor_f *target_wp = &(el->element.mission_wp.wp);
//Check proximity and wait for 'duration' seconds in proximity circle if desired
if (nav.nav_approaching(target_wp, NULL, CARROT)) {
last_mission_wp = *target_wp;
if (el->duration > 0.f) {
if (nav_check_wp_time(target_wp, el->duration)) { return false; }
} else { return false; }
}
//Go to Mission Waypoint
nav.nav_goto(target_wp);
return true;
}
/** Navigation function on a circle
*/
static inline bool mission_nav_circle(struct _mission_element *el)
{
struct EnuCoor_f *center_wp = &(el->element.mission_circle.center);
float radius = el->element.mission_circle.radius;
//Draw the desired circle for a 'duration' time
nav.nav_circle(center_wp, radius);
if (el->duration > 0.f && mission.element_time >= el->duration) {
return false;
}
if (el-> duration <= 0.f){
return false;
}
return true;
}
/** Navigation function along a segment (2D)
*/
static inline bool mission_nav_segment(struct _mission_element *el)
{
struct EnuCoor_f *from_wp = &(el->element.mission_segment.from);
struct EnuCoor_f *to_wp = &(el->element.mission_segment.to);
//Check proximity and wait for 'duration' seconds in proximity circle if desired
if (nav.nav_approaching(to_wp, from_wp, CARROT)) {
last_mission_wp = *to_wp;
if (el->duration > 0.f) {
if (nav_check_wp_time(to_wp, el->duration)) { return false; }
} else { return false; }
}
//Route Between from-to
nav.nav_route(from_wp, to_wp);
return true;
}
#ifndef MISSION_PATH_SKIP_GOTO
#define MISSION_PATH_SKIP_GOTO FALSE
#endif
/** Navigation function along a path (2D)
*/
static inline bool mission_nav_path(struct _mission_element *el)
{
if (el->element.mission_path.nb == 0) {
return false; // nothing to do
}
if (el->element.mission_path.path_idx == 0) { //first wp of path
el->element.mission_wp.wp = el->element.mission_path.path[0];
if (MISSION_PATH_SKIP_GOTO || !mission_nav_wp(el)) { el->element.mission_path.path_idx++; }
}
else if (el->element.mission_path.path_idx < el->element.mission_path.nb) { //standart wp of path
struct EnuCoor_f *from_wp = &(el->element.mission_path.path[(el->element.mission_path.path_idx) - 1]);
struct EnuCoor_f *to_wp = &(el->element.mission_path.path[el->element.mission_path.path_idx]);
//Check proximity and wait for t seconds in proximity circle if desired
if (nav.nav_approaching(to_wp, from_wp, CARROT)) {
last_mission_wp = *to_wp;
if (el->duration > 0.f) {
if (nav_check_wp_time(to_wp, el->duration)) {
el->element.mission_path.path_idx++;
}
} else { el->element.mission_path.path_idx++; }
}
//Route Between from-to
nav.nav_route(from_wp, to_wp);
} else { return false; } //end of path
return true;
}
/** Call custom navigation function
*/
static inline bool mission_nav_custom(struct _mission_custom *custom, bool init)
{
if (init) {
return custom->reg->cb(custom->nb, custom->params, MissionInit);
} else {
return custom->reg->cb(custom->nb, custom->params, MissionRun);
}
}
/** Implement waiting pattern
* Only called when MISSION_WAIT_TIMEOUT is not 0
*/
#ifndef MISSION_WAIT_TIMEOUT
#define MISSION_WAIT_TIMEOUT 30 // wait 30 seconds before ending mission
#endif
static bool mission_wait_started = false;
#if MISSION_WAIT_TIMEOUT
static float mission_wait_time = 0.f;
static struct _mission_element mission_wait_wp;
static bool mission_wait_pattern(void) {
if (!mission_wait_started) {
mission_wait_wp.element.mission_wp.wp = *stateGetPositionEnu_f();
mission_wait_time = 0.f;
mission_wait_started = true;
}
mission_nav_wp(&mission_wait_wp);
mission_wait_time += dt_navigation;
return (mission_wait_time < (float)MISSION_WAIT_TIMEOUT); // keep flying until TIMEOUT
}
#else
static bool mission_wait_pattern(void) {
return false; // no TIMEOUT, end mission now
}
#endif
int mission_run()
{
// current element
struct _mission_element *el = NULL;
if ((el = mission_get()) == NULL) {
return mission_wait_pattern();
}
mission_wait_started = false;
bool el_running = false;
switch (el->type) {
case MissionWP:
el_running = mission_nav_wp(el);
break;
case MissionCircle:
el_running = mission_nav_circle(el);
break;
case MissionSegment:
el_running = mission_nav_segment(el);
break;
case MissionPath:
el_running = mission_nav_path(el);
break;
case MissionCustom:
el_running = mission_nav_custom(&(el->element.mission_custom), mission.element_time < dt_navigation);
break;
default:
// invalid type or pattern not yet handled
break;
}
// increment element_time
mission.element_time += dt_navigation;
if (!el_running) {
// reset timer
mission.element_time = 0.;
// go to next element
mission.current_idx = (mission.current_idx + 1) % MISSION_ELEMENT_NB;
}
return true;
}
sw/airborne/modules/nav/nav_rover_base.c
+51 -50
View File
@@ -31,10 +31,17 @@ struct RoverNavBase nav_rover_base;
/** Implement basic nav function /** Implement basic nav function
*/ */
static void nav_stage_init(void)
{
nav_rover_base.circle.radians = 0.f;
nav_rover_base.goto_wp.leg_progress = 0.f;
}
static void nav_goto(struct EnuCoor_f *wp) static void nav_goto(struct EnuCoor_f *wp)
{ {
nav_rover_base.goto_wp.to = *wp; nav_rover_base.goto_wp.to = *wp;
nav_rover_base.goto_wp.dist2_to_wp = get_dist2_to_point(wp); nav_rover_base.goto_wp.dist2_to_wp = get_dist2_to_point(wp);
VECT3_COPY(nav.target, *wp);
nav.mode = NAV_MODE_WAYPOINT; nav.mode = NAV_MODE_WAYPOINT;
} }
@@ -158,82 +165,76 @@ static void _nav_oval_init(void)
static void nav_oval(struct EnuCoor_f *wp1, struct EnuCoor_f *wp2, float radius) static void nav_oval(struct EnuCoor_f *wp1, struct EnuCoor_f *wp2, float radius)
{ {
(void) wp1; float alt = wp1->z;
(void) wp2; wp2->z = alt;
(void) radius;
#if 0
radius = - radius; /* Historical error ? */
int32_t alt = waypoints[p1].enu_i.z;
waypoints[p2].enu_i.z = alt;
float p2_p1_x = waypoints[p1].enu_f.x - waypoints[p2].enu_f.x; struct FloatVect2 dir;
float p2_p1_y = waypoints[p1].enu_f.y - waypoints[p2].enu_f.y; VECT2_DIFF(dir, *wp1, *wp2);
float d = sqrtf(p2_p1_x * p2_p1_x + p2_p1_y * p2_p1_y); float_vect2_normalize(&dir);
/* Unit vector from p1 to p2 */
int32_t u_x = POS_BFP_OF_REAL(p2_p1_x / d);
int32_t u_y = POS_BFP_OF_REAL(p2_p1_y / d);
/* The half circle centers and the other leg */ /* The half circle centers and the other leg */
struct EnuCoor_i p1_center = { waypoints[p1].enu_i.x + radius * -u_y, struct EnuCoor_f p1_center = {
waypoints[p1].enu_i.y + radius * u_x, wp1->x + radius * dir.y,
alt wp1->y - radius * dir.x,
alt
}; };
struct EnuCoor_i p1_out = { waypoints[p1].enu_i.x + 2 * radius * -u_y, struct EnuCoor_f p1_out = {
waypoints[p1].enu_i.y + 2 * radius * u_x, wp1->x + 2.f * radius * dir.y,
alt wp1->y - 2.f * radius * dir.x,
alt
};
struct EnuCoor_f p2_in = {
wp2->x + 2.f * radius * dir.y,
wp2->y - 2.f * radius * dir.x,
alt
};
struct EnuCoor_f p2_center = {
wp2->x + radius * dir.y,
wp2->y - radius * dir.x,
alt
}; };
struct EnuCoor_i p2_in = { waypoints[p2].enu_i.x + 2 * radius * -u_y, float qdr_out_2 = M_PI - atan2f(dir.y, dir.x);
waypoints[p2].enu_i.y + 2 * radius * u_x, float qdr_out_1 = qdr_out_2 + M_PI;
alt if (radius > 0.f) {
}; qdr_out_2 += M_PI;
struct EnuCoor_i p2_center = { waypoints[p2].enu_i.x + radius * -u_y, qdr_out_1 += M_PI;
waypoints[p2].enu_i.y + radius * u_x,
alt
};
int32_t qdr_out_2 = INT32_ANGLE_PI - int32_atan2_2(u_y, u_x);
int32_t qdr_out_1 = qdr_out_2 + INT32_ANGLE_PI;
if (radius < 0) {
qdr_out_2 += INT32_ANGLE_PI;
qdr_out_1 += INT32_ANGLE_PI;
} }
int32_t qdr_anticipation = ANGLE_BFP_OF_REAL(radius > 0 ? -15 : 15); float qdr_anticipation = (radius > 0.f ? 15.f : -15.f);
switch (oval_status) { switch (nav_rover_base.oval.status) {
case OC1 : case OC1 :
nav_circle(&p1_center, POS_BFP_OF_REAL(-radius)); nav.nav_circle(&p1_center, radius);
if (NavQdrCloseTo(INT32_DEG_OF_RAD(qdr_out_1) - qdr_anticipation)) { if (NavQdrCloseTo(DegOfRad(qdr_out_1) - qdr_anticipation)) {
oval_status = OR12; nav_rover_base.oval.status = OR12;
InitStage(); InitStage();
LINE_START_FUNCTION; LINE_START_FUNCTION;
} }
return; return;
case OR12: case OR12:
nav_route(&p1_out, &p2_in); nav.nav_route(&p1_out, &p2_in);
if (nav_approaching_from(&p2_in, &p1_out, CARROT)) { if (nav.nav_approaching(&p2_in, &p1_out, CARROT)) {
oval_status = OC2; nav_rover_base.oval.status = OC2;
nav_oval_count++; nav_rover_base.oval.count++;
InitStage(); InitStage();
LINE_STOP_FUNCTION; LINE_STOP_FUNCTION;
} }
return; return;
case OC2 : case OC2 :
nav_circle(&p2_center, POS_BFP_OF_REAL(-radius)); nav.nav_circle(&p2_center, radius);
if (NavQdrCloseTo(INT32_DEG_OF_RAD(qdr_out_2) - qdr_anticipation)) { if (NavQdrCloseTo(DegOfRad(qdr_out_2) - qdr_anticipation)) {
oval_status = OR21; nav_rover_base.oval.status = OR21;
InitStage(); InitStage();
LINE_START_FUNCTION; LINE_START_FUNCTION;
} }
return; return;
case OR21: case OR21:
nav_route(&waypoints[p2].enu_i, &waypoints[p1].enu_i); nav.nav_route(wp2, wp1);
if (nav_approaching_from(&waypoints[p1].enu_i, &waypoints[p2].enu_i, CARROT)) { if (nav.nav_approaching(wp1, wp2, CARROT)) {
oval_status = OC1; nav_rover_base.oval.status = OC1;
InitStage(); InitStage();
LINE_STOP_FUNCTION; LINE_STOP_FUNCTION;
} }
@@ -242,7 +243,6 @@ static void nav_oval(struct EnuCoor_f *wp1, struct EnuCoor_f *wp2, float radius)
default: /* Should not occur !!! Doing nothing */ default: /* Should not occur !!! Doing nothing */
return; return;
} }
#endif
} }
#if PERIODIC_TELEMETRY #if PERIODIC_TELEMETRY
@@ -295,6 +295,7 @@ void nav_rover_init(void)
nav_rover_base.goto_wp.leg_length = 1.f; nav_rover_base.goto_wp.leg_length = 1.f;
// register nav functions // register nav functions
nav_register_stage_init(nav_stage_init);
nav_register_goto_wp(nav_goto, nav_route, nav_approaching); nav_register_goto_wp(nav_goto, nav_route, nav_approaching);
nav_register_circle(nav_circle); nav_register_circle(nav_circle);
nav_register_oval(_nav_oval_init, nav_oval); nav_register_oval(_nav_oval_init, nav_oval);
sw/simulator/nps/nps_fdm_rover.c
+114 -54
View File
@@ -1,5 +1,5 @@
/* /*
* Copyright (C) 2021 Jesús Bautista <jesusbautistavillar@gmail.com> * Copyright (C) 2021 Jesús Bautista <jesusbautistavillar@gmail.com>
* Hector García <noeth3r@gmail.com> * Hector García <noeth3r@gmail.com>
* *
* This file is part of paparazzi. * This file is part of paparazzi.
@@ -24,6 +24,7 @@
#include <stdlib.h> #include <stdlib.h>
#include <stdio.h> #include <stdio.h>
#include "std.h"
#include "math/pprz_geodetic.h" #include "math/pprz_geodetic.h"
#include "math/pprz_geodetic_double.h" #include "math/pprz_geodetic_double.h"
#include "math/pprz_geodetic_float.h" #include "math/pprz_geodetic_float.h"
@@ -33,15 +34,63 @@
#include "generated/airframe.h" #include "generated/airframe.h"
#include "generated/flight_plan.h" #include "generated/flight_plan.h"
#include "generated/modules.h"
#include "firmwares/rover/guidance/rover_guidance_steering.h"
#include "state.h" #include "state.h"
// Check if rover firmware // Check if rover firmware
#ifndef ROVER_FIRMWARE #ifndef ROVER_FIRMWARE
#error "The module nps_fdm_rover is designed for rovers and doesn't support other firmwares!!" #error "The module nps_fdm_rover is designed for rovers and doesn't support other firmwares!!"
#endif #endif
#if (defined COMMAND_STEERING) && (defined COMMAND_THROTTLE) // STEERING ROVER PHYSICS
// Friction coef, proportional to speed
#ifndef NPS_ROVER_FRICTION
#define NPS_ROVER_FRICTION 0.01f
#endif
//// Used for car-like rover (steering wheels)
// Maximum acceleration (m/s²)
#ifndef NPS_ROVER_ACCELERATION
#define NPS_ROVER_ACCELERATION 1.f
#endif
/** Physical model parameters **/
static float mu = NPS_ROVER_FRICTION;
#endif
//// Used for 2-wheels rover
#if (defined COMMAND_TURN) && (defined COMMAND_SPEED) // 2 wheels rover dynamic
// Maximum speed (m/s)
#ifndef NPS_ROVER_MAX_SPEED
#define NPS_ROVER_MAX_SPEED 2.f
#endif
// Maximum turn rate (rad/s)
#ifndef NPS_ROVER_TURN_RATE
#define NPS_ROVER_TURN_RATE 3.f
#endif
// Accel 1st order time constant
#ifndef NPS_ROVER_ACCEL_TAU
#define NPS_ROVER_ACCEL_TAU 0.2f
#endif
// Turn rate 1st order time constant
#ifndef NPS_ROVER_TURN_TAU
#define NPS_ROVER_TURN_TAU 0.1f
#endif
#include "filters/low_pass_filter.h"
static struct FirstOrderLowPass speed_filter;
static struct FirstOrderLowPass turn_filter;
#endif // 2 wheels
// NpsFdm structure // NpsFdm structure
struct NpsFdm fdm; struct NpsFdm fdm;
@@ -52,36 +101,37 @@ static struct LtpDef_d ltpdef;
static void init_ltp(void); static void init_ltp(void);
/** Physical model structures **/ /** Physical model structures **/
static struct EnuCoor_d rover_pos; static struct NedCoor_d rover_pos;
static struct EnuCoor_d rover_vel; static struct NedCoor_d rover_vel;
static struct EnuCoor_d rover_acc; static struct NedCoor_d rover_acc;
/** Physical model parameters **/
static float mu = 0.01;
/** NPS FDM rover init ***************************/ /** NPS FDM rover init ***************************/
void nps_fdm_init(double dt) void nps_fdm_init(double dt)
{ {
fdm.init_dt = dt; // (1 / simulation freq) fdm.init_dt = dt; // (1 / simulation freq)
fdm.curr_dt = 0.001; // ¿Configurable from GCS? fdm.curr_dt = dt;
fdm.time = dt; fdm.time = 0;
fdm.on_ground = TRUE; fdm.on_ground = TRUE;
fdm.nan_count = 0; fdm.nan_count = 0;
fdm.pressure = -1; fdm.pressure = -1;
fdm.pressure_sl = PPRZ_ISA_SEA_LEVEL_PRESSURE; fdm.pressure_sl = PPRZ_ISA_SEA_LEVEL_PRESSURE;
fdm.total_pressure = -1; fdm.total_pressure = -1;
fdm.dynamic_pressure = -1; fdm.dynamic_pressure = -1;
fdm.temperature = -1; fdm.temperature = -1;
fdm.ltpprz_to_body_eulers.psi = 0.0; fdm.ltpprz_to_body_eulers.psi = 0.0;
init_ltp(); init_ltp();
#if (defined COMMAND_TURN) && (defined COMMAND_SPEED) // 2 wheels rover dynamic
init_first_order_low_pass(&speed_filter, NPS_ROVER_ACCEL_TAU, fdm.curr_dt, 0.f);
init_first_order_low_pass(&turn_filter, NPS_ROVER_TURN_TAU, fdm.curr_dt, 0.f);
#endif
} }
void nps_fdm_run_step(bool launch __attribute__((unused)), double *commands, int commands_nb __attribute__((unused))) void nps_fdm_run_step(bool launch __attribute__((unused)), double *commands, int commands_nb __attribute__((unused)))
{ {
fdm.time += fdm.curr_dt;
/**************************************************************************** /****************************************************************************
PHYSICAL MODEL PHYSICAL MODEL
@@ -90,71 +140,80 @@ void nps_fdm_run_step(bool launch __attribute__((unused)), double *commands, int
the LTP plane (so we transfer every integration step to NED and ECEF at the end). the LTP plane (so we transfer every integration step to NED and ECEF at the end).
*/ */
#ifdef COMMAND_STEERING // STEERING ROVER PHYSICS // From previous step...
ned_of_ecef_point_d(&rover_pos, &ltpdef, &fdm.ecef_pos);
ned_of_ecef_vect_d(&rover_vel, &ltpdef, &fdm.ecef_ecef_vel);
double phi = fdm.ltpprz_to_body_eulers.psi;
// Steering rover cmds: #if (defined COMMAND_STEERING) && (defined COMMAND_THROTTLE) // STEERING ROVER PHYSICS
// Steering rover cmds:
// COMMAND_STEERING -> delta parameter // COMMAND_STEERING -> delta parameter
// COMMAND_TRHOTTLE -> acceleration in heading direction // COMMAND_THROTTLE -> acceleration in heading direction
double delta = RadOfDeg(commands[COMMAND_STEERING] * MAX_DELTA);
/** Physical model for car-like robots .................. **/ /** Physical model for car-like robots .................. **/
// From previous step... double delta = RadOfDeg(commands[COMMAND_STEERING] * MAX_DELTA);
enu_of_ecef_point_d(&rover_pos, &ltpdef, &fdm.ecef_pos);
enu_of_ecef_vect_d(&rover_vel, &ltpdef, &fdm.ecef_ecef_vel);
double speed = FLOAT_VECT2_NORM(rover_vel); double speed = FLOAT_VECT2_NORM(rover_vel);
double phi = fdm.ltpprz_to_body_eulers.psi; double phi_d = tan(delta) / DRIVE_SHAFT_DISTANCE * speed;
double phi_d = tan(delta) / DRIVE_SHAFT_DISTANCE * speed; double accel = NPS_ROVER_ACCELERATION * commands[COMMAND_THROTTLE];
// Setting accelerations
rover_acc.x = commands[COMMAND_THROTTLE] * cos(phi) - speed * (sin(phi) * phi_d + cos(phi) * mu);
rover_acc.y = commands[COMMAND_THROTTLE] * sin(phi) + speed * (cos(phi) * phi_d - sin(phi) * mu);
double phi_dd = tan(delta) / DRIVE_SHAFT_DISTANCE * commands[COMMAND_THROTTLE];
// NED accel = R(psi) [accel; |V|*phi_d] - mu*V
rover_acc.x = accel * cos(phi) - speed * phi_d * sin(phi) - mu * rover_vel.x;
rover_acc.y = accel * sin(phi) + speed * phi_d * cos(phi) - mu * rover_vel.y;
// Velocities (EULER INTEGRATION) // Velocities (EULER INTEGRATION)
rover_vel.x += rover_acc.x * fdm.curr_dt; rover_vel.x += rover_acc.x * fdm.curr_dt;
rover_vel.y += rover_acc.y * fdm.curr_dt; rover_vel.y += rover_acc.y * fdm.curr_dt;
phi_d = tan(delta) / DRIVE_SHAFT_DISTANCE * speed;
// Positions #elif (defined COMMAND_TURN) && (defined COMMAND_SPEED) // 2 wheels rover dynamic
rover_pos.x += rover_vel.x * fdm.curr_dt;
rover_pos.y += rover_vel.y * fdm.curr_dt; double phi_sp = NPS_ROVER_TURN_RATE * commands[COMMAND_TURN];
phi += phi_d * fdm.curr_dt; double phi_d = update_first_order_low_pass(&turn_filter, phi_sp);
double speed_sp = NPS_ROVER_MAX_SPEED * commands[COMMAND_SPEED];
// phi have to be contained in [-180º,180º). So: double speed = update_first_order_low_pass(&speed_filter, speed_sp);
phi = (phi > M_PI)? - 2*M_PI + phi : (phi < -M_PI)? 2*M_PI + phi : phi; double vx = speed * cos(phi);
double vy = speed * sin(phi);
rover_acc.x = (vx - rover_vel.x) / fdm.curr_dt;
rover_acc.y = (vy - rover_vel.y) / fdm.curr_dt;
rover_vel.x = vx;
rover_vel.y = vy;
#else #else
#warning "The physics of this rover are not yet implemented in nps_fdm_rover!!" #warning "The physics of this rover are not yet implemented in nps_fdm_rover!!"
#endif // STEERING ROVER PHYSICS #endif // STEERING ROVER PHYSICS
// Positions
rover_pos.x += rover_vel.x * fdm.curr_dt;
rover_pos.y += rover_vel.y * fdm.curr_dt;
// phi have to be contained in [-180º,180º). So:
phi += phi_d * fdm.curr_dt;
NormRadAngle(phi);
/****************************************************************************/ /****************************************************************************/
// Coordenates transformations | // Coordenates transformations |
// ----------------------------| // ----------------------------|
/* in ECEF */ /* in ECEF */
ecef_of_enu_point_d(&fdm.ecef_pos, &ltpdef, &rover_pos); ecef_of_ned_point_d(&fdm.ecef_pos, &ltpdef, &rover_pos);
ecef_of_enu_vect_d(&fdm.ecef_ecef_vel, &ltpdef, &rover_vel); ecef_of_ned_vect_d(&fdm.ecef_ecef_vel, &ltpdef, &rover_vel);
ecef_of_enu_vect_d(&fdm.ecef_ecef_accel, &ltpdef, &rover_acc); ecef_of_ned_vect_d(&fdm.ecef_ecef_accel, &ltpdef, &rover_acc);
/* in NED */ /* in NED */
ned_of_ecef_point_d(&fdm.ltpprz_pos, &ltpdef, &fdm.ecef_pos); ned_of_ecef_point_d(&fdm.ltpprz_pos, &ltpdef, &fdm.ecef_pos);
ned_of_ecef_vect_d(&fdm.ltpprz_ecef_vel, &ltpdef, &fdm.ecef_ecef_vel); ned_of_ecef_vect_d(&fdm.ltpprz_ecef_vel, &ltpdef, &fdm.ecef_ecef_vel);
ned_of_ecef_vect_d(&fdm.ltpprz_ecef_accel, &ltpdef, &fdm.ecef_ecef_accel); ned_of_ecef_vect_d(&fdm.ltpprz_ecef_accel, &ltpdef, &fdm.ecef_ecef_accel);
/* Height above sea level */
fdm.hmsl = ltpdef.hmsl - rover_pos.z;
/* Eulers */ /* Eulers */
fdm.ltpprz_to_body_eulers.psi = phi; fdm.ltpprz_to_body_eulers.psi = phi;
fdm.ltp_to_body_eulers.psi = phi;
// ENU to NED and exporting heading (psi euler angle)
fdm.ltp_to_body_eulers.psi = - phi + M_PI / 2;
// Exporting Eulers to AHRS (in quaternions) // Exporting Eulers to AHRS (in quaternions)
double_quat_of_eulers(&fdm.ltp_to_body_quat, &fdm.ltp_to_body_eulers); double_quat_of_eulers(&fdm.ltp_to_body_quat, &fdm.ltp_to_body_eulers);
// Angular vel & acc // Angular vel & acc
fdm.body_ecef_rotvel.r = phi_d; fdm.body_ecef_rotaccel.r = (phi_d - fdm.body_ecef_rotvel.r) / fdm.curr_dt;
fdm.body_ecef_rotaccel.r = phi_dd; fdm.body_ecef_rotvel.r = phi_d;
} }
@@ -165,18 +224,19 @@ void nps_fdm_run_step(bool launch __attribute__((unused)), double *commands, int
static void init_ltp(void) static void init_ltp(void)
{ {
struct LlaCoor_d llh_nav0;
struct LlaCoor_d llh_nav0; /* Height above the ellipsoid */
llh_nav0.lat = RadOfDeg((double)NAV_LAT0 / 1e7); llh_nav0.lat = RadOfDeg((double)NAV_LAT0 / 1e7);
llh_nav0.lon = RadOfDeg((double)NAV_LON0 / 1e7); llh_nav0.lon = RadOfDeg((double)NAV_LON0 / 1e7);
llh_nav0.alt = (double)(NAV_ALT0 + NAV_MSL0) / 1000.; /* Height above the ellipsoid */
struct EcefCoor_d ecef_nav0; struct EcefCoor_d ecef_nav0;
ecef_of_lla_d(&ecef_nav0, &llh_nav0); ecef_of_lla_d(&ecef_nav0, &llh_nav0);
ltp_def_from_ecef_d(&ltpdef, &ecef_nav0); ltp_def_from_ecef_d(&ltpdef, &ecef_nav0);
fdm.ecef_pos = ecef_nav0; ltpdef.hmsl = (double)NAV_ALT0 / 1000.; /* Height above the geoid */
fdm.ecef_pos = ecef_nav0;
fdm.hmsl = ltpdef.hmsl;
fdm.ltp_g.x = 0.; fdm.ltp_g.x = 0.;
fdm.ltp_g.y = 0.; fdm.ltp_g.y = 0.;
fdm.ltp_g.z = 0.; // accel data are already with the correct format fdm.ltp_g.z = 0.; // accel data are already with the correct format
@@ -188,9 +248,9 @@ static void init_ltp(void)
fdm.ltp_h.z = AHRS_H_Z; fdm.ltp_h.z = AHRS_H_Z;
PRINT_CONFIG_MSG("Using magnetic field as defined in airframe file.") PRINT_CONFIG_MSG("Using magnetic field as defined in airframe file.")
#else #else
fdm.ltp_h.x = 0.4912; fdm.ltp_h.x = 1.;
fdm.ltp_h.y = 0.1225; fdm.ltp_h.y = 0.;
fdm.ltp_h.z = 0.8624; fdm.ltp_h.z = 0.;
#endif #endif
} }