mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-21 11:55:41 +08:00
Rover steering wheel (#2845)
* Steering wheel rover main files and guidance * Fix: Undeclared function nav_reset_alt warning * GVF mission flight plan * NPS FDM for rovers * GVF low_level_control.c included in gvf.c * Rover main loop for non chibios arch (copy/paste from rotorcraft) * Delete conf/modules/rover.xml The creation of this file was a mistake. * NPS for rover fixed * rover_guidance_steering periodic -> autopilot state machine * Fixes and comments
This commit is contained in:
Jesús Bautista Villar
committed by
GitHub
GitHub
parent
f5fee3c2cb
commit
4d4fbc2852
@@ -0,0 +1,127 @@
|
||||
<!DOCTYPE airframe SYSTEM "../airframe.dtd">
|
||||
|
||||
<airframe name="Rover Steering">
|
||||
|
||||
<firmware name="rover">
|
||||
<autopilot name="rover_steering.xml"/>
|
||||
|
||||
<target name="ap" board="matek_f765_wing">
|
||||
<configure name="PERIODIC_FREQUENCY" value="100"/>
|
||||
<module name="radio_control" type="sbus">
|
||||
<!--configure name="SBUS_PORT" value="UART6"/-->
|
||||
</module>
|
||||
</target>
|
||||
|
||||
<target name="nps" board="pc">
|
||||
<module name="radio_control" type="ppm"/>
|
||||
<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="actuators" type="pwm"/>
|
||||
<module name="telemetry" type="xbee_api"/>
|
||||
|
||||
<!-- Same board module as matek_f765_wind but without OSD -->
|
||||
<module name="board" type="matek_f765_car"/> <!-- IMU included -->
|
||||
|
||||
<!-- GPS_BAUD can be manually configured, but gps_ubx_ucenter automatically do that -->
|
||||
<module name="gps" type="ublox">
|
||||
<!--configure name="GPS_BAUD" value="B115200"/-->
|
||||
</module>
|
||||
<module name="gps_ubx_ucenter"/>
|
||||
|
||||
<module name="ins"/>
|
||||
|
||||
<!-- Either of this AHRS works fine with our rover -->
|
||||
<module name="ahrs" type="float_dcm">
|
||||
<define name="AHRS_FLOAT_MIN_SPEED_GPS_COURSE" value="0.1"/>
|
||||
</module>
|
||||
|
||||
<!--module name="ahrs" type="float_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-->
|
||||
|
||||
<module name="nav" type="rover_base"/>
|
||||
<module name="gvf" type="module"/>
|
||||
|
||||
<module name="guidance" type="rover_steering">
|
||||
<define name="MAX_DELTA" value="15.0"/>
|
||||
<define name="DRIVE_SHAFT_DISTANCE" value="0.25"/>
|
||||
<define name="SR_MEASURED_KF" value="1400.0"/>
|
||||
<define name="MIN_CMD_SHUT" value="3800"/>
|
||||
<define name="MAX_CMD_SHUT" value="1350"/>
|
||||
</module>
|
||||
|
||||
</firmware>
|
||||
|
||||
|
||||
<!-- COMMANDS SECTION ..................................................................... -->
|
||||
<servos>
|
||||
<!-- "no" 1 and 2 but no 0 because our board don't have S0 actuator entry... -->
|
||||
<servo name="MOTOR_THROTTLE" no="1" min="1000" neutral="1500" max="2000"/>
|
||||
<servo name="MOTOR_STEERING" no="2" min="1000" neutral="1500" max="2000"/>
|
||||
</servos>
|
||||
|
||||
<!-- Low level commands (PWM signal) -->
|
||||
<commands>
|
||||
<axis name="THROTTLE" failsafe_value="0"/>
|
||||
<axis name="STEERING" 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"/>
|
||||
</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="0." 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="AUTOPILOT">
|
||||
<define name="MODE_MANUAL" value="AP_MODE_DIRECT"/>
|
||||
<define name="MODE_AUTO1" value="AP_MODE_CRUISER"/>
|
||||
<define name="MODE_AUTO2" value="AP_MODE_NAV"/> <!-- Ignoring radio_control.values -->
|
||||
</section>
|
||||
|
||||
<section name="SIMULATOR" prefix="NPS_">
|
||||
<define name="SENSORS_PARAMS" value="nps_sensors_params_default.h" type="string"/>
|
||||
</section>
|
||||
|
||||
<section name="BAT">
|
||||
<define name="CATASTROPHIC_BAT_LEVEL" value="7.35" unit="V"/>
|
||||
<define name="CRITIC_BAT_LEVEL" value="7.40" unit="V"/>
|
||||
<define name="LOW_BAT_LEVEL" value="7.45" unit="V"/>
|
||||
<define name="MAX_BAT_LEVEL" value="8.44" unit="V"/>
|
||||
<define name="MILLIAMP_AT_FULL_THROTTLE" value="1800" unit="mA"/>
|
||||
<define name="MILLIAMP_AT_IDLE_THROTTLE" value="1000" unit="mA"/>
|
||||
<!--define name="CURRENT_ESTIMATION_NONLINEARITY" value="1.0"/-->
|
||||
</section>
|
||||
|
||||
<section name="GCS">
|
||||
<define name="AC_ICON" value="rover"/>
|
||||
</section>
|
||||
|
||||
</airframe>
|
||||
@@ -0,0 +1,122 @@
|
||||
<!DOCTYPE autopilot SYSTEM "autopilot.dtd">
|
||||
|
||||
<autopilot name="Steering Rover Autopilot">
|
||||
|
||||
<state_machine name="ap" freq="PERIODIC_FREQUENCY" gcs_mode="true" settings_mode="true">
|
||||
|
||||
<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="navigation.h"/>
|
||||
<include name="modules/radio_control/radio_control.h"/>
|
||||
<include name="guidance/rover_guidance_steering.h"/>
|
||||
<include name="state.h"/>
|
||||
<define name="RCLost()" value="(radio_control.status == RC_REALLY_LOST)"/>
|
||||
</includes>
|
||||
|
||||
<settings>
|
||||
<dl_setting var="autopilot.kill_throttle" min="0" step="1" max="1" module="autopilot" values="Resurrect|Kill" handler="KillThrottle"/>
|
||||
</settings>
|
||||
|
||||
<exceptions>
|
||||
<exception cond="nav.too_far_from_home" deroute="HOME"/>
|
||||
<exception cond="kill_switch_is_on()" deroute="KILL"/>
|
||||
</exceptions>
|
||||
|
||||
<!-- * AP MODES * ......................................................................... -->
|
||||
<!-- RC Manual -->
|
||||
<mode name="DIRECT" shortname="MANUAL">
|
||||
<select cond="RCMode0()"/>
|
||||
<on_enter>
|
||||
<call fun="rover_guidance_steering_pid_reset()"/>
|
||||
</on_enter>
|
||||
<control freq="NAVIGATION_FREQUENCY"> <!-- Only for display -->
|
||||
<call fun="nav_periodic_task()"/>
|
||||
</control>
|
||||
<control>
|
||||
<call fun="SetCommandsFromRC(commands, radio_control.values)"/>
|
||||
<call fun="SetActuatorsFromCommands(commands, autopilot_get_mode())"/>
|
||||
|
||||
<call fun="SetAPThrottleFromCommands(commands[COMMAND_THROTTLE])"/>
|
||||
</control>
|
||||
<exception cond="RCLost()" deroute="KILL"/>
|
||||
</mode>
|
||||
|
||||
<!-- Cruiser: Speed ctrl (from state speed on enter )+ Heading from RC -->
|
||||
<mode name="CRUISER" shortname="AUTO1">
|
||||
<select cond="RCMode1()"/>
|
||||
<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="commands[COMMAND_THROTTLE] = GetCmdFromThrottle(guidance_control.throttle);"/>
|
||||
<call fun="SetActuatorsFromCommands(commands, autopilot_get_mode())"/>
|
||||
|
||||
<call fun="SetAPThrottleFromCommands()"/>
|
||||
</control>
|
||||
<exception cond="RCLost()" deroute="KILL"/>
|
||||
</mode>
|
||||
|
||||
<!-- Navigation: Speed crtl (from state speed on enter) + heading ctrl (from GCS & flight plans) -->
|
||||
<mode name="NAV">
|
||||
<select cond="RCMode2()" exception="HOME"/>
|
||||
<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="rover_guidance_steering_heading_ctrl()"/>
|
||||
|
||||
<call fun="commands[COMMAND_STEERING] = GetCmdFromDelta(guidance_control.cmd.delta);"/>
|
||||
<call fun="commands[COMMAND_THROTTLE] = GetCmdFromThrottle(guidance_control.throttle);"/>
|
||||
<call fun="SetActuatorsFromCommands(commands, autopilot_get_mode())"/>
|
||||
|
||||
<call fun="SetAPThrottleFromCommands()"/>
|
||||
</control>
|
||||
<exception cond="GpsIsLost() && autopilot_in_flight()" deroute="KILL"/>
|
||||
</mode>
|
||||
|
||||
<!-- HOME (not yet implemented) -->
|
||||
<mode name="HOME">
|
||||
<control freq="NAVIGATION_FREQUENCY">
|
||||
<call fun="nav_home()"/>
|
||||
</control>
|
||||
<control>
|
||||
<call fun="SetActuatorsFromCommands(commands, autopilot_get_mode())"/>
|
||||
|
||||
<call fun="SetAPThrottleFromCommands()"/>
|
||||
</control>
|
||||
<exception cond="GpsIsLost()" deroute="KILL"/>
|
||||
</mode>
|
||||
|
||||
<!-- Kill throttle -->
|
||||
<mode name="KILL">
|
||||
<select cond="$DEFAULT_MODE"/>
|
||||
<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="SetAPThrottleFromCommands()"/>
|
||||
</control>
|
||||
</mode>
|
||||
|
||||
</state_machine>
|
||||
|
||||
</autopilot>
|
||||
@@ -0,0 +1,95 @@
|
||||
<!DOCTYPE flight_plan SYSTEM "../flight_plan.dtd">
|
||||
|
||||
<flight_plan alt="660" ground_alt="650" lat0="40.450631" lon0="-3.726058" max_dist_from_home="1500" name="Rover Steering" security_height="0.3">
|
||||
<header>
|
||||
|
||||
</header>
|
||||
<waypoints>
|
||||
<waypoint name="HOME" x="0.0" y="0.0"/>
|
||||
<waypoint name="STDBY" x="0" y="-4"/>
|
||||
<waypoint name="ELLIPSE" x="-3" y="-3"/>
|
||||
<waypoint name="P1" x="-2" y="-4"/>
|
||||
<waypoint name="P2" x="2" y="-4"/>
|
||||
<!--waypoint name="OBJ" x="2" y="-4"/-->
|
||||
|
||||
<!-- 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="40" y="40"/>
|
||||
<waypoint name="_N2" x="40" y="-40"/>
|
||||
<waypoint name="_N3" x="-40" y="-40"/>
|
||||
<waypoint name="_N4" x="-40" y="40"/>
|
||||
</waypoints>
|
||||
|
||||
<sectors>
|
||||
<sector name="Net" 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" 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"/>
|
||||
</variables>
|
||||
|
||||
<modules>
|
||||
<module name="gvf_module"/>
|
||||
</modules>
|
||||
|
||||
<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="NavSetGroundReferenceHere()"/>
|
||||
<call_once fun="NavSetAltitudeReferenceHere()"/>
|
||||
</block>
|
||||
|
||||
<block name="Start Engine">
|
||||
<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)"/>
|
||||
</block>
|
||||
|
||||
<block name="ellipse_wp">
|
||||
<call fun="gvf_ellipse_wp(WP_ELLIPSE, a, b, gvf_ellipse_par.alpha)"/>
|
||||
<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)"/>
|
||||
<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)"/>
|
||||
<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)"/>
|
||||
<exception cond="! InsideNet(GetPosX(), GetPosY())" deroute="Standby"/>
|
||||
</block>
|
||||
|
||||
|
||||
|
||||
</blocks>
|
||||
</flight_plan>
|
||||
@@ -0,0 +1,85 @@
|
||||
<!DOCTYPE flight_plan SYSTEM "../flight_plan.dtd">
|
||||
|
||||
<flight_plan alt="660" ground_alt="650" lat0="40.450631" lon0="-3.726058" max_dist_from_home="1500" name="Test UCM Fisicas" security_height="25">
|
||||
<header>
|
||||
|
||||
</header>
|
||||
<waypoints>
|
||||
<waypoint name="HOME" x="0" y="0"/>
|
||||
<waypoint alt="710.0" name="STDBY" x="21.5" y="-73.3"/>
|
||||
<waypoint alt="690.0" name="AF" x="108.8" y="56.7"/>
|
||||
<waypoint alt="660.0" name="TD" x="41.8" y="24.3"/>
|
||||
<waypoint name="_BASELEG" x="168.8" y="-13.8"/>
|
||||
<waypoint name="CLIMB" x="96.5" y="-15.5"/>
|
||||
<waypoint alt="710.0" name="ELLIPSE" x="79.0" y="-68.1"/>
|
||||
</waypoints>
|
||||
<variables>
|
||||
<variable init="0" max="100" min="0" step="5" var="ell_delta"/>
|
||||
</variables>
|
||||
<modules>
|
||||
<module name="gvf_module"/>
|
||||
<module name="gvf_parametric"/>
|
||||
</modules>
|
||||
<blocks>
|
||||
<block name="Wait GPS">
|
||||
<set value="1" var="autopilot.kill_throttle"/>
|
||||
<while cond="!GpsFixValid()"/>
|
||||
</block>
|
||||
<block name="Geo init">
|
||||
<while cond="LessThan(NavBlockTime(), 10)"/>
|
||||
<call_once fun="NavSetGroundReferenceHere()"/>
|
||||
</block>
|
||||
<block name="Holding point">
|
||||
<set value="1" var="autopilot.kill_throttle"/>
|
||||
<attitude roll="0" throttle="0" vmode="throttle"/>
|
||||
</block>
|
||||
<block group="home" key="t" name="Takeoff" strip_button="Takeoff (wp CLIMB)" strip_icon="takeoff.png">
|
||||
<exception cond="GetPosAlt() > GetAltRef()+25" deroute="Standby"/>
|
||||
<set value="0" var="autopilot.kill_throttle"/>
|
||||
<set value="0" var="autopilot.flight_time"/>
|
||||
<go from="HOME" pitch="15" throttle="1.0" vmode="throttle" wp="CLIMB"/>
|
||||
</block>
|
||||
<block group="home" key="Ctrl+a" name="Standby" strip_button="Standby" strip_icon="home.png">
|
||||
<call_once fun="NavVerticalAutoThrottleMode(0.0)"/>
|
||||
<call_once fun="NavVerticalAltitudeMode(flight_altitude, 0.0)"/>
|
||||
<call fun="gvf_ellipse_wp(WP_STDBY, nav_radius, nav_radius, 0)"/>
|
||||
</block>
|
||||
<block name="ellipse">
|
||||
<call_once fun="NavVerticalAutoThrottleMode(0.0)"/>
|
||||
<call_once fun="NavVerticalAltitudeMode(flight_altitude, 0.0)"/>
|
||||
<call fun="gvf_ellipse_wp(WP_ELLIPSE, gvf_ellipse_par.a, gvf_ellipse_par.b, gvf_ellipse_par.alpha)"/>
|
||||
</block>
|
||||
<block name="3D_ellipse">
|
||||
<call fun="gvf_parametric_3D_ellipse_wp_delta(WP_ELLIPSE,gvf_parametric_3d_ellipse_par.r,flight_altitude-ground_alt,ell_delta,gvf_parametric_3d_ellipse_par.alpha)"/>
|
||||
</block>
|
||||
<block name="3D_lissajous">
|
||||
<call fun="gvf_parametric_3D_lissajous_wp_center(WP_ELLIPSE,flight_altitude-ground_alt,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>
|
||||
<block name="2D_trefoil">
|
||||
<call_once fun="NavVerticalAutoThrottleMode(0.0)"/>
|
||||
<call_once fun="NavVerticalAltitudeMode(flight_altitude, 0.0)"/>
|
||||
<call fun="gvf_parametric_2D_trefoil_wp(WP_ELLIPSE,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 group="land" name="Land Right AF-TD" strip_button="Land right (wp AF-TD)" strip_icon="land-right.png">
|
||||
<set value="DEFAULT_CIRCLE_RADIUS" var="nav_radius"/>
|
||||
<deroute block="land"/>
|
||||
</block>
|
||||
<block group="land" name="Land Left AF-TD" strip_button="Land left (wp AF-TD)" strip_icon="land-left.png">
|
||||
<set value="-DEFAULT_CIRCLE_RADIUS" var="nav_radius"/>
|
||||
<deroute block="land"/>
|
||||
</block>
|
||||
<block name="land">
|
||||
<call_once fun="nav_compute_baseleg(WP_AF, WP_TD, WP__BASELEG, nav_radius)"/>
|
||||
<circle radius="nav_radius" until="NavCircleCount() > 0.5" wp="_BASELEG"/>
|
||||
<circle radius="nav_radius" until="And(NavQdrCloseTo(DegOfRad(baseleg_out_qdr)-(nav_radius/fabs(nav_radius))*10), 10 > fabs(GetPosAlt() - WaypointAlt(WP__BASELEG)))" wp="_BASELEG"/>
|
||||
</block>
|
||||
<block name="final">
|
||||
<exception cond="GetAltRef() + 10 > GetPosAlt()" deroute="flare"/>
|
||||
<go from="AF" hmode="route" vmode="glide" wp="TD"/>
|
||||
</block>
|
||||
<block name="flare">
|
||||
<go approaching_time="0" from="AF" hmode="route" throttle="0.0" vmode="throttle" wp="TD"/>
|
||||
<attitude roll="0.0" throttle="0.0" until="FALSE" vmode="throttle"/>
|
||||
</block>
|
||||
</blocks>
|
||||
</flight_plan>
|
||||
@@ -0,0 +1,41 @@
|
||||
<!DOCTYPE module SYSTEM "module.dtd">
|
||||
|
||||
<module name="board_matek_f765_car" dir="boards">
|
||||
<doc>
|
||||
<description>
|
||||
Autoload several onboard sensors and subsystems
|
||||
for the Matek F765 Wing board with proper configuration fixed for rovers.
|
||||
IMU ICM20602 (auto-detected from MPU6000 driver)
|
||||
Baro (BMP280)
|
||||
OSD (desactivated for rovers)
|
||||
Normal back of the board is on the battery/ESC wires
|
||||
Normal up of the board is on MCU side.
|
||||
</description>
|
||||
<configure name="BOARD_MATEK_ROTATED" value="FALSE|TRUE" description="if TRUE, the board is not using is default orientation and axis can be redefined by hand"/>
|
||||
</doc>
|
||||
<dep>
|
||||
<depends>spi_master,baro_bmp280_i2c,current_sensor</depends>
|
||||
</dep>
|
||||
<autoload name="imu" type="mpu6000"/>
|
||||
|
||||
<makefile target="!sim|nps|fbw">
|
||||
<!-- IMU CONFIGURATION -->
|
||||
<configure name="IMU_MPU_SPI_DEV" value="spi3" case="upper|lower"/>
|
||||
<configure name="IMU_MPU_SPI_SLAVE_IDX" value="SPI_SLAVE4"/>
|
||||
<define name="IMU_MPU_GYRO_RANGE" value="MPU60X0_GYRO_RANGE_1000"/>
|
||||
<define name="IMU_MPU_ACCEL_RANGE" value="MPU60X0_ACCEL_RANGE_8G"/>
|
||||
<define name="IMU_MPU_CHAN_X" value="1" cond="ifeq (,$(findstring $(BOARD_MATEK_ROTATED),1 TRUE))"/>
|
||||
<define name="IMU_MPU_CHAN_Y" value="0" cond="ifeq (,$(findstring $(BOARD_MATEK_ROTATED),1 TRUE))"/>
|
||||
<define name="IMU_MPU_X_SIGN" value="-1" cond="ifeq (,$(findstring $(BOARD_MATEK_ROTATED),1 TRUE))"/>
|
||||
<!-- BAROMETER BMP280 CONFIGURATION -->
|
||||
<configure name="BMP280_I2C_DEV" value="i2c2"/>
|
||||
<!-- OSD CONFIGURATION -->
|
||||
<configure name="MAX7456_SPI_DEV" value="spi2"/>
|
||||
<configure name="MAX7456_SLAVE_IDX" value="SPI_SLAVE5"/>
|
||||
<define name="USE_MATEK_TYPE_OSD_CHIP" value="false" />
|
||||
<define name="USE_PAL_FOR_OSD_VIDEO" value="false" />
|
||||
<define name="BARO_ALTITUDE_VAR" value="baro_alt" /> <!-- if non defined the default var is ''baro_alt'' -->
|
||||
<!-- CURRENT SENSOR -->
|
||||
<configure name="ADC_CURRENT_SENSOR" value="ADC_4"/>
|
||||
</makefile>
|
||||
</module>
|
||||
@@ -0,0 +1,18 @@
|
||||
<!DOCTYPE module SYSTEM "module.dtd">
|
||||
|
||||
<module name="fdm_rover" dir="fdm">
|
||||
<doc>
|
||||
<description>
|
||||
Rover FDM for NPS simulator
|
||||
|
||||
This module introduces the foundations to construct any rover physics for the NPS simulator.
|
||||
If you have a non designed rover physiscs, in nps_fdm_rover.c you can find the instructions
|
||||
to build it.
|
||||
</description>
|
||||
</doc>
|
||||
<header/>
|
||||
<makefile target="nps|hitl">
|
||||
<file name="nps_fdm_rover.c" dir="nps"/>
|
||||
</makefile>
|
||||
</module>
|
||||
|
||||
@@ -27,5 +27,9 @@ $(error "Rover firmware should use generated autopilot")
|
||||
endif
|
||||
</raw>
|
||||
</makefile>
|
||||
<makefile target="nps|hitl">
|
||||
<define name="AP"/>
|
||||
<file name="nps_autopilot_rover.c" dir="nps"/>
|
||||
</makefile>
|
||||
</module>
|
||||
|
||||
|
||||
@@ -0,0 +1,46 @@
|
||||
<!DOCTYPE module SYSTEM "module.dtd">
|
||||
|
||||
<module name="guidance_rover_steering" dir="guidance" task="control">
|
||||
<doc>
|
||||
<description>
|
||||
Steering guidance for rover.
|
||||
</description>
|
||||
<!-- TODO: Rover hardware configuration without defining global var.
|
||||
<configure name="MAX_DELTA_C" value="15.0"/>
|
||||
<configure name="MIN_DELTA_C" value="-$(MAX_DELTA_C)"/>
|
||||
<configure name="DRIVE_SHAFT_DISTANCE_C" value="0.25"/>
|
||||
-->
|
||||
</doc>
|
||||
|
||||
<settings name="SR Guidance">
|
||||
<dl_settings>
|
||||
<dl_settings NAME="SR Guidance">
|
||||
<dl_settings NAME="Steering control">
|
||||
<!--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_settings>
|
||||
<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="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.ki" shortname="ki"/>
|
||||
</dl_settings>
|
||||
</dl_settings>
|
||||
</dl_settings>
|
||||
</settings>
|
||||
|
||||
<dep>
|
||||
<depends>@navigation</depends>
|
||||
<provides>guidance,commands</provides>
|
||||
</dep>
|
||||
|
||||
<header>
|
||||
<file name="rover_guidance_steering.h"/>
|
||||
</header>
|
||||
|
||||
<init fun="rover_guidance_steering_init()"/>
|
||||
<makefile target="ap|nps" firmware="rover">
|
||||
<file name="rover_guidance_steering.c" dir="$(SRC_FIRMWARE)/guidance"/>
|
||||
</makefile>
|
||||
|
||||
</module>
|
||||
@@ -78,5 +78,13 @@ For more details we refer to https://wiki.paparazziuav.org/wiki/Module/guidance_
|
||||
<file name="nav/nav_survey_polygon_gvf.c"/>
|
||||
</makefile>
|
||||
|
||||
<makefile firmware="rover">
|
||||
<file name="gvf.c"/>
|
||||
<file name="trajectories/gvf_line.c"/>
|
||||
<file name="trajectories/gvf_sin.c"/>
|
||||
<file name="trajectories/gvf_ellipse.c"/>
|
||||
<!--file name="nav/nav_survey_polygon_gvf.c"/-->
|
||||
</makefile>
|
||||
|
||||
</module>
|
||||
|
||||
|
||||
@@ -0,0 +1,63 @@
|
||||
<?xml version="1.0"?>
|
||||
<!-- $Id$
|
||||
--
|
||||
-- (c) 2013 Gautier Hattenberger
|
||||
--
|
||||
-- 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.
|
||||
-->
|
||||
|
||||
<!--
|
||||
-- Attributes of root (Radio) tag :
|
||||
-- name: name of RC
|
||||
-- data_min: min width of a pulse to be considered as a data pulse
|
||||
-- data_max: max width of a pulse to be considered as a data pulse
|
||||
-- sync_min: min width of a pulse to be considered as a synchro pulse
|
||||
-- sync_max: max width of a pulse to be considered as a synchro pulse
|
||||
-- pulse_type: POSITIVE ( Futaba and others) | NEGATIVE (JR)
|
||||
-- min, max and sync are expressed in micro-seconds
|
||||
-->
|
||||
|
||||
<!--
|
||||
-- Attributes of channel tag :
|
||||
-- function: logical command
|
||||
-- average: (boolean) channel filtered through several frames (for discrete commands)
|
||||
-- min: minimum pulse length (micro-seconds)
|
||||
-- max: maximum pulse length (micro-seconds)
|
||||
-- neutral: neutral pulse length (micro-seconds)
|
||||
-- Note: a command may be reversed by exchanging min and max values
|
||||
-->
|
||||
|
||||
<!DOCTYPE radio SYSTEM "../radio.dtd">
|
||||
<radio name="Futaba T16SZ with SBUS" data_min="1000" data_max="2000" sync_min ="5000" sync_max ="15000" pulse_type="POSITIVE">
|
||||
<channel function="ROLL" min="1000" neutral="1500" max="2000" average="0"/>
|
||||
<channel function="PITCH" min="1000" neutral="1500" max="2000" average="0"/>
|
||||
<channel function="THROTTLE" min="1000" neutral="1500" max="2000" average="0"/>
|
||||
<channel function="YAW" min="1000" neutral="1500" max="2000" average="0"/>
|
||||
<channel function="MODE" min="1000" neutral="1520" max="2000" average="1" reverse="1"/>
|
||||
<channel function="GAIN1" min="1000" neutral="1520" max="2000" average="0"/>
|
||||
<channel function="GAIN2" min="1000" neutral="1520" max="2000" average="0"/>
|
||||
<channel function="GAIN3" min="1000" neutral="1500" max="2000" average="0"/>
|
||||
<channel function="GAIN4" min="1000" neutral="1500" max="2000" average="0"/>
|
||||
<channel function="GAIN5" min="1000" neutral="2000" max="2000" average="0" reverse="1"/>
|
||||
<channel function="GAIN6" min="1000" neutral="1500" max="2000" average="0"/>
|
||||
<channel function="GAIN7" min="1000" neutral="1520" max="2000" average="1" reverse="1"/>
|
||||
<channel function="GAIN8" min="1000" neutral="1520" max="2000" average="0"/>
|
||||
<channel function="GAIN9" min="1000" neutral="1520" max="2000" average="0"/>
|
||||
<channel function="GAIN10" min="1000" neutral="1500" max="2000" average="0"/>
|
||||
<channel function="GAIN11" min="1000" neutral="1500" max="2000" average="0"/>
|
||||
</radio>
|
||||
@@ -0,0 +1,119 @@
|
||||
/*
|
||||
* Copyright (C) 2021 Jesús Bautista <jesusbautistavillar@gmail.com>
|
||||
* Hector García <noeth3r@gmail.com>
|
||||
*
|
||||
* 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/>.
|
||||
*/
|
||||
|
||||
#define AUTOPILOT_CORE_GUIDANCE_C
|
||||
|
||||
/** Mandatory dependencies header **/
|
||||
#include "firmwares/rover/guidance/rover_guidance_steering.h"
|
||||
|
||||
#include "generated/airframe.h"
|
||||
|
||||
#include "modules/actuators/actuators_default.h"
|
||||
#include "modules/radio_control/radio_control.h"
|
||||
#include "autopilot.h"
|
||||
#include "navigation.h"
|
||||
#include "state.h"
|
||||
|
||||
#include "filters/pid.h" // Used for p+i speed controller
|
||||
|
||||
#include <math.h>
|
||||
#include <stdio.h>
|
||||
|
||||
// Guidance control main variables
|
||||
rover_ctrl guidance_control;
|
||||
|
||||
static struct PID_f rover_pid;
|
||||
static float time_step;
|
||||
static float last_speed_cmd;
|
||||
static uint8_t last_ap_mode;
|
||||
|
||||
/** INIT function **/
|
||||
void rover_guidance_steering_init(void)
|
||||
{
|
||||
guidance_control.cmd.delta = 0.0;
|
||||
guidance_control.cmd.speed = 0.0;
|
||||
guidance_control.throttle = 0.0;
|
||||
|
||||
last_speed_cmd = 0.0;
|
||||
last_ap_mode = AP_MODE_KILL;
|
||||
|
||||
guidance_control.speed_error = 0.0;
|
||||
guidance_control.kf = SR_MEASURED_KF;
|
||||
guidance_control.kp = 10;
|
||||
guidance_control.ki = 100;
|
||||
|
||||
init_pid_f(&rover_pid, guidance_control.kp, 0.f, guidance_control.ki, MAX_PPRZ*0.2);
|
||||
|
||||
// Based on autopilot state machine frequency
|
||||
time_step = 1.f/PERIODIC_FREQUENCY;
|
||||
}
|
||||
|
||||
/** CTRL functions **/
|
||||
// Steering control (GVF)
|
||||
void rover_guidance_steering_heading_ctrl(void)
|
||||
{
|
||||
float delta = 0.0;
|
||||
float omega = guidance_control.gvf_omega; //GVF give us this omega
|
||||
|
||||
// Speed is bounded to avoid GPS noise while driving at small velocity
|
||||
float speed = BoundSpeed(stateGetHorizontalSpeedNorm_f());
|
||||
|
||||
if (fabs(omega)>0.0) {
|
||||
delta = DegOfRad(-atanf(omega*DRIVE_SHAFT_DISTANCE/speed));
|
||||
}
|
||||
|
||||
guidance_control.cmd.delta = BoundDelta(delta);
|
||||
}
|
||||
|
||||
// Speed control (feed feed forward + propotional + integral controler) (PID)
|
||||
void rover_guidance_steering_speed_ctrl(void)
|
||||
{
|
||||
// - Looking for setting update
|
||||
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());
|
||||
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));
|
||||
}
|
||||
|
||||
|
||||
/** PID RESET function**/
|
||||
void rover_guidance_steering_pid_reset(void)
|
||||
{
|
||||
// Reset speed PID
|
||||
if (rover_pid.sum != 0) {
|
||||
reset_pid_f(&rover_pid);
|
||||
}
|
||||
}
|
||||
|
||||
void rover_guidance_steering_kill(void)
|
||||
{
|
||||
guidance_control.cmd.delta = 0.0;
|
||||
guidance_control.cmd.speed = 0.0;
|
||||
}
|
||||
@@ -0,0 +1,153 @@
|
||||
/*
|
||||
* Copyright (C) 2021 Jesús Bautista <jesusbautistavillar@gmail.com>
|
||||
* Hector García <noeth3r@gmail.com>
|
||||
*
|
||||
* 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/>.
|
||||
*/
|
||||
|
||||
#ifndef ROVER_GUIDANCE_STEERING_H
|
||||
#define ROVER_GUIDANCE_STEERING_H
|
||||
|
||||
/** Generated airframe.h from airframe.xml
|
||||
* - fun: SetActuatorsFromCommands
|
||||
* - var: commands
|
||||
* - var: hardware and construction parameters
|
||||
**/
|
||||
|
||||
#include "std.h"
|
||||
#include <math.h>
|
||||
|
||||
#include "generated/airframe.h"
|
||||
|
||||
// Check critical global definitiones
|
||||
#ifndef SERVO_MOTOR_THROTTLE
|
||||
#error "Steering rover firmware requires the servo MOTOR_THROTTLE"
|
||||
#endif
|
||||
|
||||
#ifndef SERVO_MOTOR_STEERING
|
||||
#error "Steering rover firmware requires the servo MOTOR_STEERING"
|
||||
#endif
|
||||
|
||||
#ifndef COMMAND_THROTTLE
|
||||
#error "Steering rover firmware requires the command COMMAND_THROTTLE"
|
||||
#endif
|
||||
|
||||
#ifndef COMMAND_STEERING
|
||||
#error "Steering rover firmware requires the command COMMAND_STEERING"
|
||||
#endif
|
||||
|
||||
|
||||
/** Global variables definitions **/
|
||||
|
||||
// MIN_DELTA, MAX_DELTA: Min and max wheels turning angle (deg)
|
||||
// You should measure this angle if you want to have an
|
||||
// efficient control in your steering
|
||||
#ifndef MAX_DELTA
|
||||
#define MAX_DELTA 15.0
|
||||
#endif
|
||||
#ifndef MIN_DELTA
|
||||
#define MIN_DELTA MAX_DELTA
|
||||
#endif
|
||||
|
||||
// This variables allow you to configurate de max and min
|
||||
// steering actuator signal. There is a mecanic limitation
|
||||
// for the actuator in the steering of our rover, so we have
|
||||
// to limit the actuator travel.
|
||||
#ifndef MAX_CMD_SHUT
|
||||
#define MAX_CMD_SHUT 0
|
||||
#endif
|
||||
#ifndef MIN_CMD_SHUT
|
||||
#define MIN_CMD_SHUT 0
|
||||
#endif
|
||||
|
||||
// MIN_SPEED, MAX_SPEED: Min and max state speed (m/s)
|
||||
#ifndef MAX_SPEED
|
||||
#define MAX_SPEED 999.0 //We don't really use that variable
|
||||
#endif
|
||||
#ifndef MIN_SPEED
|
||||
#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).
|
||||
|
||||
// DRIVE_SHAFT_DISTANCE: Distance between front and rear wheels (m)
|
||||
#ifndef DRIVE_SHAFT_DISTANCE
|
||||
#define DRIVE_SHAFT_DISTANCE 0.25
|
||||
#warning "Construction variable DRIVE_SHAFT_DISTANCE for steering wheel rover not defined"
|
||||
#endif
|
||||
|
||||
// SR_MEASURED_KF: Lineal feed forward control constant (have to be measured in new servos)
|
||||
#ifndef SR_MEASURED_KF
|
||||
#define SR_MEASURED_KF 10
|
||||
#warning "Construction constant SR_MEASURED_KF for steering wheel rover not defined"
|
||||
#endif
|
||||
|
||||
|
||||
/** Steering rover guidance STRUCTURES **/
|
||||
// High level commands
|
||||
typedef struct {
|
||||
float speed;
|
||||
float delta;
|
||||
} sr_cmd_t;
|
||||
|
||||
// Main structure
|
||||
typedef struct {
|
||||
sr_cmd_t cmd;
|
||||
float gvf_omega;
|
||||
float throttle;
|
||||
|
||||
float speed_error;
|
||||
float kf;
|
||||
float kp;
|
||||
float ki;
|
||||
} rover_ctrl;
|
||||
|
||||
extern rover_ctrl guidance_control;
|
||||
|
||||
/** Steering rover guidance EXT FUNCTIONS **/
|
||||
extern void rover_guidance_steering_init(void);
|
||||
extern void rover_guidance_steering_heading_ctrl(void);
|
||||
extern void rover_guidance_steering_speed_ctrl(void);
|
||||
extern void rover_guidance_steering_pid_reset(void);
|
||||
extern void rover_guidance_steering_kill(void);
|
||||
|
||||
|
||||
/** MACROS **/
|
||||
// Bound delta
|
||||
#define BoundDelta(delta) (delta < -MIN_DELTA ? -MIN_DELTA : \
|
||||
(delta > MAX_DELTA ? MAX_DELTA : \
|
||||
delta));
|
||||
|
||||
// Bound speed
|
||||
#define BoundSpeed(speed) (speed < MIN_SPEED ? MIN_SPEED : \
|
||||
(speed > MAX_SPEED ? MAX_SPEED : \
|
||||
speed));
|
||||
|
||||
// Bound throttle
|
||||
#define BoundThrottle(throttle) TRIM_PPRZ((int)throttle);
|
||||
|
||||
// Set low level commands from high level commands
|
||||
#define GetCmdFromDelta(delta) (delta >= 0 ? -delta/MAX_DELTA * (MAX_PPRZ - (int)MAX_CMD_SHUT) : \
|
||||
-delta/MIN_DELTA * (MAX_PPRZ - (int)MIN_CMD_SHUT));
|
||||
|
||||
// This macro is for NAV state
|
||||
#define GetCmdFromThrottle(throttle) TRIM_PPRZ((int)throttle);
|
||||
|
||||
// Set AP throttle value
|
||||
#define SetAPThrottleFromCommands(void) { \
|
||||
autopilot.throttle = commands[COMMAND_THROTTLE]; \
|
||||
}
|
||||
|
||||
#endif // ROVER_GUIDANCE_STEERING_H
|
||||
@@ -174,6 +174,7 @@ extern void nav_home(void);
|
||||
extern void nav_set_manual(float speed, float turn);
|
||||
|
||||
extern void nav_reset_reference(void) __attribute__((unused));
|
||||
extern void nav_reset_alt(void) __attribute__((unused));
|
||||
extern void nav_periodic_task(void);
|
||||
|
||||
extern bool nav_is_in_flight(void);
|
||||
|
||||
@@ -28,9 +28,19 @@
|
||||
#include "modules/guidance/gvf/trajectories/gvf_line.h"
|
||||
#include "modules/guidance/gvf/trajectories/gvf_sin.h"
|
||||
|
||||
#ifdef FIXEDWING_FIRMWARE
|
||||
#include "firmwares/fixedwing/nav.h"
|
||||
#include "modules/nav/common_nav.h"
|
||||
#include "firmwares/fixedwing/stabilization/stabilization_attitude.h"
|
||||
#elif defined(ROVER_FIRMWARE)
|
||||
#include "state.h"
|
||||
#include "modules/nav/nav_rover_base.h"
|
||||
#include "firmwares/rover/navigation.h"
|
||||
struct EnuCoor_f gvf_draw_wp;
|
||||
#else
|
||||
#error "Firmware not supported by GVF!"
|
||||
#endif
|
||||
|
||||
#include "autopilot.h"
|
||||
|
||||
// Control
|
||||
@@ -83,7 +93,7 @@ static void send_circle(struct transport_tx *trans, struct link_device *dev)
|
||||
|
||||
if (delta_T < 200)
|
||||
if (gvf_trajectory.type == ELLIPSE &&
|
||||
(gvf_trajectory.p[2] == gvf_trajectory.p[3])) {
|
||||
((int)gvf_trajectory.p[2] == (int)gvf_trajectory.p[3])) {
|
||||
pprz_msg_send_CIRCLE(trans, dev, AC_ID,
|
||||
&gvf_trajectory.p[0], &gvf_trajectory.p[1],
|
||||
&gvf_trajectory.p[2]);
|
||||
@@ -156,11 +166,19 @@ void gvf_control_2D(float ke, float kn, float e,
|
||||
{
|
||||
gvf_t0 = get_sys_time_msec();
|
||||
|
||||
struct FloatEulers *att = stateGetNedToBodyEulers_f();
|
||||
float ground_speed = stateGetHorizontalSpeedNorm_f();
|
||||
float course = stateGetHorizontalSpeedDir_f();
|
||||
float px_dot = ground_speed * sinf(course);
|
||||
float py_dot = ground_speed * cosf(course);
|
||||
#if defined(FIXEDWING_FIRMWARE)
|
||||
float ground_speed = stateGetHorizontalSpeedNorm_f();
|
||||
float course = stateGetHorizontalSpeedDir_f();
|
||||
float px_dot = ground_speed * sinf(course);
|
||||
float py_dot = ground_speed * cosf(course);
|
||||
#elif defined(ROVER_FIRMWARE)
|
||||
// We assume that the course and psi
|
||||
// of the rover (steering wheel) are the same
|
||||
float course = stateGetNedToBodyEulers_f()->psi;
|
||||
float px_dot = stateGetSpeedEnu_f()->x;
|
||||
float py_dot = stateGetSpeedEnu_f()->y;
|
||||
#endif
|
||||
|
||||
int s = gvf_control.s;
|
||||
|
||||
// gradient Phi
|
||||
@@ -209,14 +227,28 @@ void gvf_control_2D(float ke, float kn, float e,
|
||||
|
||||
float omega = omega_d + kn * (mr_x * md_y - mr_y * md_x);
|
||||
|
||||
// Coordinated turn
|
||||
#if defined(FIXEDWING_FIRMWARE)
|
||||
if (autopilot_get_mode() == AP_MODE_AUTO2) {
|
||||
|
||||
// Coordinated turn
|
||||
struct FloatEulers *att = stateGetNedToBodyEulers_f();
|
||||
float ground_speed = stateGetHorizontalSpeedNorm_f();
|
||||
|
||||
lateral_mode = LATERAL_MODE_ROLL;
|
||||
|
||||
h_ctl_roll_setpoint =
|
||||
-atanf(omega * ground_speed / GVF_GRAVITY / cosf(att->theta));
|
||||
BoundAbs(h_ctl_roll_setpoint, h_ctl_roll_max_setpoint);
|
||||
|
||||
lateral_mode = LATERAL_MODE_ROLL;
|
||||
}
|
||||
|
||||
#elif defined(ROVER_FIRMWARE)
|
||||
if (autopilot_get_mode() != AP_MODE_DIRECT) {
|
||||
guidance_control.gvf_omega = omega; //TODO: set omega into external GVF variable
|
||||
}
|
||||
|
||||
#else
|
||||
#error GVF does not support your firmware yet
|
||||
#endif
|
||||
}
|
||||
|
||||
void gvf_set_direction(int8_t s)
|
||||
@@ -224,6 +256,10 @@ void gvf_set_direction(int8_t s)
|
||||
gvf_control.s = s;
|
||||
}
|
||||
|
||||
|
||||
|
||||
#ifdef FIXEDWING_FIRMWARE // FIXEDWING TRAJECTORIES
|
||||
|
||||
// STRAIGHT LINE
|
||||
|
||||
static void gvf_line(float a, float b, float heading)
|
||||
@@ -242,8 +278,9 @@ static void gvf_line(float a, float b, float heading)
|
||||
gvf_control_2D(1e-2 * gvf_line_par.ke, gvf_line_par.kn, e, &grad_line, &Hess_line);
|
||||
|
||||
gvf_control.error = e;
|
||||
|
||||
|
||||
horizontal_mode = HORIZONTAL_MODE_WAYPOINT;
|
||||
|
||||
gvf_segment.seg = 0;
|
||||
}
|
||||
|
||||
@@ -260,7 +297,7 @@ bool gvf_line_XY1_XY2(float x1, float y1, float x2, float y2)
|
||||
float zy = y2 - y1;
|
||||
|
||||
gvf_line_XY_heading(x1, y1, atan2f(zx, zy));
|
||||
|
||||
|
||||
horizontal_mode = HORIZONTAL_MODE_ROUTE;
|
||||
gvf_segment.seg = 1;
|
||||
gvf_segment.x1 = x1;
|
||||
@@ -295,6 +332,7 @@ bool gvf_segment_loop_XY1_XY2(float x1, float y1, float x2, float y2, float d1,
|
||||
gvf_line(x1, y1, alpha);
|
||||
|
||||
horizontal_mode = HORIZONTAL_MODE_ROUTE;
|
||||
|
||||
gvf_segment.seg = 1;
|
||||
gvf_segment.x1 = x1;
|
||||
gvf_segment.y1 = y1;
|
||||
@@ -348,7 +386,7 @@ bool gvf_segment_wp1_wp2(uint8_t wp1, uint8_t wp2)
|
||||
|
||||
bool gvf_line_wp_heading(uint8_t wp, float heading)
|
||||
{
|
||||
heading = heading * M_PI / 180;
|
||||
heading = RadOfDeg(heading);
|
||||
|
||||
float a = waypoints[wp].x;
|
||||
float b = waypoints[wp].y;
|
||||
@@ -370,15 +408,16 @@ bool gvf_ellipse_XY(float x, float y, float a, float b, float alpha)
|
||||
gvf_trajectory.p[2] = a;
|
||||
gvf_trajectory.p[3] = b;
|
||||
gvf_trajectory.p[4] = alpha;
|
||||
|
||||
|
||||
// SAFE MODE
|
||||
if (a < 1 || b < 1) {
|
||||
gvf_trajectory.p[2] = 60;
|
||||
gvf_trajectory.p[3] = 60;
|
||||
}
|
||||
|
||||
if (gvf_trajectory.p[2] == gvf_trajectory.p[3]) {
|
||||
if ((int)gvf_trajectory.p[2] == (int)gvf_trajectory.p[3]) {
|
||||
horizontal_mode = HORIZONTAL_MODE_CIRCLE;
|
||||
|
||||
} else {
|
||||
horizontal_mode = HORIZONTAL_MODE_WAYPOINT;
|
||||
}
|
||||
@@ -447,7 +486,7 @@ bool gvf_sin_wp1_wp2(uint8_t wp1, uint8_t wp2, float w, float off, float A)
|
||||
bool gvf_sin_wp_alpha(uint8_t wp, float alpha, float w, float off, float A)
|
||||
{
|
||||
w = 2 * M_PI * w;
|
||||
alpha = alpha * M_PI / 180;
|
||||
alpha = RadOfDeg(alpha);
|
||||
|
||||
float x = waypoints[wp].x;
|
||||
float y = waypoints[wp].y;
|
||||
@@ -457,3 +496,259 @@ bool gvf_sin_wp_alpha(uint8_t wp, float alpha, float w, float off, float A)
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
#elif defined(ROVER_FIRMWARE) // ROVER TRAJECTORIES
|
||||
|
||||
// STRAIGHT LINE
|
||||
|
||||
static void gvf_line(float a, float b, float heading)
|
||||
{
|
||||
float e;
|
||||
struct gvf_grad grad_line;
|
||||
struct gvf_Hess Hess_line;
|
||||
|
||||
gvf_trajectory.type = 0;
|
||||
gvf_trajectory.p[0] = a;
|
||||
gvf_trajectory.p[1] = b;
|
||||
gvf_trajectory.p[2] = heading;
|
||||
|
||||
gvf_line_info(&e, &grad_line, &Hess_line);
|
||||
gvf_control.ke = gvf_line_par.ke;
|
||||
gvf_control_2D(1e-2 * gvf_line_par.ke, gvf_line_par.kn, e, &grad_line, &Hess_line);
|
||||
|
||||
gvf_control.error = e;
|
||||
|
||||
nav.mode = NAV_MODE_WAYPOINT;
|
||||
|
||||
gvf_segment.seg = 0;
|
||||
}
|
||||
|
||||
bool gvf_line_XY_heading(float a, float b, float heading)
|
||||
{
|
||||
gvf_set_direction(1);
|
||||
gvf_line(a, b, heading);
|
||||
return true;
|
||||
}
|
||||
|
||||
bool gvf_line_XY1_XY2(float x1, float y1, float x2, float y2)
|
||||
{
|
||||
float zx = x2 - x1;
|
||||
float zy = y2 - y1;
|
||||
|
||||
gvf_line_XY_heading(x1, y1, atan2f(zx, zy));
|
||||
|
||||
nav.mode = NAV_MODE_ROUTE;
|
||||
|
||||
gvf_segment.seg = 1;
|
||||
gvf_segment.x1 = x1;
|
||||
gvf_segment.y1 = y1;
|
||||
gvf_segment.x2 = x2;
|
||||
gvf_segment.y2 = y2;
|
||||
|
||||
// Send rover_base navigation data to draw segment (ROUTE)
|
||||
nav_rover_base.goto_wp.from.x = x1;
|
||||
nav_rover_base.goto_wp.from.y = y1;
|
||||
nav_rover_base.goto_wp.to.x = x2;
|
||||
nav_rover_base.goto_wp.to.y = y2;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool gvf_line_wp1_wp2(uint8_t wp1, uint8_t wp2)
|
||||
{
|
||||
float x1 = waypoint_get_x(wp1);
|
||||
float y1 = waypoint_get_y(wp1);
|
||||
float x2 = waypoint_get_x(wp2);
|
||||
float y2 = waypoint_get_y(wp2);
|
||||
|
||||
return gvf_line_XY1_XY2(x1, y1, x2, y2);
|
||||
}
|
||||
|
||||
bool gvf_segment_loop_XY1_XY2(float x1, float y1, float x2, float y2, float d1, float d2)
|
||||
{
|
||||
int s = out_of_segment_area(x1, y1, x2, y2, d1, d2);
|
||||
if (s != 0) {
|
||||
gvf_control.s = s;
|
||||
}
|
||||
|
||||
float zx = x2 - x1;
|
||||
float zy = y2 - y1;
|
||||
float alpha = atanf(zx / zy);
|
||||
|
||||
gvf_line(x1, y1, alpha);
|
||||
|
||||
nav.mode = NAV_MODE_ROUTE;
|
||||
|
||||
gvf_segment.seg = 1;
|
||||
gvf_segment.x1 = x1;
|
||||
gvf_segment.y1 = y1;
|
||||
gvf_segment.x2 = x2;
|
||||
gvf_segment.y2 = y2;
|
||||
|
||||
// Send rover_base navigation data to draw segment (ROUTE)
|
||||
nav_rover_base.goto_wp.from.x = x1;
|
||||
nav_rover_base.goto_wp.from.y = y1;
|
||||
nav_rover_base.goto_wp.to.x = x2;
|
||||
nav_rover_base.goto_wp.to.y = y2;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool gvf_segment_loop_wp1_wp2(uint8_t wp1, uint8_t wp2, float d1, float d2)
|
||||
{
|
||||
float x1 = waypoint_get_x(wp1);
|
||||
float y1 = waypoint_get_y(wp1);
|
||||
float x2 = waypoint_get_x(wp2);
|
||||
float y2 = waypoint_get_y(wp2);
|
||||
|
||||
return gvf_segment_loop_XY1_XY2(x1, y1, x2, y2, d1, d2);
|
||||
}
|
||||
|
||||
bool gvf_segment_XY1_XY2(float x1, float y1, float x2, float y2)
|
||||
{
|
||||
struct EnuCoor_f *p = stateGetPositionEnu_f();
|
||||
float px = p->x - x1;
|
||||
float py = p->y - y1;
|
||||
|
||||
float zx = x2 - x1;
|
||||
float zy = y2 - y1;
|
||||
|
||||
float beta = atan2f(zy, zx);
|
||||
float cosb = cosf(-beta);
|
||||
float sinb = sinf(-beta);
|
||||
float zxr = zx * cosb - zy * sinb;
|
||||
float pxr = px * cosb - py * sinb;
|
||||
|
||||
if ((zxr > 0 && pxr > zxr) || (zxr < 0 && pxr < zxr)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
return gvf_line_XY1_XY2(x1, y1, x2, y2);
|
||||
}
|
||||
|
||||
bool gvf_segment_wp1_wp2(uint8_t wp1, uint8_t wp2)
|
||||
{
|
||||
float x1 = waypoint_get_x(wp1);
|
||||
float y1 = waypoint_get_y(wp1);
|
||||
float x2 = waypoint_get_x(wp2);
|
||||
float y2 = waypoint_get_y(wp2);
|
||||
|
||||
return gvf_segment_XY1_XY2(x1, y1, x2, y2);
|
||||
}
|
||||
|
||||
bool gvf_line_wp_heading(uint8_t wp, float heading)
|
||||
{
|
||||
heading = RadOfDeg(heading);
|
||||
|
||||
float a = waypoint_get_x(wp);
|
||||
float b = waypoint_get_y(wp);
|
||||
|
||||
return gvf_line_XY_heading(a, b, heading);
|
||||
}
|
||||
|
||||
// ELLIPSE
|
||||
|
||||
bool gvf_ellipse_XY(float x, float y, float a, float b, float alpha)
|
||||
{
|
||||
float e;
|
||||
struct gvf_grad grad_ellipse;
|
||||
struct gvf_Hess Hess_ellipse;
|
||||
|
||||
gvf_trajectory.type = 1;
|
||||
gvf_trajectory.p[0] = x;
|
||||
gvf_trajectory.p[1] = y;
|
||||
gvf_trajectory.p[2] = a;
|
||||
gvf_trajectory.p[3] = b;
|
||||
gvf_trajectory.p[4] = alpha;
|
||||
|
||||
// SAFE MODE
|
||||
if (a < 1 || b < 1) {
|
||||
gvf_trajectory.p[2] = 60;
|
||||
gvf_trajectory.p[3] = 60;
|
||||
}
|
||||
|
||||
// Send rover_base navigation data to draw circle (CIRCLE)
|
||||
if (gvf_trajectory.p[2] == gvf_trajectory.p[3]) {
|
||||
nav.mode = NAV_MODE_CIRCLE;
|
||||
nav_rover_base.circle.center.x = x;
|
||||
nav_rover_base.circle.center.y = y;
|
||||
nav_rover_base.circle.radius = a;
|
||||
} else {
|
||||
nav.mode = NAV_MODE_WAYPOINT;
|
||||
}
|
||||
|
||||
gvf_ellipse_info(&e, &grad_ellipse, &Hess_ellipse);
|
||||
gvf_control.ke = gvf_ellipse_par.ke;
|
||||
gvf_control_2D(gvf_ellipse_par.ke, gvf_ellipse_par.kn,
|
||||
e, &grad_ellipse, &Hess_ellipse);
|
||||
|
||||
gvf_control.error = e;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
bool gvf_ellipse_wp(uint8_t wp, float a, float b, float alpha)
|
||||
{
|
||||
gvf_ellipse_XY(waypoint_get_x(wp), waypoint_get_y(wp), a, b, alpha);
|
||||
return true;
|
||||
}
|
||||
|
||||
// SINUSOIDAL (if w = 0 and off = 0, then we just have the straight line case)
|
||||
|
||||
bool gvf_sin_XY_alpha(float a, float b, float alpha, float w, float off, float A)
|
||||
{
|
||||
float e;
|
||||
struct gvf_grad grad_line;
|
||||
struct gvf_Hess Hess_line;
|
||||
|
||||
gvf_trajectory.type = 2;
|
||||
gvf_trajectory.p[0] = a;
|
||||
gvf_trajectory.p[1] = b;
|
||||
gvf_trajectory.p[2] = alpha;
|
||||
gvf_trajectory.p[3] = w;
|
||||
gvf_trajectory.p[4] = off;
|
||||
gvf_trajectory.p[5] = A;
|
||||
|
||||
gvf_sin_info(&e, &grad_line, &Hess_line);
|
||||
gvf_control.ke = gvf_sin_par.ke;
|
||||
gvf_control_2D(1e-2 * gvf_sin_par.ke, gvf_sin_par.kn, e, &grad_line, &Hess_line);
|
||||
|
||||
gvf_control.error = e;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool gvf_sin_wp1_wp2(uint8_t wp1, uint8_t wp2, float w, float off, float A)
|
||||
{
|
||||
w = 2 * M_PI * w;
|
||||
|
||||
float x1 = waypoint_get_x(wp1);
|
||||
float y1 = waypoint_get_y(wp1);
|
||||
float x2 = waypoint_get_x(wp2);
|
||||
float y2 = waypoint_get_y(wp2);
|
||||
|
||||
float zx = x1 - x2;
|
||||
float zy = y1 - y2;
|
||||
|
||||
float alpha = atanf(zy / zx);
|
||||
|
||||
gvf_sin_XY_alpha(x1, y1, alpha, w, off, A);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool gvf_sin_wp_alpha(uint8_t wp, float alpha, float w, float off, float A)
|
||||
{
|
||||
w = 2 * M_PI * w;
|
||||
alpha = RadOfDeg(alpha);
|
||||
|
||||
float x = waypoint_get_x(wp);
|
||||
float y = waypoint_get_y(wp);
|
||||
|
||||
gvf_sin_XY_alpha(x, y, alpha, w, off, A);
|
||||
|
||||
return true;
|
||||
}
|
||||
#endif
|
||||
|
||||
|
||||
@@ -0,0 +1,211 @@
|
||||
/*
|
||||
*
|
||||
* 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.
|
||||
*/
|
||||
|
||||
#include "nps_autopilot.h"
|
||||
|
||||
#include "main_ap.h"
|
||||
#include "nps_sensors.h"
|
||||
#include "nps_radio_control.h"
|
||||
#include "nps_electrical.h"
|
||||
#include "nps_fdm.h"
|
||||
|
||||
#include "modules/radio_control/radio_control.h"
|
||||
#include "modules/imu/imu.h"
|
||||
#include "mcu_periph/sys_time.h"
|
||||
#include "state.h"
|
||||
#include "modules/ahrs/ahrs.h"
|
||||
#include "modules/ins/ins.h"
|
||||
#include "math/pprz_algebra.h"
|
||||
|
||||
/**
|
||||
#ifndef NPS_NO_MOTOR_MIXING
|
||||
#include "modules/actuators/motor_mixing.h"
|
||||
|
||||
#if NPS_COMMANDS_NB != MOTOR_MIXING_NB_MOTOR
|
||||
#warning "NPS_COMMANDS_NB does not match MOTOR_MIXING_NB_MOTOR!"
|
||||
#endif
|
||||
#endif
|
||||
**/
|
||||
|
||||
#include "modules/core/abi.h"
|
||||
|
||||
#include "pprzlink/messages.h"
|
||||
#include "modules/datalink/downlink.h"
|
||||
|
||||
// for datalink_time hack
|
||||
#include "modules/datalink/datalink.h"
|
||||
#include "modules/actuators/actuators.h"
|
||||
|
||||
struct NpsAutopilot nps_autopilot;
|
||||
bool nps_bypass_ahrs;
|
||||
bool nps_bypass_ins;
|
||||
|
||||
#ifndef NPS_BYPASS_AHRS
|
||||
#define NPS_BYPASS_AHRS FALSE
|
||||
#endif
|
||||
|
||||
#ifndef NPS_BYPASS_INS
|
||||
#define NPS_BYPASS_INS FALSE
|
||||
#endif
|
||||
|
||||
#if INDI_RPM_FEEDBACK
|
||||
#error "INDI_RPM_FEEDBACK can not be used in simulation!"
|
||||
#endif
|
||||
|
||||
void nps_autopilot_init(enum NpsRadioControlType type_rc, int num_rc_script, char *rc_dev)
|
||||
{
|
||||
nps_autopilot.launch = TRUE;
|
||||
|
||||
nps_radio_control_init(type_rc, num_rc_script, rc_dev);
|
||||
nps_electrical_init();
|
||||
|
||||
nps_bypass_ahrs = NPS_BYPASS_AHRS;
|
||||
nps_bypass_ins = NPS_BYPASS_INS;
|
||||
|
||||
modules_mcu_init();
|
||||
main_ap_init();
|
||||
|
||||
}
|
||||
|
||||
void nps_autopilot_run_systime_step(void)
|
||||
{
|
||||
sys_tick_handler();
|
||||
}
|
||||
|
||||
#include <stdio.h>
|
||||
#include "modules/gps/gps.h"
|
||||
|
||||
void nps_autopilot_run_step(double time)
|
||||
{
|
||||
|
||||
nps_electrical_run_step(time);
|
||||
|
||||
#if RADIO_CONTROL && !RADIO_CONTROL_TYPE_DATALINK
|
||||
if (nps_radio_control_available(time)) {
|
||||
radio_control_feed();
|
||||
main_ap_event();
|
||||
}
|
||||
#endif
|
||||
|
||||
if (nps_sensors_gyro_available()) {
|
||||
imu_feed_gyro_accel();
|
||||
main_ap_event();
|
||||
}
|
||||
|
||||
if (nps_sensors_mag_available()) {
|
||||
imu_feed_mag();
|
||||
main_ap_event();
|
||||
}
|
||||
|
||||
if (nps_sensors_baro_available()) {
|
||||
uint32_t now_ts = get_sys_time_usec();
|
||||
float pressure = (float) sensors.baro.value;
|
||||
AbiSendMsgBARO_ABS(BARO_SIM_SENDER_ID, now_ts, pressure);
|
||||
main_ap_event();
|
||||
}
|
||||
|
||||
if (nps_sensors_temperature_available()) {
|
||||
AbiSendMsgTEMPERATURE(BARO_SIM_SENDER_ID, (float)sensors.temp.value);
|
||||
main_ap_event();
|
||||
}
|
||||
|
||||
#if USE_AIRSPEED
|
||||
if (nps_sensors_airspeed_available()) {
|
||||
stateSetAirspeed_f((float)sensors.airspeed.value);
|
||||
AbiSendMsgAIRSPEED(AIRSPEED_NPS_ID, (float)sensors.airspeed.value);
|
||||
main_ap_event();
|
||||
}
|
||||
#endif
|
||||
|
||||
#if USE_SONAR
|
||||
if (nps_sensors_sonar_available()) {
|
||||
uint32_t now_ts = get_sys_time_usec();
|
||||
float dist = (float) sensors.sonar.value;
|
||||
if (dist >= 0.0) {
|
||||
AbiSendMsgAGL(AGL_SONAR_NPS_ID, now_ts, dist);
|
||||
}
|
||||
|
||||
#ifdef SENSOR_SYNC_SEND_SONAR
|
||||
uint16_t foo = 0;
|
||||
DOWNLINK_SEND_SONAR(DefaultChannel, DefaultDevice, &foo, &dist);
|
||||
#endif
|
||||
|
||||
main_ap_event();
|
||||
}
|
||||
#endif
|
||||
|
||||
#if USE_GPS
|
||||
if (nps_sensors_gps_available()) {
|
||||
gps_feed_value();
|
||||
main_ap_event();
|
||||
}
|
||||
#endif
|
||||
|
||||
if (nps_bypass_ahrs) {
|
||||
sim_overwrite_ahrs();
|
||||
}
|
||||
|
||||
if (nps_bypass_ins) {
|
||||
sim_overwrite_ins();
|
||||
}
|
||||
|
||||
main_ap_periodic();
|
||||
|
||||
/* feeding the fdm with raw low level commands */
|
||||
for (uint8_t i = 0; i < NPS_COMMANDS_NB; i++) {
|
||||
commands[i] = autopilot_get_motors_on() ? commands[i] : 0;
|
||||
nps_autopilot.commands[i] = (double)commands[i] / MAX_PPRZ;
|
||||
}
|
||||
|
||||
PRINT_CONFIG_MSG("Using throttle, steering commands because rover's fdm don't have explicit actuators.")
|
||||
}
|
||||
|
||||
|
||||
void sim_overwrite_ahrs(void)
|
||||
{
|
||||
|
||||
struct FloatQuat quat_f;
|
||||
QUAT_COPY(quat_f, fdm.ltp_to_body_quat);
|
||||
stateSetNedToBodyQuat_f(&quat_f);
|
||||
|
||||
struct FloatRates rates_f;
|
||||
RATES_COPY(rates_f, fdm.body_ecef_rotvel);
|
||||
stateSetBodyRates_f(&rates_f);
|
||||
|
||||
}
|
||||
|
||||
void sim_overwrite_ins(void)
|
||||
{
|
||||
|
||||
struct NedCoor_f ltp_pos;
|
||||
VECT3_COPY(ltp_pos, fdm.ltpprz_pos);
|
||||
stateSetPositionNed_f(<p_pos);
|
||||
|
||||
struct NedCoor_f ltp_speed;
|
||||
VECT3_COPY(ltp_speed, fdm.ltpprz_ecef_vel);
|
||||
stateSetSpeedNed_f(<p_speed);
|
||||
|
||||
struct NedCoor_f ltp_accel;
|
||||
VECT3_COPY(ltp_accel, fdm.ltpprz_ecef_accel);
|
||||
stateSetAccelNed_f(<p_accel);
|
||||
|
||||
// Here we make sure that ENU states are recalculated
|
||||
stateCalcPositionEnu_i();
|
||||
}
|
||||
@@ -0,0 +1,221 @@
|
||||
/*
|
||||
* Copyright (C) 2021 Jesús Bautista <jesusbautistavillar@gmail.com>
|
||||
* Hector García <noeth3r@gmail.com>
|
||||
*
|
||||
* 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/>.
|
||||
*/
|
||||
|
||||
#include "nps_fdm.h"
|
||||
|
||||
#include <stdlib.h>
|
||||
#include <stdio.h>
|
||||
|
||||
#include "math/pprz_geodetic.h"
|
||||
#include "math/pprz_geodetic_double.h"
|
||||
#include "math/pprz_geodetic_float.h"
|
||||
#include "math/pprz_algebra.h"
|
||||
#include "math/pprz_algebra_float.h"
|
||||
#include "math/pprz_isa.h"
|
||||
|
||||
#include "generated/airframe.h"
|
||||
#include "generated/flight_plan.h"
|
||||
|
||||
#include "firmwares/rover/guidance/rover_guidance_steering.h"
|
||||
#include "state.h"
|
||||
|
||||
// Check if rover firmware
|
||||
#ifndef ROVER_FIRMWARE
|
||||
#error "The module nps_fdm_rover is designed for rovers and doesn't support other firmwares!!"
|
||||
#endif
|
||||
|
||||
// NpsFdm structure
|
||||
struct NpsFdm fdm;
|
||||
|
||||
// Reference point
|
||||
static struct LtpDef_d ltpdef;
|
||||
|
||||
// Static functions declaration
|
||||
static void init_ltp(void);
|
||||
|
||||
/** Physical model structures **/
|
||||
static struct EnuCoor_d rover_pos;
|
||||
static struct EnuCoor_d rover_vel;
|
||||
static struct EnuCoor_d rover_acc;
|
||||
|
||||
/** Physical model parameters **/
|
||||
static float mu = 0.01;
|
||||
|
||||
|
||||
/** NPS FDM rover init ***************************/
|
||||
void nps_fdm_init(double dt)
|
||||
{
|
||||
fdm.init_dt = dt; // (1 / simulation freq)
|
||||
fdm.curr_dt = 0.001; // ¿Configurable from GCS?
|
||||
fdm.time = dt;
|
||||
|
||||
fdm.on_ground = TRUE;
|
||||
|
||||
fdm.nan_count = 0;
|
||||
fdm.pressure = -1;
|
||||
fdm.pressure_sl = PPRZ_ISA_SEA_LEVEL_PRESSURE;
|
||||
fdm.total_pressure = -1;
|
||||
fdm.dynamic_pressure = -1;
|
||||
fdm.temperature = -1;
|
||||
|
||||
fdm.ltpprz_to_body_eulers.psi = 0.0;
|
||||
init_ltp();
|
||||
}
|
||||
|
||||
void nps_fdm_run_step(bool launch __attribute__((unused)), double *commands, int commands_nb __attribute__((unused)))
|
||||
{
|
||||
|
||||
/****************************************************************************
|
||||
PHYSICAL MODEL
|
||||
-------------
|
||||
The physical model of your rover goes here. This physics takes place in
|
||||
the LTP plane (so we transfer every integration step to NED and ECEF at the end).
|
||||
*/
|
||||
|
||||
#ifdef COMMAND_STEERING // STEERING ROVER PHYSICS
|
||||
|
||||
// Steering rover cmds:
|
||||
// COMMAND_STEERING -> delta parameter
|
||||
// COMMAND_TRHOTTLE -> acceleration in heading direction
|
||||
|
||||
double delta = RadOfDeg(commands[COMMAND_STEERING] * MAX_DELTA);
|
||||
|
||||
/** Physical model for car-like robots .................. **/
|
||||
// From previous step...
|
||||
enu_of_ecef_point_d(&rover_pos, <pdef, &fdm.ecef_pos);
|
||||
enu_of_ecef_vect_d(&rover_vel, <pdef, &fdm.ecef_ecef_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;
|
||||
|
||||
// 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];
|
||||
|
||||
// Velocities (EULER INTEGRATION)
|
||||
rover_vel.x += rover_acc.x * fdm.curr_dt;
|
||||
rover_vel.y += rover_acc.y * fdm.curr_dt;
|
||||
phi_d = tan(delta) / DRIVE_SHAFT_DISTANCE * speed;
|
||||
|
||||
// Positions
|
||||
rover_pos.x += rover_vel.x * fdm.curr_dt;
|
||||
rover_pos.y += rover_vel.y * fdm.curr_dt;
|
||||
phi += phi_d * fdm.curr_dt;
|
||||
|
||||
// phi have to be contained in [-180º,180º). So:
|
||||
phi = (phi > M_PI)? - 2*M_PI + phi : (phi < -M_PI)? 2*M_PI + phi : phi;
|
||||
|
||||
#else
|
||||
#warning "The physics of this rover are not yet implemented in nps_fdm_rover!!"
|
||||
#endif // STEERING ROVER PHYSICS
|
||||
|
||||
|
||||
/****************************************************************************/
|
||||
// Coordenates transformations |
|
||||
// ----------------------------|
|
||||
|
||||
/* in ECEF */
|
||||
ecef_of_enu_point_d(&fdm.ecef_pos, <pdef, &rover_pos);
|
||||
ecef_of_enu_vect_d(&fdm.ecef_ecef_vel, <pdef, &rover_vel);
|
||||
ecef_of_enu_vect_d(&fdm.ecef_ecef_accel, <pdef, &rover_acc);
|
||||
|
||||
/* in NED */
|
||||
ned_of_ecef_point_d(&fdm.ltpprz_pos, <pdef, &fdm.ecef_pos);
|
||||
ned_of_ecef_vect_d(&fdm.ltpprz_ecef_vel, <pdef, &fdm.ecef_ecef_vel);
|
||||
ned_of_ecef_vect_d(&fdm.ltpprz_ecef_accel, <pdef, &fdm.ecef_ecef_accel);
|
||||
|
||||
/* Eulers */
|
||||
fdm.ltpprz_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)
|
||||
double_quat_of_eulers(&fdm.ltp_to_body_quat, &fdm.ltp_to_body_eulers);
|
||||
|
||||
// Angular vel & acc
|
||||
fdm.body_ecef_rotvel.r = phi_d;
|
||||
fdm.body_ecef_rotaccel.r = phi_dd;
|
||||
|
||||
}
|
||||
|
||||
|
||||
/**************************
|
||||
** Generating LTP plane **
|
||||
**************************/
|
||||
|
||||
static void init_ltp(void)
|
||||
{
|
||||
|
||||
struct LlaCoor_d llh_nav0; /* Height above the ellipsoid */
|
||||
llh_nav0.lat = RadOfDeg((double)NAV_LAT0 / 1e7);
|
||||
llh_nav0.lon = RadOfDeg((double)NAV_LON0 / 1e7);
|
||||
|
||||
struct EcefCoor_d ecef_nav0;
|
||||
|
||||
ecef_of_lla_d(&ecef_nav0, &llh_nav0);
|
||||
|
||||
ltp_def_from_ecef_d(<pdef, &ecef_nav0);
|
||||
fdm.ecef_pos = ecef_nav0;
|
||||
|
||||
fdm.ltp_g.x = 0.;
|
||||
fdm.ltp_g.y = 0.;
|
||||
fdm.ltp_g.z = 0.; // accel data are already with the correct format
|
||||
|
||||
|
||||
#ifdef AHRS_H_X
|
||||
fdm.ltp_h.x = AHRS_H_X;
|
||||
fdm.ltp_h.y = AHRS_H_Y;
|
||||
fdm.ltp_h.z = AHRS_H_Z;
|
||||
PRINT_CONFIG_MSG("Using magnetic field as defined in airframe file.")
|
||||
#else
|
||||
fdm.ltp_h.x = 0.4912;
|
||||
fdm.ltp_h.y = 0.1225;
|
||||
fdm.ltp_h.z = 0.8624;
|
||||
#endif
|
||||
|
||||
}
|
||||
|
||||
|
||||
/*****************************************************/
|
||||
// Atmosphere function (we don't need that features) //
|
||||
void nps_fdm_set_wind(double speed __attribute__((unused)),
|
||||
double dir __attribute__((unused)))
|
||||
{
|
||||
}
|
||||
|
||||
void nps_fdm_set_wind_ned(double wind_north __attribute__((unused)),
|
||||
double wind_east __attribute__((unused)),
|
||||
double wind_down __attribute__((unused)))
|
||||
{
|
||||
}
|
||||
|
||||
void nps_fdm_set_turbulence(double wind_speed __attribute__((unused)),
|
||||
int turbulence_severity __attribute__((unused)))
|
||||
{
|
||||
}
|
||||
|
||||
void nps_fdm_set_temperature(double temp __attribute__((unused)),
|
||||
double h __attribute__((unused)))
|
||||
{
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user