mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-09 14:18:00 +08:00
[gvf] Refactor GVF module framework, add GVF_IK, fix bugs, and provide full example config (#3451)
This commit is contained in:
Jesús Bautista Villar
committed by
GitHub
GitHub
parent
f19834aaa7
commit
4ee8e5e1b9
@@ -327,7 +327,7 @@ test_tudelft: all
|
||||
|
||||
# test GVF conf
|
||||
test_gvf: all
|
||||
CONF_XML=conf/userconf/GVF/gvf_conf.xml prove tests/aircrafts/
|
||||
CONF_XML=conf/userconf/conf_example_gvf.xml prove tests/aircrafts/
|
||||
|
||||
|
||||
# compiles all aircrafts in conf_tests.xml
|
||||
|
||||
@@ -44,10 +44,8 @@
|
||||
<configure name="USE_MAGNETOMETER" value="0"/>
|
||||
</module-->
|
||||
|
||||
<module name="nav" type="rover_base">
|
||||
<define name="ROVER_BASE_SEND_TRAJECTORY" value="FALSE"/>
|
||||
</module>
|
||||
<module name="gvf" type="module">
|
||||
<module name="nav" type="rover_base"/>
|
||||
<module name="gvf" type="classic">
|
||||
<define name="GVF_OCAML_GCS" value="FALSE"/>
|
||||
</module>
|
||||
|
||||
|
||||
@@ -0,0 +1,307 @@
|
||||
<!DOCTYPE airframe SYSTEM "../airframe.dtd">
|
||||
|
||||
<!--
|
||||
Sonic model
|
||||
|
||||
- Propeller: 5x5 3 blades
|
||||
- Motor: 2206 2300KV
|
||||
- Motor controller: Flycolor 30A LV
|
||||
- Radio modem: Xbee Pro S1 2.4 Ghz
|
||||
- Radio control: Futaba T16SZ
|
||||
- GPS: Ublox M8N (5-10Hz max)
|
||||
- Autopilot; Apogee V1.0
|
||||
|
||||
-->
|
||||
|
||||
<airframe name="Sonic_1">
|
||||
|
||||
<!-- FIRMWARE & MODULES .................................... -->
|
||||
|
||||
<firmware name="fixedwing">
|
||||
|
||||
<define name="NAVIGATION_FREQUENCY" value="50"/> <!-- unit="Hz" -->
|
||||
|
||||
<!--define name="USE_I2C1"/-->
|
||||
<define name="USE_GYRO_PITCH_RATE"/>
|
||||
|
||||
<target name="ap" board="apogee_1.0_chibios">
|
||||
<module name="radio_control" type="sbus"/>
|
||||
</target>
|
||||
|
||||
<target name="nps" board="pc">
|
||||
<module name="radio_control" type="ppm"/>
|
||||
<module name="fdm" type="jsbsim"/>
|
||||
<!-- <configure name="MODEM_PORT_IN" value="4248"/>
|
||||
<configure name="MODEM_PORT_OUT" value="4249"/> -->
|
||||
</target>
|
||||
|
||||
<!-- Communication -->
|
||||
<module name="telemetry" type="xbee_api"/>
|
||||
|
||||
<!-- SD logging -->
|
||||
<!--module name="tlsf"/>
|
||||
<module name="logger" type="sd_chibios"/>
|
||||
<module name="pprzlog"/>
|
||||
<module name="flight_recorder"/-->
|
||||
|
||||
<!-- IMU, AHRS, INS -->
|
||||
<module name="imu" type="apogee">
|
||||
<define name="IMU_APOGEE_CHAN_X" value="1"/>
|
||||
<define name="IMU_APOGEE_CHAN_Y" value="0"/>
|
||||
</module>
|
||||
|
||||
<module name="ahrs" type="float_dcm"/>
|
||||
<configure name="USE_MAGNETOMETER" value="FALSE"/>
|
||||
<define name="USE_GPS_HEADING" value="TRUE"/>
|
||||
|
||||
<module name="ins" type="alt_float"/>
|
||||
|
||||
<!-- Sensors -->
|
||||
<module name="gps_ubx_ucenter"/>
|
||||
<module name="gps" type="ublox">
|
||||
<configure name="GPS_BAUD" value="B38400"/>
|
||||
</module>
|
||||
|
||||
<!-- GNC -->
|
||||
<!-- TODO:
|
||||
The auto-takeoff needs to be tested more extensively to prevent the aircraft
|
||||
from making abrupt movements during the initial moments of takeoff.
|
||||
These modifications will likely be included in the flight plan.
|
||||
|
||||
For auto-landing, the skid_landing module must be tested.
|
||||
This requires some integration work with the vertical controller we are using.
|
||||
-->
|
||||
<!--module name="takeoff_detect"/-->
|
||||
<module name="control" type="new"/>
|
||||
<!--module name="nav" type="skid_landing"/--> <!-- not integrated yet -->
|
||||
<module name="navigation"/>
|
||||
|
||||
</firmware>
|
||||
|
||||
<!-- ....................................................... -->
|
||||
|
||||
<!-- HARDWARE settings ..................................... -->
|
||||
|
||||
<servos>
|
||||
<servo name="MOTOR" no="0" max="2000" neutral="1000" min="1000"/>
|
||||
<servo name="AILEVON_RIGHT" no="1" max="1000" neutral="1430" min="2000"/>
|
||||
<servo name="AILEVON_LEFT" no="2" max="2000" neutral="1693" min="1000"/>
|
||||
</servos>
|
||||
|
||||
<commands>
|
||||
<axis name="THROTTLE" failsafe_value="0"/>
|
||||
<axis name="ROLL" failsafe_value="0"/>
|
||||
<axis name="PITCH" failsafe_value="0"/>
|
||||
</commands>
|
||||
|
||||
<rc_commands>
|
||||
<set command="THROTTLE" value="@THROTTLE"/>
|
||||
<set command="ROLL" value="@ROLL"/>
|
||||
<set command="PITCH" value="@PITCH"/>
|
||||
</rc_commands>
|
||||
|
||||
<section name="MIXER">
|
||||
<define name="AILEVON_AILERON_RATE" value="0.75"/>
|
||||
<define name="AILEVON_ELEVATOR_RATE" value="0.75"/>
|
||||
</section>
|
||||
|
||||
<command_laws>
|
||||
<let var="aileron" value="@ROLL * AILEVON_AILERON_RATE"/>
|
||||
<let var="elevator" value="@PITCH * AILEVON_ELEVATOR_RATE"/>
|
||||
<set servo="MOTOR" value="@THROTTLE"/>
|
||||
<set servo="AILEVON_LEFT" value="$elevator - $aileron"/>
|
||||
<set servo="AILEVON_RIGHT" value="$elevator + $aileron"/>
|
||||
</command_laws>
|
||||
|
||||
<section name="AUTO1" prefix="AUTO1_">
|
||||
<define name="MAX_ROLL" value="0.85"/>
|
||||
<define name="MAX_PITCH" value="0.6"/>
|
||||
</section>
|
||||
|
||||
<section name="IMU" prefix="IMU_">
|
||||
|
||||
<!-- Needed by NPS (simulation) -->
|
||||
<define name="MAG_X_NEUTRAL" value="0"/>
|
||||
<define name="MAG_Y_NEUTRAL" value="0"/>
|
||||
<define name="MAG_Z_NEUTRAL" value="0"/>
|
||||
|
||||
<!-- Calibration Neutral -->
|
||||
<define name="GYRO_P_SIGN" value="1"/>
|
||||
<define name="GYRO_Q_SIGN" value="1"/>
|
||||
<define name="GYRO_R_SIGN" value="-1"/>
|
||||
<define name="GYRO_P_NEUTRAL" value="0"/>
|
||||
<define name="GYRO_Q_NEUTRAL" value="0"/>
|
||||
<define name="GYRO_R_NEUTRAL" value="0"/>
|
||||
|
||||
<define name="ACCEL_X_SIGN" value="1"/>
|
||||
<define name="ACCEL_Y_SIGN" value="1"/>
|
||||
<define name="ACCEL_Z_SIGN" value="-1"/>
|
||||
<define name="ACCEL_X_NEUTRAL" value="109"/>
|
||||
<define name="ACCEL_Y_NEUTRAL" value="13"/>
|
||||
<define name="ACCEL_Z_NEUTRAL" value="-404"/>
|
||||
<define name="ACCEL_X_SENS" value="2.45045342816" integer="16"/>
|
||||
<define name="ACCEL_Y_SENS" value="2.44747844234" integer="16"/>
|
||||
<define name="ACCEL_Z_SENS" value="2.42689216106" integer="16"/>
|
||||
|
||||
<define name="BODY_TO_IMU_PHI" value="0"/>
|
||||
<define name="BODY_TO_IMU_THETA" value="-0.0349069999999"/>
|
||||
<define name="BODY_TO_IMU_PSI" value="0"/>
|
||||
</section>
|
||||
|
||||
<section name="INS" prefix="INS_">
|
||||
<define name="H_X" value="0.5141"/>
|
||||
<define name="H_Y" value="0.0002"/>
|
||||
<define name="H_Z" value="0.8576"/>
|
||||
</section>
|
||||
|
||||
<section name="BAT">
|
||||
<!-- LiPo 3S -->
|
||||
<define name="MAX_BAT_LEVEL" value="12.60" unit="V"/>
|
||||
<define name="LOW_BAT_LEVEL" value="11.20" unit="V"/>
|
||||
<define name="CRITIC_BAT_LEVEL" value="10.80" unit="V"/>
|
||||
<define name="CATASTROPHIC_BAT_LEVEL" value="9.00" unit="V"/>
|
||||
|
||||
<define name="MilliAmpereOfAdc(_adc)" value="(_adc-632)*4.14"/>
|
||||
</section>
|
||||
|
||||
<!-- ....................................................... -->
|
||||
|
||||
<!-- GNS section ........................................... -->
|
||||
|
||||
<section name="VERTICAL CONTROL" prefix="V_CTL_">
|
||||
<!-- outer loop proportional gain -->
|
||||
<define name="ALTITUDE_PGAIN" value="0.13"/>
|
||||
<!-- outer loop saturation -->
|
||||
<define name="ALTITUDE_MAX_CLIMB" value="4."/>
|
||||
<!-- disable climb rate limiter -->
|
||||
<define name="AUTO_CLIMB_LIMIT" value="2*V_CTL_ALTITUDE_MAX_CLIMB"/>
|
||||
|
||||
<!-- Cruise throttle + limits -->
|
||||
<define name="AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE" value="0.3"/>
|
||||
<define name="AUTO_THROTTLE_MIN_CRUISE_THROTTLE" value="0.25"/>
|
||||
<define name="AUTO_THROTTLE_MAX_CRUISE_THROTTLE" value="0.85"/>
|
||||
|
||||
<!-- Climb loop (throttle) -->
|
||||
<define name="AUTO_THROTTLE_CLIMB_THROTTLE_INCREMENT" value="0.07" unit="%/(m/s)"/>
|
||||
<define name="AUTO_THROTTLE_PGAIN" value="0.012"/>
|
||||
<define name="AUTO_THROTTLE_DGAIN" value="0.002"/>
|
||||
<define name="AUTO_THROTTLE_IGAIN" value="0.004"/>
|
||||
<define name="AUTO_THROTTLE_PITCH_OF_VZ_PGAIN" value="0.1"/>
|
||||
|
||||
<!-- Climb loop (pitch) -->
|
||||
<define name="AUTO_PITCH_PGAIN" value="0.048"/>
|
||||
<define name="AUTO_PITCH_DGAIN" value="0.01"/>
|
||||
<define name="AUTO_PITCH_IGAIN" value="0.0"/>
|
||||
<define name="AUTO_PITCH_MAX_PITCH" value="20" unit="deg"/>
|
||||
<define name="AUTO_PITCH_MIN_PITCH" value="-20" unit="deg"/>
|
||||
|
||||
<!-- Trims -->
|
||||
<define name="PITCH_TRIM" value="0.5" unit="deg"/>
|
||||
|
||||
<!-- airspeed control -->
|
||||
<!--XPA NOT USED />
|
||||
<define name="AUTO_AIRSPEED_SETPOINT" value="16."/>
|
||||
<define name="AUTO_AIRSPEED_THROTTLE_PGAIN" value="0.1"/>
|
||||
<define name="AUTO_AIRSPEED_THROTTLE_DGAIN" value="0.12"/>
|
||||
<define name="AUTO_AIRSPEED_THROTTLE_IGAIN" value="0.0"/>
|
||||
<define name="AUTO_AIRSPEED_PITCH_PGAIN" value="0.06"/>
|
||||
<define name="AUTO_AIRSPEED_PITCH_DGAIN" value="0.0"/>
|
||||
<define name="AUTO_AIRSPEED_PITCH_IGAIN" value="0.042"/>
|
||||
<define name="AIRSPEED_MAX" value="30"/>
|
||||
<define name="AIRSPEED_MIN" value="10"/-->
|
||||
|
||||
<!-- groundspeed control -->
|
||||
<!--XPA NOT USED />
|
||||
<define name="AUTO_GROUNDSPEED_SETPOINT" value="15"/>
|
||||
<define name="AUTO_GROUNDSPEED_PGAIN" value="1."/>
|
||||
<define name="AUTO_GROUNDSPEED_IGAIN" value="0."/-->
|
||||
|
||||
<!-- pitch trim -->
|
||||
<!--XPA NOT USED />
|
||||
<define name="PITCH_LOITER_TRIM" value="0." unit="deg"/>
|
||||
<define name="PITCH_DASH_TRIM" value="0." unit="deg"/-->
|
||||
|
||||
<define name="THROTTLE_SLEW" value="0.1"/>
|
||||
|
||||
</section>
|
||||
|
||||
<section name="HORIZONTAL CONTROL" prefix="H_CTL_">
|
||||
<define name="COURSE_PGAIN" value="0.743"/>
|
||||
<!--XPA NOT USED />
|
||||
<define name="COURSE_TAU" value="0.5"/-->
|
||||
<define name="ROLL_MAX_SETPOINT" value="40." unit="deg"/>
|
||||
<define name="ROLL_MIN_SETPOINT" value="-40." unit="deg"/>
|
||||
<define name="PITCH_MAX_SETPOINT" value="30." unit="deg"/>
|
||||
<define name="PITCH_MIN_SETPOINT" value="-30." unit="deg"/>
|
||||
|
||||
<define name="ROLL_ATTITUDE_GAIN" value="11000."/>
|
||||
<define name="ROLL_RATE_GAIN" value="1000."/>
|
||||
<define name="ROLL_IGAIN" value="100."/>
|
||||
<!--XPA NOT USED />
|
||||
<define name="ROLL_KFFA" value="0"/>
|
||||
<define name="ROLL_KFFD" value="0"/-->
|
||||
|
||||
<define name="PITCH_PGAIN" value="17250"/>
|
||||
<define name="PITCH_DGAIN" value="500."/>
|
||||
<define name="PITCH_IGAIN" value="400"/>
|
||||
|
||||
<!--XPA NOT USED />
|
||||
<define name="PITCH_KFFA" value="0."/>
|
||||
<define name="PITCH_KFFD" value="0."/-->
|
||||
|
||||
<define name="PITCH_OF_ROLL" value="1." unit="deg"/>
|
||||
<define name="AILERON_OF_THROTTLE" value="0.0"/>
|
||||
<define name="ELEVATOR_OF_ROLL" value="0"/>
|
||||
</section>
|
||||
|
||||
<!--XPA NOT USED />
|
||||
<section name="NAV">
|
||||
<define name="NAV_GLIDE_PITCH_TRIM" value="0"/>
|
||||
</section-->
|
||||
|
||||
<!-- ....................................................... -->
|
||||
|
||||
<!-- OTHERS ................................................ -->
|
||||
|
||||
<section name="MISC">
|
||||
<define name="NOMINAL_AIRSPEED" value="12." unit="m/s"/>
|
||||
<define name="CARROT" value="5." unit="s"/>
|
||||
<define name="KILL_MODE_DISTANCE" value="(1.5*MAX_DIST_FROM_HOME)"/>
|
||||
<define name="DEFAULT_CIRCLE_RADIUS" value="80."/>
|
||||
<define name="UNLOCKED_HOME_MODE" value="TRUE"/>
|
||||
<define name="APP_ANGLE" value="8" unit="deg"/>
|
||||
<!--ATID: Network 3351 (default:3350)
|
||||
ATCH: Channel 17
|
||||
ATP1: Api mode
|
||||
ATBD6: Baudrate 57600
|
||||
ATWR: Write in Flash-->
|
||||
<!--define name="XBEE_INIT" value="\"ATID3351\rATCH17\rATP1\rATBD6\r\""/-->
|
||||
</section>
|
||||
|
||||
<section name="CATAPULT" prefix="NAV_CATAPULT_">
|
||||
<define name="ACCELERATION_THRESHOLD" value="1.0"/>
|
||||
<define name="INITIAL_PITCH" value="20" unit="deg"/>
|
||||
<define name="CLIMB_DISTANCE" value="160"/>
|
||||
</section>
|
||||
|
||||
<section name="FAILSAFE" prefix="FAILSAFE_">
|
||||
<define name="DELAY_WITHOUT_GPS" value="2" unit="s"/>
|
||||
<define name="DEFAULT_THROTTLE" value="0.3" unit="%"/>
|
||||
<define name="DEFAULT_ROLL" value="0.3" unit="rad"/>
|
||||
<define name="DEFAULT_PITCH" value="0.5" unit="rad"/>
|
||||
<define name="HOME_RADIUS" value="100" unit="m"/>
|
||||
</section>
|
||||
|
||||
<section name="SIMULATOR" prefix="NPS_">
|
||||
<define name="JSBSIM_LAUNCHSPEED" value="15"/>
|
||||
<define name="JSBSIM_MODEL" value="easystar" type="string"/>
|
||||
<define name="SENSORS_PARAMS" value="nps_sensors_params_default.h" type="string"/>
|
||||
</section>
|
||||
|
||||
<section name="GCS">
|
||||
<define name="AC_ICON" value="flyingwing"/>
|
||||
</section>
|
||||
|
||||
<!-- ....................................................... -->
|
||||
|
||||
</airframe>
|
||||
@@ -0,0 +1,307 @@
|
||||
<!DOCTYPE airframe SYSTEM "../airframe.dtd">
|
||||
|
||||
<!--
|
||||
Sonic model
|
||||
|
||||
- Propeller: 5x5 3 blades
|
||||
- Motor: 2206 2300KV
|
||||
- Motor controller: Flycolor 30A LV
|
||||
- Radio modem: Xbee Pro S1 2.4 Ghz
|
||||
- Radio control: Futaba T16SZ
|
||||
- GPS: Ublox M8N (5-10Hz max)
|
||||
- Autopilot; Apogee V1.0
|
||||
|
||||
-->
|
||||
|
||||
<airframe name="Sonic_2">
|
||||
|
||||
<!-- FIRMWARE & MODULES .................................... -->
|
||||
|
||||
<firmware name="fixedwing">
|
||||
|
||||
<define name="NAVIGATION_FREQUENCY" value="50"/> <!-- unit="Hz" -->
|
||||
|
||||
<!--define name="USE_I2C1"/-->
|
||||
<define name="USE_GYRO_PITCH_RATE"/>
|
||||
|
||||
<target name="ap" board="apogee_1.0_chibios">
|
||||
<module name="radio_control" type="sbus"/>
|
||||
</target>
|
||||
|
||||
<target name="nps" board="pc">
|
||||
<module name="radio_control" type="ppm"/>
|
||||
<module name="fdm" type="jsbsim"/>
|
||||
<!-- <configure name="MODEM_PORT_IN" value="4250"/>
|
||||
<configure name="MODEM_PORT_OUT" value="4251"/> -->
|
||||
</target>
|
||||
|
||||
<!-- Communication -->
|
||||
<module name="telemetry" type="xbee_api"/>
|
||||
|
||||
<!-- SD logging -->
|
||||
<!--module name="tlsf"/>
|
||||
<module name="logger" type="sd_chibios"/>
|
||||
<module name="pprzlog"/>
|
||||
<module name="flight_recorder"/-->
|
||||
|
||||
<!-- IMU, AHRS, INS -->
|
||||
<module name="imu" type="apogee">
|
||||
<define name="IMU_APOGEE_CHAN_X" value="1"/>
|
||||
<define name="IMU_APOGEE_CHAN_Y" value="0"/>
|
||||
</module>
|
||||
|
||||
<module name="ahrs" type="float_dcm"/>
|
||||
<configure name="USE_MAGNETOMETER" value="FALSE"/>
|
||||
<define name="USE_GPS_HEADING" value="TRUE"/>
|
||||
|
||||
<module name="ins" type="alt_float"/>
|
||||
|
||||
<!-- Sensors -->
|
||||
<module name="gps_ubx_ucenter"/>
|
||||
<module name="gps" type="ublox">
|
||||
<configure name="GPS_BAUD" value="B38400"/>
|
||||
</module>
|
||||
|
||||
<!-- GNC -->
|
||||
<!-- TODO:
|
||||
The auto-takeoff needs to be tested more extensively to prevent the aircraft
|
||||
from making abrupt movements during the initial moments of takeoff.
|
||||
These modifications will likely be included in the flight plan.
|
||||
|
||||
For auto-landing, the skid_landing module must be tested.
|
||||
This requires some integration work with the vertical controller we are using.
|
||||
-->
|
||||
<!--module name="takeoff_detect"/-->
|
||||
<module name="control" type="new"/>
|
||||
<!--module name="nav" type="skid_landing"/--> <!-- not integrated yet -->
|
||||
<module name="navigation"/>
|
||||
|
||||
</firmware>
|
||||
|
||||
<!-- ....................................................... -->
|
||||
|
||||
<!-- HARDWARE settings ..................................... -->
|
||||
|
||||
<servos>
|
||||
<servo name="MOTOR" no="0" min="1000" neutral="1000" max="2000"/>
|
||||
<servo name="AILEVON_RIGHT" no="1" max="1000" neutral="1500" min="2000"/>
|
||||
<servo name="AILEVON_LEFT" no="2" max="2000" neutral="1500" min="1000"/>
|
||||
</servos>
|
||||
|
||||
<commands>
|
||||
<axis name="THROTTLE" failsafe_value="0"/>
|
||||
<axis name="ROLL" failsafe_value="0"/>
|
||||
<axis name="PITCH" failsafe_value="0"/>
|
||||
</commands>
|
||||
|
||||
<rc_commands>
|
||||
<set command="THROTTLE" value="@THROTTLE"/>
|
||||
<set command="ROLL" value="@ROLL"/>
|
||||
<set command="PITCH" value="@PITCH"/>
|
||||
</rc_commands>
|
||||
|
||||
<section name="MIXER">
|
||||
<define name="AILEVON_AILERON_RATE" value="0.75"/>
|
||||
<define name="AILEVON_ELEVATOR_RATE" value="0.75"/>
|
||||
</section>
|
||||
|
||||
<command_laws>
|
||||
<let var="aileron" value="@ROLL * AILEVON_AILERON_RATE"/>
|
||||
<let var="elevator" value="@PITCH * AILEVON_ELEVATOR_RATE"/>
|
||||
<set servo="MOTOR" value="@THROTTLE"/>
|
||||
<set servo="AILEVON_LEFT" value="$elevator - $aileron"/>
|
||||
<set servo="AILEVON_RIGHT" value="$elevator + $aileron"/>
|
||||
</command_laws>
|
||||
|
||||
<section name="AUTO1" prefix="AUTO1_">
|
||||
<define name="MAX_ROLL" value="0.85"/>
|
||||
<define name="MAX_PITCH" value="0.6"/>
|
||||
</section>
|
||||
|
||||
<section name="IMU" prefix="IMU_">
|
||||
|
||||
<!-- Needed by NPS (simulation) -->
|
||||
<define name="MAG_X_NEUTRAL" value="0"/>
|
||||
<define name="MAG_Y_NEUTRAL" value="0"/>
|
||||
<define name="MAG_Z_NEUTRAL" value="0"/>
|
||||
|
||||
<!-- Calibration Neutral -->
|
||||
<define name="GYRO_P_SIGN" value="1"/>
|
||||
<define name="GYRO_Q_SIGN" value="1"/>
|
||||
<define name="GYRO_R_SIGN" value="-1"/>
|
||||
<define name="GYRO_P_NEUTRAL" value="0"/>
|
||||
<define name="GYRO_Q_NEUTRAL" value="0"/>
|
||||
<define name="GYRO_R_NEUTRAL" value="0"/>
|
||||
|
||||
<define name="ACCEL_X_SIGN" value="1"/>
|
||||
<define name="ACCEL_Y_SIGN" value="1"/>
|
||||
<define name="ACCEL_Z_SIGN" value="-1"/>
|
||||
<define name="ACCEL_X_NEUTRAL" value="109"/>
|
||||
<define name="ACCEL_Y_NEUTRAL" value="13"/>
|
||||
<define name="ACCEL_Z_NEUTRAL" value="-404"/>
|
||||
<define name="ACCEL_X_SENS" value="2.45045342816" integer="16"/>
|
||||
<define name="ACCEL_Y_SENS" value="2.44747844234" integer="16"/>
|
||||
<define name="ACCEL_Z_SENS" value="2.42689216106" integer="16"/>
|
||||
|
||||
<define name="BODY_TO_IMU_PHI" value="0"/>
|
||||
<define name="BODY_TO_IMU_THETA" value="-0.0349069999999"/>
|
||||
<define name="BODY_TO_IMU_PSI" value="0"/>
|
||||
</section>
|
||||
|
||||
<section name="INS" prefix="INS_">
|
||||
<define name="H_X" value="0.5141"/>
|
||||
<define name="H_Y" value="0.0002"/>
|
||||
<define name="H_Z" value="0.8576"/>
|
||||
</section>
|
||||
|
||||
<section name="BAT">
|
||||
<!-- LiPo 3S -->
|
||||
<define name="MAX_BAT_LEVEL" value="12.60" unit="V"/>
|
||||
<define name="LOW_BAT_LEVEL" value="11.20" unit="V"/>
|
||||
<define name="CRITIC_BAT_LEVEL" value="10.80" unit="V"/>
|
||||
<define name="CATASTROPHIC_BAT_LEVEL" value="9.00" unit="V"/>
|
||||
|
||||
<define name="MilliAmpereOfAdc(_adc)" value="(_adc-632)*4.14"/>
|
||||
</section>
|
||||
|
||||
<!-- ....................................................... -->
|
||||
|
||||
<!-- GNS section ........................................... -->
|
||||
|
||||
<section name="VERTICAL CONTROL" prefix="V_CTL_">
|
||||
<!-- outer loop proportional gain -->
|
||||
<define name="ALTITUDE_PGAIN" value="0.13"/>
|
||||
<!-- outer loop saturation -->
|
||||
<define name="ALTITUDE_MAX_CLIMB" value="4."/>
|
||||
<!-- disable climb rate limiter -->
|
||||
<define name="AUTO_CLIMB_LIMIT" value="2*V_CTL_ALTITUDE_MAX_CLIMB"/>
|
||||
|
||||
<!-- Cruise throttle + limits -->
|
||||
<define name="AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE" value="0.3"/>
|
||||
<define name="AUTO_THROTTLE_MIN_CRUISE_THROTTLE" value="0.25"/>
|
||||
<define name="AUTO_THROTTLE_MAX_CRUISE_THROTTLE" value="0.85"/>
|
||||
|
||||
<!-- Climb loop (throttle) -->
|
||||
<define name="AUTO_THROTTLE_CLIMB_THROTTLE_INCREMENT" value="0.07" unit="%/(m/s)"/>
|
||||
<define name="AUTO_THROTTLE_PGAIN" value="0.012"/>
|
||||
<define name="AUTO_THROTTLE_DGAIN" value="0.002"/>
|
||||
<define name="AUTO_THROTTLE_IGAIN" value="0.004"/>
|
||||
<define name="AUTO_THROTTLE_PITCH_OF_VZ_PGAIN" value="0.1"/>
|
||||
|
||||
<!-- Climb loop (pitch) -->
|
||||
<define name="AUTO_PITCH_PGAIN" value="0.048"/>
|
||||
<define name="AUTO_PITCH_DGAIN" value="0.01"/>
|
||||
<define name="AUTO_PITCH_IGAIN" value="0.0"/>
|
||||
<define name="AUTO_PITCH_MAX_PITCH" value="20" unit="deg"/>
|
||||
<define name="AUTO_PITCH_MIN_PITCH" value="-20" unit="deg"/>
|
||||
|
||||
<!-- Trims -->
|
||||
<define name="PITCH_TRIM" value="0.5" unit="deg"/>
|
||||
|
||||
<!-- airspeed control -->
|
||||
<!--XPA NOT USED />
|
||||
<define name="AUTO_AIRSPEED_SETPOINT" value="16."/>
|
||||
<define name="AUTO_AIRSPEED_THROTTLE_PGAIN" value="0.1"/>
|
||||
<define name="AUTO_AIRSPEED_THROTTLE_DGAIN" value="0.12"/>
|
||||
<define name="AUTO_AIRSPEED_THROTTLE_IGAIN" value="0.0"/>
|
||||
<define name="AUTO_AIRSPEED_PITCH_PGAIN" value="0.06"/>
|
||||
<define name="AUTO_AIRSPEED_PITCH_DGAIN" value="0.0"/>
|
||||
<define name="AUTO_AIRSPEED_PITCH_IGAIN" value="0.042"/>
|
||||
<define name="AIRSPEED_MAX" value="30"/>
|
||||
<define name="AIRSPEED_MIN" value="10"/-->
|
||||
|
||||
<!-- groundspeed control -->
|
||||
<!--XPA NOT USED />
|
||||
<define name="AUTO_GROUNDSPEED_SETPOINT" value="15"/>
|
||||
<define name="AUTO_GROUNDSPEED_PGAIN" value="1."/>
|
||||
<define name="AUTO_GROUNDSPEED_IGAIN" value="0."/-->
|
||||
|
||||
<!-- pitch trim -->
|
||||
<!--XPA NOT USED />
|
||||
<define name="PITCH_LOITER_TRIM" value="0." unit="deg"/>
|
||||
<define name="PITCH_DASH_TRIM" value="0." unit="deg"/-->
|
||||
|
||||
<define name="THROTTLE_SLEW" value="0.1"/>
|
||||
|
||||
</section>
|
||||
|
||||
<section name="HORIZONTAL CONTROL" prefix="H_CTL_">
|
||||
<define name="COURSE_PGAIN" value="0.743"/>
|
||||
<!--XPA NOT USED />
|
||||
<define name="COURSE_TAU" value="0.5"/-->
|
||||
<define name="ROLL_MAX_SETPOINT" value="30." unit="deg"/>
|
||||
<define name="ROLL_MIN_SETPOINT" value="-30." unit="deg"/>
|
||||
<define name="PITCH_MAX_SETPOINT" value="30." unit="deg"/>
|
||||
<define name="PITCH_MIN_SETPOINT" value="-30." unit="deg"/>
|
||||
|
||||
<define name="ROLL_ATTITUDE_GAIN" value="11000."/>
|
||||
<define name="ROLL_RATE_GAIN" value="1000."/>
|
||||
<define name="ROLL_IGAIN" value="100."/>
|
||||
<!--XPA NOT USED />
|
||||
<define name="ROLL_KFFA" value="0"/>
|
||||
<define name="ROLL_KFFD" value="0"/-->
|
||||
|
||||
<define name="PITCH_PGAIN" value="17250"/>
|
||||
<define name="PITCH_DGAIN" value="500."/>
|
||||
<define name="PITCH_IGAIN" value="400"/>
|
||||
|
||||
<!--XPA NOT USED />
|
||||
<define name="PITCH_KFFA" value="0."/>
|
||||
<define name="PITCH_KFFD" value="0."/-->
|
||||
|
||||
<define name="PITCH_OF_ROLL" value="1." unit="deg"/>
|
||||
<define name="AILERON_OF_THROTTLE" value="0.0"/>
|
||||
<define name="ELEVATOR_OF_ROLL" value="0"/>
|
||||
</section>
|
||||
|
||||
<!--XPA NOT USED />
|
||||
<section name="NAV">
|
||||
<define name="NAV_GLIDE_PITCH_TRIM" value="0"/>
|
||||
</section-->
|
||||
|
||||
<!-- ....................................................... -->
|
||||
|
||||
<!-- OTHERS ................................................ -->
|
||||
|
||||
<section name="MISC">
|
||||
<define name="NOMINAL_AIRSPEED" value="12." unit="m/s"/>
|
||||
<define name="CARROT" value="5." unit="s"/>
|
||||
<define name="KILL_MODE_DISTANCE" value="(1.5*MAX_DIST_FROM_HOME)"/>
|
||||
<define name="DEFAULT_CIRCLE_RADIUS" value="80."/>
|
||||
<define name="UNLOCKED_HOME_MODE" value="TRUE"/>
|
||||
<define name="APP_ANGLE" value="8" unit="deg"/>
|
||||
<!--ATID: Network 3351 (default:3350)
|
||||
ATCH: Channel 17
|
||||
ATP1: Api mode
|
||||
ATBD6: Baudrate 57600
|
||||
ATWR: Write in Flash-->
|
||||
<!--define name="XBEE_INIT" value="\"ATID3351\rATCH17\rATP1\rATBD6\r\""/-->
|
||||
</section>
|
||||
|
||||
<section name="CATAPULT" prefix="NAV_CATAPULT_">
|
||||
<define name="ACCELERATION_THRESHOLD" value="1.0"/>
|
||||
<define name="INITIAL_PITCH" value="20" unit="deg"/>
|
||||
<define name="CLIMB_DISTANCE" value="160"/>
|
||||
</section>
|
||||
|
||||
<section name="FAILSAFE" prefix="FAILSAFE_">
|
||||
<define name="DELAY_WITHOUT_GPS" value="2" unit="s"/>
|
||||
<define name="DEFAULT_THROTTLE" value="0.3" unit="%"/>
|
||||
<define name="DEFAULT_ROLL" value="0.3" unit="rad"/>
|
||||
<define name="DEFAULT_PITCH" value="0.5" unit="rad"/>
|
||||
<define name="HOME_RADIUS" value="100" unit="m"/>
|
||||
</section>
|
||||
|
||||
<section name="SIMULATOR" prefix="NPS_">
|
||||
<define name="JSBSIM_LAUNCHSPEED" value="15"/>
|
||||
<define name="JSBSIM_MODEL" value="easystar" type="string"/>
|
||||
<define name="SENSORS_PARAMS" value="nps_sensors_params_default.h" type="string"/>
|
||||
</section>
|
||||
|
||||
<section name="GCS">
|
||||
<define name="AC_ICON" value="flyingwing"/>
|
||||
</section>
|
||||
|
||||
<!-- ....................................................... -->
|
||||
|
||||
</airframe>
|
||||
@@ -0,0 +1,136 @@
|
||||
<!DOCTYPE airframe SYSTEM "../airframe.dtd">
|
||||
|
||||
<airframe name="Rover Steering">
|
||||
|
||||
<!-- FIRMWARE & MODULES .................................... -->
|
||||
|
||||
<firmware name="rover">
|
||||
<autopilot name="rover_steering_cruise.xml"/>
|
||||
|
||||
<!--target name="ap" board="matek_f405_wmn">
|
||||
<configure name="PERIODIC_FREQUENCY" value="100"/>
|
||||
<module name="radio_control" type="sbus"/>
|
||||
</target-->
|
||||
|
||||
<target name="nps" board="pc">
|
||||
<module name="radio_control" type="ppm"/>
|
||||
<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="4244"/>
|
||||
<configure name="MODEM_PORT_IN" value="4245"/-->
|
||||
</target>
|
||||
|
||||
<module name="actuators" type="pwm"/>
|
||||
<module name="telemetry" type="xbee_api"/>
|
||||
|
||||
<module name="gps" type="ublox">
|
||||
<configure name="GPS_BAUD" value="B57600"/>
|
||||
<configure name="GPS_PORT" value="UART4"/>
|
||||
</module>
|
||||
<!--module name="gps_ubx_ucenter"/-->
|
||||
<module name="imu" type="icm42688"/>
|
||||
<module name="ins"/>
|
||||
<module name="ahrs" type="float_dcm">
|
||||
<define name="AHRS_FLOAT_MIN_SPEED_GPS_COURSE" value="0.1"/>
|
||||
</module>
|
||||
|
||||
<module name="nav" type="rover_base"/>
|
||||
|
||||
<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>
|
||||
|
||||
<!-- ....................................................... -->
|
||||
|
||||
<!-- HARDWARE settings ..................................... -->
|
||||
|
||||
<servos>
|
||||
<servo name="MOTOR_THROTTLE" no="0" min="1000" neutral="1500" max="2000"/>
|
||||
<servo name="MOTOR_STEERING" no="3" min="2000" neutral="1500" max="1000"/>
|
||||
</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>
|
||||
|
||||
<section name="IMU" prefix="IMU_">
|
||||
<!-- Matek_f405_wmn IMU (ICM42688) sensor to body calibration -->
|
||||
<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"/>
|
||||
|
||||
<define name="GYRO_P_SIGN" value="1"/>
|
||||
<define name="GYRO_Q_SIGN" value="1"/>
|
||||
<define name="GYRO_R_SIGN" value="1"/>
|
||||
|
||||
<define name="ACCEL_X_SIGN" value="1"/>
|
||||
<define name="ACCEL_Y_SIGN" value="1"/>
|
||||
<define name="ACCEL_Z_SIGN" value="1"/>
|
||||
</section>
|
||||
|
||||
<!-- LiPo 3300mA 7.4V 2S1P CELL (Soaring)-->
|
||||
<section name="BAT">
|
||||
<define name="CATASTROPHIC_BAT_LEVEL" value="6.20" unit="V"/> <!-- 3.1 V per cell -->
|
||||
<define name="CRITIC_BAT_LEVEL" value="6.50" unit="V"/>
|
||||
<define name="LOW_BAT_LEVEL" value="7.00" unit="V"/>
|
||||
<define name="MAX_BAT_LEVEL" value="8.39" unit="V"/>
|
||||
<define name="MILLIAMP_AT_FULL_THROTTLE" value="3300" unit="mA"/>
|
||||
<define name="MILLIAMP_AT_IDLE_THROTTLE" value="1000" unit="mA"/>
|
||||
<!--define name="CURRENT_ESTIMATION_NONLINEARITY" value="1.0"/-->
|
||||
</section>
|
||||
|
||||
<!-- ....................................................... -->
|
||||
|
||||
<!-- GNS 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="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>
|
||||
|
||||
<!-- ....................................................... -->
|
||||
|
||||
<!-- OTHERS ................................................ -->
|
||||
|
||||
<section name="SIMULATOR" prefix="NPS_">
|
||||
</section>
|
||||
|
||||
<section name="GCS">
|
||||
<define name="AC_ICON" value="rover"/>
|
||||
</section>
|
||||
|
||||
<!-- ....................................................... -->
|
||||
|
||||
</airframe>
|
||||
@@ -0,0 +1,136 @@
|
||||
<!DOCTYPE airframe SYSTEM "../airframe.dtd">
|
||||
|
||||
<airframe name="Rover Steering">
|
||||
|
||||
<!-- FIRMWARE & MODULES .................................... -->
|
||||
|
||||
<firmware name="rover">
|
||||
<autopilot name="rover_steering_cruise.xml"/>
|
||||
|
||||
<!--target name="ap" board="matek_f405_wmn">
|
||||
<configure name="PERIODIC_FREQUENCY" value="100"/>
|
||||
<module name="radio_control" type="sbus"/>
|
||||
</target-->
|
||||
|
||||
<target name="nps" board="pc">
|
||||
<module name="radio_control" type="ppm"/>
|
||||
<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="gps" type="ublox">
|
||||
<configure name="GPS_BAUD" value="B57600"/>
|
||||
<configure name="GPS_PORT" value="UART4"/>
|
||||
</module>
|
||||
<!--module name="gps_ubx_ucenter"/-->
|
||||
<module name="imu" type="icm42688"/>
|
||||
<module name="ins"/>
|
||||
<module name="ahrs" type="float_dcm">
|
||||
<define name="AHRS_FLOAT_MIN_SPEED_GPS_COURSE" value="0.1"/>
|
||||
</module>
|
||||
|
||||
<module name="nav" type="rover_base"/>
|
||||
|
||||
<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>
|
||||
|
||||
<!-- ....................................................... -->
|
||||
|
||||
<!-- HARDWARE settings ..................................... -->
|
||||
|
||||
<servos>
|
||||
<servo name="MOTOR_THROTTLE" no="0" min="1000" neutral="1500" max="2000"/>
|
||||
<servo name="MOTOR_STEERING" no="3" min="1700" neutral="1400" max="1000"/>
|
||||
</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>
|
||||
|
||||
<section name="IMU" prefix="IMU_">
|
||||
<!-- Matek_f405_wmn IMU (ICM42688) sensor to body calibration -->
|
||||
<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"/>
|
||||
|
||||
<define name="GYRO_P_SIGN" value="1"/>
|
||||
<define name="GYRO_Q_SIGN" value="1"/>
|
||||
<define name="GYRO_R_SIGN" value="1"/>
|
||||
|
||||
<define name="ACCEL_X_SIGN" value="1"/>
|
||||
<define name="ACCEL_Y_SIGN" value="1"/>
|
||||
<define name="ACCEL_Z_SIGN" value="1"/>
|
||||
</section>
|
||||
|
||||
<!-- LiPo 3300mA 7.4V 2S1P CELL (Soaring)-->
|
||||
<section name="BAT">
|
||||
<define name="CATASTROPHIC_BAT_LEVEL" value="6.20" unit="V"/> <!-- 3.1 V per cell -->
|
||||
<define name="CRITIC_BAT_LEVEL" value="6.50" unit="V"/>
|
||||
<define name="LOW_BAT_LEVEL" value="7.00" unit="V"/>
|
||||
<define name="MAX_BAT_LEVEL" value="8.39" unit="V"/>
|
||||
<define name="MILLIAMP_AT_FULL_THROTTLE" value="3300" unit="mA"/>
|
||||
<define name="MILLIAMP_AT_IDLE_THROTTLE" value="1000" unit="mA"/>
|
||||
<!--define name="CURRENT_ESTIMATION_NONLINEARITY" value="1.0"/-->
|
||||
</section>
|
||||
|
||||
<!-- ....................................................... -->
|
||||
|
||||
<!-- GNS 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="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>
|
||||
|
||||
<!-- ....................................................... -->
|
||||
|
||||
<!-- OTHERS ................................................ -->
|
||||
|
||||
<section name="SIMULATOR" prefix="NPS_">
|
||||
</section>
|
||||
|
||||
<section name="GCS">
|
||||
<define name="AC_ICON" value="rover"/>
|
||||
</section>
|
||||
|
||||
<!-- ....................................................... -->
|
||||
|
||||
</airframe>
|
||||
@@ -0,0 +1,136 @@
|
||||
<!DOCTYPE airframe SYSTEM "../airframe.dtd">
|
||||
|
||||
<airframe name="Rover Steering">
|
||||
|
||||
<!-- FIRMWARE & MODULES .................................... -->
|
||||
|
||||
<firmware name="rover">
|
||||
<autopilot name="rover_steering_cruise.xml"/>
|
||||
|
||||
<!--target name="ap" board="matek_f405_wmn">
|
||||
<configure name="PERIODIC_FREQUENCY" value="100"/>
|
||||
<module name="radio_control" type="sbus"/>
|
||||
</target-->
|
||||
|
||||
<target name="nps" board="pc">
|
||||
<module name="radio_control" type="ppm"/>
|
||||
<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="4248"/>
|
||||
<configure name="MODEM_PORT_IN" value="4249"/-->
|
||||
</target>
|
||||
|
||||
<module name="actuators" type="pwm"/>
|
||||
<module name="telemetry" type="xbee_api"/>
|
||||
|
||||
<module name="gps" type="ublox">
|
||||
<configure name="GPS_BAUD" value="B57600"/>
|
||||
<configure name="GPS_PORT" value="UART4"/>
|
||||
</module>
|
||||
<!--module name="gps_ubx_ucenter"/-->
|
||||
<module name="imu" type="icm42688"/>
|
||||
<module name="ins"/>
|
||||
<module name="ahrs" type="float_dcm">
|
||||
<define name="AHRS_FLOAT_MIN_SPEED_GPS_COURSE" value="0.1"/>
|
||||
</module>
|
||||
|
||||
<module name="nav" type="rover_base"/>
|
||||
|
||||
<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>
|
||||
|
||||
<!-- ....................................................... -->
|
||||
|
||||
<!-- HARDWARE settings ..................................... -->
|
||||
|
||||
<servos>
|
||||
<servo name="MOTOR_THROTTLE" no="0" min="1000" neutral="1500" max="2000"/>
|
||||
<servo name="MOTOR_STEERING" no="3" min="2000" neutral="1500" max="1000"/>
|
||||
</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>
|
||||
|
||||
<section name="IMU" prefix="IMU_">
|
||||
<!-- Matek_f405_wmn IMU (ICM42688) sensor to body calibration -->
|
||||
<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"/>
|
||||
|
||||
<define name="GYRO_P_SIGN" value="1"/>
|
||||
<define name="GYRO_Q_SIGN" value="1"/>
|
||||
<define name="GYRO_R_SIGN" value="1"/>
|
||||
|
||||
<define name="ACCEL_X_SIGN" value="1"/>
|
||||
<define name="ACCEL_Y_SIGN" value="1"/>
|
||||
<define name="ACCEL_Z_SIGN" value="1"/>
|
||||
</section>
|
||||
|
||||
<!-- LiPo 3300mA 7.4V 2S1P CELL (Soaring)-->
|
||||
<section name="BAT">
|
||||
<define name="CATASTROPHIC_BAT_LEVEL" value="6.20" unit="V"/> <!-- 3.1 V per cell -->
|
||||
<define name="CRITIC_BAT_LEVEL" value="6.50" unit="V"/>
|
||||
<define name="LOW_BAT_LEVEL" value="7.00" unit="V"/>
|
||||
<define name="MAX_BAT_LEVEL" value="8.39" unit="V"/>
|
||||
<define name="MILLIAMP_AT_FULL_THROTTLE" value="3300" unit="mA"/>
|
||||
<define name="MILLIAMP_AT_IDLE_THROTTLE" value="1000" unit="mA"/>
|
||||
<!--define name="CURRENT_ESTIMATION_NONLINEARITY" value="1.0"/-->
|
||||
</section>
|
||||
|
||||
<!-- ....................................................... -->
|
||||
|
||||
<!-- GNS 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="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>
|
||||
|
||||
<!-- ....................................................... -->
|
||||
|
||||
<!-- OTHERS ................................................ -->
|
||||
|
||||
<section name="SIMULATOR" prefix="NPS_">
|
||||
</section>
|
||||
|
||||
<section name="GCS">
|
||||
<define name="AC_ICON" value="rover"/>
|
||||
</section>
|
||||
|
||||
<!-- ....................................................... -->
|
||||
|
||||
</airframe>
|
||||
@@ -13,7 +13,7 @@
|
||||
<define name="STABILIZATION_INDI_G2_R" value="0.0"/><!-- for jsbsim (rotor inertia is not modelled) -->
|
||||
<!-- <define name="STABILIZATION_INDI_G2_R" value="0.20"/> --><!-- for gazebo -->
|
||||
</target>
|
||||
|
||||
|
||||
<!-- Subsystem section -->
|
||||
<module name="telemetry" type="transparent_udp"/>
|
||||
<module name="radio_control" type="datalink"/>
|
||||
@@ -23,8 +23,7 @@
|
||||
<module name="gps" type="ublox"/>
|
||||
<module name="stabilization" type="indi_simple"/>
|
||||
<module name="ins" type="ekf2"/>
|
||||
<module name="gvf_module"/>
|
||||
<!--module name="gvf_parametric"/-->
|
||||
|
||||
<module name="air_data"/>
|
||||
<module name="gps" type="ubx_ucenter"/>
|
||||
<module name="logger_file">
|
||||
|
||||
@@ -78,7 +78,7 @@
|
||||
</control>
|
||||
<control>
|
||||
<call fun="rover_guidance_steering_speed_ctrl()"/>
|
||||
<call fun="rover_guidance_steering_heading_ctrl(gvf_c_omega.omega)"/>
|
||||
<call fun="rover_guidance_steering_heading_ctrl(gvf_c_ctrl.omega)"/>
|
||||
|
||||
<call fun="commands[COMMAND_STEERING] = GetCmdFromDelta(guidance_control.cmd.delta);"/>
|
||||
<call fun="commands[COMMAND_THROTTLE] = GetCmdFromThrottle(guidance_control.throttle);"/>
|
||||
|
||||
@@ -0,0 +1,147 @@
|
||||
<!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="modules/radio_control/radio_control.h"/>
|
||||
<include name="modules/guidance/gvf_common.h"/>
|
||||
<include name="navigation.h"/>
|
||||
<include name="state.h"/>
|
||||
|
||||
<define name="RCLost()" value="(radio_control.status == RC_REALLY_LOST)"/> <!-- TODO: define it in nav.h?? -->
|
||||
<define name="RCCruiseMode0()" value="rc_mode_switch(RADIO_CRUISE_MODE, 0, 2)" cond="ifdef RADIO_CRUISE_MODE"/>
|
||||
<define name="RCCruiseMode1()" value="rc_mode_switch(RADIO_CRUISE_MODE, 1, 2)" cond="ifdef RADIO_CRUISE_MODE"/>
|
||||
<define name="RCCruiseMode0()" value="TRUE" cond="ifndef RADIO_CRUISE_MODE"/>
|
||||
<define name="RCCruiseMode1()" value="FALSE" cond="ifndef RADIO_CRUISE_MODE"/>
|
||||
</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"/> <!-- Radio KILL_SWITCH -->
|
||||
<exception cond="RCLost() || (GpsIsLost() && autopilot_in_flight())" deroute="KILL"/>
|
||||
</exceptions>
|
||||
|
||||
<!-- * AP MODES * ...................................................... -->
|
||||
<!-- RC Manual -->
|
||||
<mode name="DIRECT" shortname="MANUAL">
|
||||
<select cond="RCMode0() && RCCruiseMode0()"/>
|
||||
<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>
|
||||
</mode>
|
||||
|
||||
<!-- Cruiser: Speed ctrl (from state speed on enter)
|
||||
+ heading from RC --> <!-- TODO: delete this mode?? -->
|
||||
<mode name="CRUISER">
|
||||
<select cond="(RCMode1() || RCMode0()) && RCCruiseMode1()"/>
|
||||
<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>
|
||||
</mode>
|
||||
|
||||
<!-- Assisted: RC manul throttle
|
||||
+ heading ctrl (from GCS & flight plans) -->
|
||||
<mode name="ASSISTED">
|
||||
<select cond="(RCMode1() || RCMode2()) && RCCruiseMode0()"/>
|
||||
<on_enter>
|
||||
<call fun="rover_guidance_steering_pid_reset()"/>
|
||||
</on_enter>
|
||||
<control freq="NAVIGATION_FREQUENCY">
|
||||
<call fun="nav_periodic_task()"/>
|
||||
</control>
|
||||
<control>
|
||||
<call fun="SetCommandsFromRC(commands, radio_control.values)"/>
|
||||
<call fun="rover_guidance_steering_heading_ctrl(gvf_c_ctrl.omega)"/>
|
||||
|
||||
<call fun="commands[COMMAND_STEERING] = GetCmdFromDelta(guidance_control.cmd.delta);"/>
|
||||
<call fun="SetActuatorsFromCommands(commands, autopilot_get_mode())"/>
|
||||
|
||||
<call fun="SetAPThrottleFromCommands(commands[COMMAND_THROTTLE])"/>
|
||||
</control>
|
||||
</mode>
|
||||
|
||||
<!-- Navigation: Speed crtl (from state speed on enter)
|
||||
+ heading ctrl (from GCS & flight plans) -->
|
||||
<mode name="NAV">
|
||||
<select cond="$DEFAULT_MODE"/>
|
||||
<select cond="RCMode2() && RCCruiseMode1()"/>
|
||||
<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(gvf_c_ctrl.omega)"/>
|
||||
|
||||
<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>
|
||||
</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>
|
||||
</mode>
|
||||
|
||||
<!-- Kill throttle -->
|
||||
<mode name="KILL">
|
||||
<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>
|
||||
@@ -117,8 +117,8 @@
|
||||
telemetry="telemetry/GVF/gvf_default_rotorcraft.xml"
|
||||
flight_plan="flight_plans/demo_gvf_rotorcraft.xml"
|
||||
settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_speed.xml"
|
||||
settings_modules="modules/air_data.xml modules/electrical.xml modules/gps.xml modules/gps_ublox.xml modules/gps_ubx_ucenter.xml modules/guidance_pid_rotorcraft.xml modules/guidance_rotorcraft.xml modules/gvf_module.xml~GVF~ modules/imu_common.xml modules/ins_ekf2.xml modules/nav_rotorcraft.xml modules/stabilization_indi_simple.xml"
|
||||
gui_color="red"
|
||||
settings_modules="modules/air_data.xml modules/electrical.xml modules/gps.xml modules/gps_ublox.xml modules/gps_ubx_ucenter.xml modules/guidance_pid_rotorcraft.xml modules/guidance_rotorcraft.xml modules/gvf_classic.xml~GVF~ modules/imu_common.xml modules/ins_ekf2.xml modules/nav_rotorcraft.xml modules/stabilization_indi_simple.xml"
|
||||
gui_color="#6200a000ea00"
|
||||
/>
|
||||
<aircraft
|
||||
name="bebop2_opticflow"
|
||||
|
||||
@@ -45,7 +45,7 @@
|
||||
</variables>
|
||||
|
||||
<modules>
|
||||
<module name="gvf_module">
|
||||
<module name="gvf_classic">
|
||||
<define name="GVF_OCAML_GCS" value="FALSE"/>
|
||||
</module>
|
||||
<module name="gvf_parametric"/>
|
||||
@@ -76,43 +76,43 @@
|
||||
</block>
|
||||
|
||||
<block key="s" name="Standby" strip_button="Standby" strip_icon="home.png" group="home">
|
||||
<call fun="gvf_ellipse_wp(WP_ELLIPSE, a_stb, b_stb, gvf_ellipse_par.alpha)"/>
|
||||
<call fun="nav_gvf_ellipse_wp(WP_ELLIPSE, a_stb, b_stb, gvf_ellipse_par.alpha)"/>
|
||||
</block>
|
||||
|
||||
<block name="line_to_HOME" strip_icon="home_drop.png">
|
||||
<call fun="gvf_segment_XY1_XY2(GetPosX(), GetPosY(), 0.f, 0.f)"/>
|
||||
<call fun="nav_gvf_segment_XY1_XY2(GetPosX(), GetPosY(), 0.f, 0.f)"/>
|
||||
</block>
|
||||
|
||||
<block name="ellipse_wp" strip_icon="oval.png">
|
||||
<call fun="gvf_ellipse_wp(WP_ELLIPSE, gvf_ellipse_par.a, gvf_ellipse_par.b, gvf_ellipse_par.alpha)"/>
|
||||
<call fun="nav_gvf_ellipse_wp(WP_ELLIPSE, gvf_ellipse_par.a, gvf_ellipse_par.b, gvf_ellipse_par.alpha)"/>
|
||||
</block>
|
||||
|
||||
<block name="gvf_ellipse_XY" strip_icon="oval.png">
|
||||
<call fun="gvf_ellipse_XY(10.1, 10.1, gvf_ellipse_par.a, gvf_ellipse_par.b, gvf_ellipse_par.alpha)"/>
|
||||
<block name="nav_gvf_ellipse_XY" strip_icon="oval.png">
|
||||
<call fun="nav_gvf_ellipse_XY(10.1, 10.1, gvf_ellipse_par.a, gvf_ellipse_par.b, gvf_ellipse_par.alpha)"/>
|
||||
</block>
|
||||
|
||||
<block name="line_P1_P2" strip_icon="line.png">
|
||||
<call fun="gvf_line_wp1_wp2(WP_P1, WP_P2)"/>
|
||||
<call fun="nav_gvf_line_wp1_wp2(WP_P1, WP_P2)"/>
|
||||
</block>
|
||||
|
||||
<block name="segment_turn_P1_P2" strip_icon="line.png">
|
||||
<call fun="gvf_segment_loop_wp1_wp2(WP_P1, WP_P2, gvf_segment_par.d1, gvf_segment_par.d2)"/>
|
||||
<call fun="nav_gvf_segment_loop_wp1_wp2(WP_P1, WP_P2, gvf_segment_par.d1, gvf_segment_par.d2)"/>
|
||||
</block>
|
||||
|
||||
<block name="sin_p1_p2" strip_icon="line_drop.png">
|
||||
<call fun="gvf_sin_wp1_wp2(WP_P1, WP_P2, gvf_sin_par.w, gvf_sin_par.off, gvf_sin_par.A)"/>
|
||||
<call fun="nav_gvf_sin_wp1_wp2(WP_P1, WP_P2, gvf_sin_par.w, gvf_sin_par.off, gvf_sin_par.A)"/>
|
||||
</block>
|
||||
|
||||
<block name="2D_trefoil_wp" strip_icon="eight.png">
|
||||
<call fun="gvf_parametric_2D_trefoil_wp(WP_P1, gvf_parametric_2d_trefoil_par.w1, gvf_parametric_2d_trefoil_par.w2, gvf_parametric_2d_trefoil_par.ratio, gvf_parametric_2d_trefoil_par.r, gvf_parametric_2d_trefoil_par.alpha)"/>
|
||||
<call fun="nav_gvf_parametric_2D_trefoil_wp(WP_P1, gvf_parametric_2d_trefoil_par.w1, gvf_parametric_2d_trefoil_par.w2, gvf_parametric_2d_trefoil_par.ratio, gvf_parametric_2d_trefoil_par.r, gvf_parametric_2d_trefoil_par.alpha)"/>
|
||||
</block>
|
||||
|
||||
<block name="3D_ellipse_wp" strip_icon="oval.png">
|
||||
<call fun="gvf_parametric_3D_ellipse_wp(WP_ELLIPSE, gvf_parametric_3d_ellipse_par.r, gvf_parametric_3d_ellipse_par.zl, gvf_parametric_3d_ellipse_par.zh, gvf_parametric_3d_ellipse_par.alpha)"/>
|
||||
<call fun="nav_gvf_parametric_3D_ellipse_wp(WP_ELLIPSE, gvf_parametric_3d_ellipse_par.r, gvf_parametric_3d_ellipse_par.zl, gvf_parametric_3d_ellipse_par.zh, gvf_parametric_3d_ellipse_par.alpha)"/>
|
||||
</block>
|
||||
|
||||
<block name="3D_lissajous_wp" strip_icon="eight.png">
|
||||
<call fun="gvf_parametric_3D_lissajous_wp_center(WP_P2, 100, gvf_parametric_3d_lissajous_par.cx, gvf_parametric_3d_lissajous_par.cy, gvf_parametric_3d_lissajous_par.cz, gvf_parametric_3d_lissajous_par.wx, gvf_parametric_3d_lissajous_par.wy, gvf_parametric_3d_lissajous_par.wz, gvf_parametric_3d_lissajous_par.dx, gvf_parametric_3d_lissajous_par.dy, gvf_parametric_3d_lissajous_par.dz, gvf_parametric_3d_lissajous_par.alpha)"/>
|
||||
<call fun="nav_gvf_parametric_3D_lissajous_wp_center(WP_P2, 100, gvf_parametric_3d_lissajous_par.cx, gvf_parametric_3d_lissajous_par.cy, gvf_parametric_3d_lissajous_par.cz, gvf_parametric_3d_lissajous_par.wx, gvf_parametric_3d_lissajous_par.wy, gvf_parametric_3d_lissajous_par.wz, gvf_parametric_3d_lissajous_par.dx, gvf_parametric_3d_lissajous_par.dy, gvf_parametric_3d_lissajous_par.dz, gvf_parametric_3d_lissajous_par.alpha)"/>
|
||||
</block>
|
||||
|
||||
</blocks>
|
||||
|
||||
@@ -79,36 +79,36 @@
|
||||
</block>
|
||||
|
||||
<block name="Standby" strip_button="Standby" strip_icon="home.png">
|
||||
<call fun="gvf_ellipse_wp(WP_STDBY, a_stb, b_stb, 0)"/>
|
||||
<call fun="nav_gvf_ellipse_wp(WP_STDBY, a_stb, b_stb, 0)"/>
|
||||
</block>
|
||||
|
||||
<block name="line_to_HOME" strip_icon="home_drop.png">
|
||||
<call fun="gvf_segment_XY1_XY2(GetPosX(), GetPosY(), 0.f, 0.f)"/>
|
||||
<call fun="nav_gvf_segment_XY1_XY2(GetPosX(), GetPosY(), 0.f, 0.f)"/>
|
||||
<exception cond="! InsideNet(GetPosX(), GetPosY())" deroute="Standby"/>
|
||||
</block>
|
||||
|
||||
<block name="ellipse_wp" strip_icon="oval.png">
|
||||
<call fun="gvf_ellipse_wp(WP_ELLIPSE, gvf_ellipse_par.a, gvf_ellipse_par.b, gvf_ellipse_par.alpha)"/>
|
||||
<call fun="nav_gvf_ellipse_wp(WP_ELLIPSE, gvf_ellipse_par.a, gvf_ellipse_par.b, gvf_ellipse_par.alpha)"/>
|
||||
<exception cond="! InsideNet(GetPosX(), GetPosY())" deroute="Standby"/>
|
||||
</block>
|
||||
|
||||
<block name="line_P1_P2" strip_icon="line.png">
|
||||
<call fun="gvf_line_wp1_wp2(WP_P1, WP_P2)"/>
|
||||
<call fun="nav_gvf_line_wp1_wp2(WP_P1, WP_P2)"/>
|
||||
<exception cond="! InsideNet(GetPosX(), GetPosY())" deroute="Standby"/>
|
||||
</block>
|
||||
|
||||
<block name="segment_turn_P1_P2" strip_icon="line.png">
|
||||
<call fun="gvf_segment_loop_wp1_wp2(WP_P1, WP_P2, gvf_segment_par.d1, gvf_segment_par.d2)"/>
|
||||
<call fun="nav_gvf_segment_loop_wp1_wp2(WP_P1, WP_P2, gvf_segment_par.d1, gvf_segment_par.d2)"/>
|
||||
<exception cond="! InsideNet(GetPosX(), GetPosY())" deroute="Standby"/>
|
||||
</block>
|
||||
|
||||
<block name="sin_p1_p2" strip_icon="line_drop.png">
|
||||
<call fun="gvf_sin_wp1_wp2(WP_P1, WP_P2, gvf_sin_par.w, gvf_sin_par.off, gvf_sin_par.A)"/>
|
||||
<call fun="nav_gvf_sin_wp1_wp2(WP_P1, WP_P2, gvf_sin_par.w, gvf_sin_par.off, gvf_sin_par.A)"/>
|
||||
<exception cond="! InsideNet(GetPosX(), GetPosY())" deroute="Standby"/>
|
||||
</block>
|
||||
|
||||
<block name="2D_bezier" strip_icon="eight.png">
|
||||
<call fun="gvf_parametric_2D_bezier_wp(WP_BZ0)"/>
|
||||
<call fun="nav_gvf_parametric_2D_bezier_wp(WP_BZ0)"/>
|
||||
<exception cond="! InsideNet(GetPosX(), GetPosY())" deroute="Standby"/>
|
||||
</block>
|
||||
|
||||
|
||||
@@ -0,0 +1,188 @@
|
||||
<!DOCTYPE flight_plan SYSTEM "../flight_plan.dtd">
|
||||
|
||||
<flight_plan alt="750" geofence_max_alt="3000" geofence_max_height="1000" ground_alt="720" home_mode_height="200" lat0="37.29745306" lon0="-3.68272961" max_dist_from_home="1500" name="Sonic2" qfu="90" security_height="25">
|
||||
|
||||
<header>
|
||||
</header>
|
||||
|
||||
<waypoints>
|
||||
<waypoint name="HOME" alt="713" x="-0.648" y="-0.647"/>
|
||||
<waypoint name="CLIMB" alt="753" x="-77.375" y="-46.790"/>
|
||||
<!--waypoint name="AF" alt="730" x="-139.4" y="-55.5"/>
|
||||
<waypoint name="TD" alt="720" x="-10.4" y="-3.5"/-->
|
||||
|
||||
<waypoint name="STDBY" lat="37.29826926318695" lon="-3.683916936727701"/>
|
||||
<waypoint name="ELL" lat="37.2997200" lon="-3.6868681"/>
|
||||
<waypoint name="PARAM" lat="37.2987704" lon="-3.6844594"/>
|
||||
<waypoint name="P0" x="-44.1543" y="-30.2445"/>
|
||||
<waypoint name="P1" x="29.784" y="5.24597"/>
|
||||
<waypoint name="P2" x="-269.5" y="143.0"/>
|
||||
<waypoint name="P3" x="-245.7" y="-314.3"/>
|
||||
|
||||
</waypoints>
|
||||
|
||||
<variables>
|
||||
<variable var="a_stb" init="90.0" min="1.0" max="150.0" step="1.0"/>
|
||||
<variable var="b_stb" init="90.0" min="1.0" max="150.0" step="1.0"/>
|
||||
<variable var="alpha_stb" init="0.0" min="0.0" max="180.0" step="1.0"/>
|
||||
</variables>
|
||||
|
||||
<modules>
|
||||
<module name="gvf_classic">
|
||||
<define name="GVF_OCAML_GCS" value="FALSE"/>
|
||||
|
||||
<define name="GVF_ELLIPSE_KE" value="1.2"/>
|
||||
<define name="GVF_ELLIPSE_KN" value="1.1"/>
|
||||
</module>
|
||||
|
||||
<module name="gvf_ik">
|
||||
<define name="GVF_OCAML_GCS" value="FALSE"/>
|
||||
|
||||
<define name="GVF_IK_GAMMA_AMPLITUDE" value=".3"/>
|
||||
<define name="GVF_IK_GAMMA_OMEGA" value=".6"/>
|
||||
|
||||
<define name="GVF_IK_ELLIPSE_KE" value=".6"/>
|
||||
<define name="GVF_IK_ELLIPSE_KN" value="1"/>
|
||||
|
||||
<define name="GVF_IK_LINE_KE" value="18"/>
|
||||
<define name="GVF_IK_LINE_KN" value="1"/>
|
||||
</module>
|
||||
|
||||
<module name="gvf_parametric">
|
||||
<define name="GVF_PARAMETRIC_2D_TREFOIL_R" value="50"/>
|
||||
<define name="GVF_PARAMETRIC_2D_TREFOIL_RATIO" value="160"/>
|
||||
|
||||
<define name="GVF_PARAMETRIC_3D_ELLIPSE_ZL" value="40"/>
|
||||
<define name="GVF_PARAMETRIC_3D_ELLIPSE_ZH" value="60"/>
|
||||
|
||||
<define name="GVF_PARAMETRIC_3D_LISSAJOUS_KX" value="0.01"/>
|
||||
<define name="GVF_PARAMETRIC_3D_LISSAJOUS_KY" value="0.01"/>
|
||||
<define name="GVF_PARAMETRIC_3D_LISSAJOUS_KZ" value="0.01"/>
|
||||
<define name="GVF_PARAMETRIC_3D_LISSAJOUS_CX" value="120"/>
|
||||
<define name="GVF_PARAMETRIC_3D_LISSAJOUS_CY" value="120"/>
|
||||
<define name="GVF_PARAMETRIC_3D_LISSAJOUS_CZ" value="5"/>
|
||||
<define name="GVF_PARAMETRIC_3D_LISSAJOUS_WX" value="1"/>
|
||||
<define name="GVF_PARAMETRIC_3D_LISSAJOUS_WY" value="2"/>
|
||||
<define name="GVF_PARAMETRIC_3D_LISSAJOUS_WZ" value="2"/>
|
||||
<define name="GVF_PARAMETRIC_3D_LISSAJOUS_DX" value="90"/>
|
||||
<define name="GVF_PARAMETRIC_3D_LISSAJOUS_DY" value="90"/>
|
||||
<define name="GVF_PARAMETRIC_3D_LISSAJOUS_DZ" value="-10"/>
|
||||
<define name="GVF_PARAMETRIC_3D_LISSAJOUS_ALPHA" value="105"/>
|
||||
</module>
|
||||
|
||||
</modules>
|
||||
|
||||
<!-- EXCEPTIONS ..................................................... -->
|
||||
|
||||
<exceptions>
|
||||
<!-- Takeoff - Standby -->
|
||||
<exception cond="(IndexOfBlock('Takeoff') == nav_block) @AND
|
||||
(GetPosAlt() @GT (GetAltRef() + 30))" deroute="Standby"/>
|
||||
|
||||
<!-- Block Time GT 300 -->
|
||||
<!--exception cond="!(IndexOfBlock('Standby') @GEQ nav_block) @AND
|
||||
(NavBlockTime() @GT 180)" deroute="Standby"/-->
|
||||
|
||||
<!-- Bat Low -->
|
||||
<!--exception cond="!(IndexOfBlock('Standby') @GEQ nav_block) @AND
|
||||
electrical.bat_critical" deroute="Standby"/-->
|
||||
|
||||
<!-- RC lost && Block Time GT 300 -->
|
||||
<!--exception cond="!(IndexOfBlock('Standby') @GEQ nav_block) @AND
|
||||
(radio_control.status == RC_REALLY_LOST) @AND
|
||||
(NavBlockTime() @GT 300)" deroute="Standby"/-->
|
||||
</exceptions>
|
||||
|
||||
<!-- ................................................................ -->
|
||||
|
||||
<blocks>
|
||||
|
||||
<!-- ## INIT ## -->
|
||||
|
||||
<block name="Wait GPS" strip_icon="gps.png">
|
||||
<set value="1" var="autopilot.kill_throttle"/>
|
||||
<while cond="!GpsFixValid()"/>
|
||||
</block>
|
||||
|
||||
<block name="Geo init" strip_icon="googleearth.png">
|
||||
<while cond="LessThan(NavBlockTime(), 10)"/>
|
||||
<!--call_once fun="NavSetGroundReferenceHere()"/-->
|
||||
</block>
|
||||
|
||||
<block name="Holding point" strip_icon="mob.png">
|
||||
<set value="1" var="autopilot.kill_throttle"/>
|
||||
<attitude roll="0" throttle="0" vmode="throttle"/>
|
||||
</block>
|
||||
|
||||
<!-- ## AUTO TAKEOFF ## -->
|
||||
|
||||
<block name="Takeoff" strip_button="Takeoff (wp CLIMB)" strip_icon="takeoff.png">
|
||||
<set value="0" var="autopilot.kill_throttle"/>
|
||||
<set value="0" var="autopilot.flight_time"/>
|
||||
<set value="1" var="autopilot.launch"/> <!-- Just for simulation -->
|
||||
<go from="HOME" throttle="1.0" vmode="throttle" wp="CLIMB" pitch="25"/>
|
||||
</block>
|
||||
|
||||
<block name="Standby" key="s" strip_button="Standby" strip_icon="home.png">
|
||||
<set var="flight_altitude" value="GetAltRef() + 50"/>
|
||||
<call_once fun="NavVerticalAutoThrottleMode(0.0)"/>
|
||||
<call_once fun="NavVerticalAltitudeMode(flight_altitude, 0.0)"/>
|
||||
<call fun="nav_gvf_ellipse_wp(WP_STDBY, a_stb, b_stb, alpha_stb)"/>
|
||||
</block>
|
||||
|
||||
<!-- ## MISSION ## -->
|
||||
|
||||
<block name="Ellipse ELL" strip_icon="oval.png">
|
||||
<call_once fun="NavVerticalAutoThrottleMode(0.0)"/>
|
||||
<call_once fun="NavVerticalAltitudeMode(flight_altitude, 0.0)"/>
|
||||
<call fun="nav_gvf_ellipse_wp(WP_ELL, gvf_ellipse_par.a, gvf_ellipse_par.b, gvf_ellipse_par.alpha)"/>
|
||||
</block>
|
||||
|
||||
<block name="Segment P0-P1" strip_icon="line.png">
|
||||
<call_once fun="NavVerticalAutoThrottleMode(0.0)"/>
|
||||
<call_once fun="NavVerticalAltitudeMode(flight_altitude, 0.0)"/>
|
||||
<call fun="nav_gvf_segment_wp1_wp2(WP_P0, WP_P1)"/>
|
||||
<deroute block="Standby"/>
|
||||
</block>
|
||||
|
||||
<block name="Line" strip_icon="line.png">
|
||||
<call_once fun="NavVerticalAutoThrottleMode(0.0)"/>
|
||||
<call_once fun="NavVerticalAltitudeMode(flight_altitude, 0.0)"/>
|
||||
<call fun="nav_gvf_line_wp_heading(WP_P0, gvf_line_par.heading)"/>
|
||||
</block>
|
||||
|
||||
<block name="2D Trefoil PARAM" strip_icon="eight.png">
|
||||
<call_once fun="NavVerticalAutoThrottleMode(0.0)"/>
|
||||
<call_once fun="NavVerticalAltitudeMode(flight_altitude, 0.0)"/>
|
||||
<call fun="nav_gvf_parametric_2D_trefoil_wp(WP_PARAM, gvf_parametric_2d_trefoil_par.w1, gvf_parametric_2d_trefoil_par.w2, gvf_parametric_2d_trefoil_par.ratio, gvf_parametric_2d_trefoil_par.r, gvf_parametric_2d_trefoil_par.alpha)"/>
|
||||
</block>
|
||||
|
||||
<block name="3D Ellipse PARAM" strip_icon="eight.png">
|
||||
<call fun="nav_gvf_parametric_3D_ellipse_wp(WP_PARAM, gvf_parametric_3d_ellipse_par.r, gvf_parametric_3d_ellipse_par.zl, gvf_parametric_3d_ellipse_par.zh, gvf_parametric_3d_ellipse_par.alpha)"/>
|
||||
</block>
|
||||
|
||||
<block name="3D Lissajous PARAM" strip_icon="eight.png">
|
||||
<call fun="nav_gvf_parametric_3D_lissajous_wp_center(WP_PARAM,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="Ellipse IK ELL" strip_icon="oval.png">
|
||||
<call_once fun="NavVerticalAutoThrottleMode(0.0)"/>
|
||||
<call_once fun="NavVerticalAltitudeMode(flight_altitude, 0.0)"/>
|
||||
<call fun="nav_gvf_ik_ellipse_wp(WP_ELL, gvf_ik_ellipse_par.a, gvf_ik_ellipse_par.b, gvf_ik_ellipse_par.alpha)"/>
|
||||
</block>
|
||||
|
||||
<block name="Segment IK P2-P3" strip_icon="line.png">
|
||||
<call_once fun="NavVerticalAutoThrottleMode(0.0)"/>
|
||||
<call_once fun="NavVerticalAltitudeMode(flight_altitude, 0.0)"/>
|
||||
<call fun="nav_gvf_ik_segment_wp1_wp2(WP_P2, WP_P3)"/>
|
||||
<deroute block="Standby"/>
|
||||
</block>
|
||||
|
||||
<!-- ## AUTO LANDING ## -->
|
||||
|
||||
<!-- TODO: implement once it is ready -->
|
||||
|
||||
<!-- ########################### -->
|
||||
|
||||
</blocks>
|
||||
</flight_plan>
|
||||
@@ -0,0 +1,129 @@
|
||||
<!DOCTYPE flight_plan SYSTEM "../flight_plan.dtd">
|
||||
|
||||
<flight_plan alt="750" geofence_max_alt="3000" geofence_max_height="1000" ground_alt="720" home_mode_height="200" lat0="37.29745306" lon0="-3.68272961" max_dist_from_home="1500" name="Sonic2" qfu="90" security_height="25">
|
||||
|
||||
<header>
|
||||
</header>
|
||||
|
||||
<waypoints>
|
||||
<waypoint name="HOME" alt="713" x="-0.648" y="-0.647"/>
|
||||
<waypoint name="CLIMB" alt="753" x="-77.375" y="-46.790"/>
|
||||
<!--waypoint name="AF" alt="730" x="-139.4" y="-55.5"/>
|
||||
<waypoint name="TD" alt="720" x="-10.4" y="-3.5"/-->
|
||||
|
||||
<waypoint name="STDBY" lat="37.29826926318695" lon="-3.683916936727701"/>
|
||||
<waypoint name="ELL" lat="37.2997200" lon="-3.6868681"/>
|
||||
<waypoint name="P0" x="-44.1543" y="-30.2445"/>
|
||||
<waypoint name="P1" x="29.784" y="5.24597"/>
|
||||
</waypoints>
|
||||
|
||||
<variables>
|
||||
<variable var="a_stb" init="90.0" min="1.0" max="150.0" step="1.0"/>
|
||||
<variable var="b_stb" init="90.0" min="1.0" max="150.0" step="1.0"/>
|
||||
<variable var="alpha_stb" init="0.0" min="0.0" max="180.0" step="1.0"/>
|
||||
<variable var="land_rad" init="50.0" min="1.0" max="150.0" step="1.0"/>
|
||||
<variable var="ell_delta" init="10.0" min="0.0" max="100.0" step="1.0" />
|
||||
</variables>
|
||||
|
||||
<modules>
|
||||
<module name="gvf_classic">
|
||||
<define name="GVF_OCAML_GCS" value="FALSE"/>
|
||||
|
||||
<define name="GVF_ELLIPSE_KE" value="1.2"/>
|
||||
<define name="GVF_ELLIPSE_KN" value="1.1"/>
|
||||
</module>
|
||||
|
||||
<module name="gvf_ik">
|
||||
<define name="GVF_OCAML_GCS" value="FALSE"/>
|
||||
|
||||
<define name="GVF_IK_GAMMA_AMPLITUDE" value=".3"/>
|
||||
<define name="GVF_IK_GAMMA_OMEGA" value=".6"/>
|
||||
|
||||
<define name="GVF_IK_ELLIPSE_KE" value=".4"/>
|
||||
<define name="GVF_IK_ELLIPSE_KN" value="1"/>
|
||||
|
||||
<define name="GVF_IK_LINE_KE" value=".4"/>
|
||||
<define name="GVF_IK_LINE_KN" value="1"/>
|
||||
</module>
|
||||
</modules>
|
||||
|
||||
<!-- EXCEPTIONS ..................................................... -->
|
||||
|
||||
<exceptions>
|
||||
<!-- Takeoff - Standby -->
|
||||
<exception cond="(IndexOfBlock('Takeoff') == nav_block) @AND
|
||||
(GetPosAlt() @GT (GetAltRef() + 30))" deroute="Standby"/>
|
||||
|
||||
<!-- Block Time GT 300 -->
|
||||
<!--exception cond="!(IndexOfBlock('Standby') @GEQ nav_block) @AND
|
||||
(NavBlockTime() @GT 180)" deroute="Standby"/-->
|
||||
|
||||
<!-- Bat Low -->
|
||||
<!--exception cond="!(IndexOfBlock('Standby') @GEQ nav_block) @AND
|
||||
electrical.bat_critical" deroute="Standby"/-->
|
||||
|
||||
<!-- RC lost && Block Time GT 300 -->
|
||||
<!--exception cond="!(IndexOfBlock('Standby') @GEQ nav_block) @AND
|
||||
(radio_control.status == RC_REALLY_LOST) @AND
|
||||
(NavBlockTime() @GT 300)" deroute="Standby"/-->
|
||||
</exceptions>
|
||||
|
||||
<!-- ................................................................ -->
|
||||
|
||||
<blocks>
|
||||
|
||||
<!-- ## INIT ## -->
|
||||
|
||||
<block name="Wait GPS" strip_icon="gps.png">
|
||||
<set value="1" var="autopilot.kill_throttle"/>
|
||||
<while cond="!GpsFixValid()"/>
|
||||
</block>
|
||||
|
||||
<block name="Geo init" strip_icon="googleearth.png">
|
||||
<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>
|
||||
|
||||
<!-- ## AUTO TAKEOFF ## -->
|
||||
|
||||
<block name="Takeoff" strip_button="Takeoff (wp CLIMB)" strip_icon="takeoff.png">
|
||||
<set value="0" var="autopilot.kill_throttle"/>
|
||||
<set value="0" var="autopilot.flight_time"/>
|
||||
<set value="1" var="autopilot.launch"/> <!-- Just for simulation -->
|
||||
<go from="HOME" throttle="1.0" vmode="throttle" wp="CLIMB" pitch="25"/>
|
||||
</block>
|
||||
|
||||
<block name="Standby" key="s" strip_button="Standby" strip_icon="home.png">
|
||||
<set var="flight_altitude" value="GetAltRef() + 50"/>
|
||||
<call_once fun="NavVerticalAutoThrottleMode(0.0)"/>
|
||||
<call_once fun="NavVerticalAltitudeMode(flight_altitude, 0.0)"/>
|
||||
<call fun="nav_gvf_ellipse_wp(WP_STDBY, a_stb, b_stb, alpha_stb)"/>
|
||||
</block>
|
||||
|
||||
<!-- ## MISSION ## -->
|
||||
|
||||
<block name="Ellipse ELL" strip_icon="oval.png">
|
||||
<call fun="nav_gvf_ellipse_wp(WP_ELL, gvf_ellipse_par.a, gvf_ellipse_par.b, gvf_ellipse_par.alpha)"/>
|
||||
</block>
|
||||
|
||||
<block name="Ellipse IK ELL" strip_icon="oval.png">
|
||||
<call fun="nav_gvf_ik_ellipse_wp(WP_ELL, gvf_ik_ellipse_par.a, gvf_ik_ellipse_par.b, gvf_ik_ellipse_par.alpha)"/>
|
||||
</block>
|
||||
|
||||
<block name="Segment IK P2-P3" strip_icon="line.png">
|
||||
<call fun="nav_gvf_ik_line_wp1_wp2(WP_P0, WP_P1)"/>
|
||||
</block>
|
||||
|
||||
<!-- ## AUTO LANDING ## -->
|
||||
|
||||
<!-- TODO: implement once it is ready -->
|
||||
|
||||
<!-- ########################### -->
|
||||
|
||||
</blocks>
|
||||
</flight_plan>
|
||||
@@ -0,0 +1,150 @@
|
||||
<!DOCTYPE flight_plan SYSTEM "../flight_plan.dtd">
|
||||
|
||||
<flight_plan alt="750" geofence_max_alt="3000" geofence_max_height="1000" ground_alt="720" home_mode_height="200" lat0="37.29745306" lon0="-3.68272961" max_dist_from_home="1500" name="param" qfu="90" security_height="25">
|
||||
|
||||
<header>
|
||||
</header>
|
||||
|
||||
<waypoints>
|
||||
<waypoint name="HOME" alt="713" x="-0.648" y="-0.647"/>
|
||||
<waypoint name="CLIMB" alt="753" x="-77.375" y="-46.790"/>
|
||||
<!--waypoint name="AF" alt="730" x="-139.4" y="-55.5"/>
|
||||
<waypoint name="TD" alt="720" x="-10.4" y="-3.5"/-->
|
||||
|
||||
<waypoint name="STDBY" lat="37.29826926318695" lon="-3.683916936727701"/>
|
||||
<waypoint name="ELL" lat="37.2982692" lon="-3.6839169"/>
|
||||
<waypoint name="TREF" lat="37.2987704" lon="-3.6844594"/>
|
||||
<waypoint name="PARAM" lat="37.2987704" lon="-3.6844594"/>
|
||||
<waypoint name="P0" x="-44.1543" y="-30.2445"/>
|
||||
<waypoint name="P1" x="29.784" y="5.24597"/>
|
||||
</waypoints>
|
||||
|
||||
<variables>
|
||||
<variable var="a_stb" init="90.0" min="1.0" max="150.0" step="1.0"/>
|
||||
<variable var="b_stb" init="90.0" min="1.0" max="150.0" step="1.0"/>
|
||||
<variable var="alpha_stb" init="0.0" min="0.0" max="180.0" step="1.0"/>
|
||||
<variable var="land_rad" init="90.0" min="1.0" max="150.0" step="1.0"/>
|
||||
<variable var="land_min_dist" init="10" min="1.0" max="150.0" step="1.0"/>
|
||||
<variable var="flare_alt" init="5" min="1.0" max="30.0" step="1.0"/>
|
||||
</variables>
|
||||
|
||||
<modules>
|
||||
<module name="gvf_classic">
|
||||
<define name="GVF_OCAML_GCS" value="FALSE"/>
|
||||
</module>
|
||||
<module name="gvf_parametric">
|
||||
<define name="GVF_PARAMETRIC_2D_TREFOIL_R" value="50"/>
|
||||
<define name="GVF_PARAMETRIC_2D_TREFOIL_RATIO" value="160"/>
|
||||
|
||||
<define name="GVF_PARAMETRIC_3D_ELLIPSE_ZL" value="40"/>
|
||||
<define name="GVF_PARAMETRIC_3D_ELLIPSE_ZH" value="60"/>
|
||||
|
||||
<define name="GVF_PARAMETRIC_3D_LISSAJOUS_KX" value="0.01"/>
|
||||
<define name="GVF_PARAMETRIC_3D_LISSAJOUS_KY" value="0.01"/>
|
||||
<define name="GVF_PARAMETRIC_3D_LISSAJOUS_KZ" value="0.01"/>
|
||||
<define name="GVF_PARAMETRIC_3D_LISSAJOUS_CX" value="120"/>
|
||||
<define name="GVF_PARAMETRIC_3D_LISSAJOUS_CY" value="120"/>
|
||||
<define name="GVF_PARAMETRIC_3D_LISSAJOUS_CZ" value="5"/>
|
||||
<define name="GVF_PARAMETRIC_3D_LISSAJOUS_WX" value="1"/>
|
||||
<define name="GVF_PARAMETRIC_3D_LISSAJOUS_WY" value="2"/>
|
||||
<define name="GVF_PARAMETRIC_3D_LISSAJOUS_WZ" value="2"/>
|
||||
<define name="GVF_PARAMETRIC_3D_LISSAJOUS_DX" value="90"/>
|
||||
<define name="GVF_PARAMETRIC_3D_LISSAJOUS_DY" value="90"/>
|
||||
<define name="GVF_PARAMETRIC_3D_LISSAJOUS_DZ" value="-10"/>
|
||||
<define name="GVF_PARAMETRIC_3D_LISSAJOUS_ALPHA" value="105"/>
|
||||
</module>
|
||||
</modules>
|
||||
|
||||
<!-- EXCEPTIONS ..................................................... -->
|
||||
|
||||
<exceptions>
|
||||
<!-- Takeoff - Standby -->
|
||||
<exception cond="(IndexOfBlock('Takeoff') == nav_block) @AND
|
||||
(GetPosAlt() @GT (GetAltRef() + 30))" deroute="Standby"/>
|
||||
|
||||
<!-- Block Time GT 300 -->
|
||||
<!--exception cond="!(IndexOfBlock('Standby') @GEQ nav_block) @AND
|
||||
(NavBlockTime() @GT 180)" deroute="Standby"/-->
|
||||
|
||||
<!-- Bat Low -->
|
||||
<!--exception cond="!(IndexOfBlock('Standby') @GEQ nav_block) @AND
|
||||
electrical.bat_critical" deroute="Standby"/-->
|
||||
|
||||
<!-- RC lost && Block Time GT 300 -->
|
||||
<!--exception cond="!(IndexOfBlock('Standby') @GEQ nav_block) @AND
|
||||
(radio_control.status == RC_REALLY_LOST) @AND
|
||||
(NavBlockTime() @GT 300)" deroute="Standby"/-->
|
||||
</exceptions>
|
||||
|
||||
<!-- ................................................................ -->
|
||||
|
||||
<blocks>
|
||||
|
||||
<!-- ## INIT ## -->
|
||||
|
||||
<block name="Wait GPS" strip_icon="gps.png">
|
||||
<set value="1" var="autopilot.kill_throttle"/>
|
||||
<while cond="!GpsFixValid()"/>
|
||||
</block>
|
||||
|
||||
<block name="Geo init" strip_icon="googleearth.png">
|
||||
<while cond="LessThan(NavBlockTime(), 10)"/>
|
||||
<!--call_once fun="NavSetGroundReferenceHere()"/-->
|
||||
<!-- <call_once fun="NavSetAltitudeReferenceHere()"/> -->
|
||||
</block>
|
||||
|
||||
<block name="Holding point">
|
||||
<set value="1" var="autopilot.kill_throttle"/>
|
||||
<attitude roll="0" throttle="0" vmode="throttle"/>
|
||||
</block>
|
||||
|
||||
<!-- ## AUTO TAKEOFF ## -->
|
||||
|
||||
<block name="Takeoff" strip_button="Takeoff (wp CLIMB)" strip_icon="takeoff.png">
|
||||
<set value="0" var="autopilot.kill_throttle"/>
|
||||
<set value="0" var="autopilot.flight_time"/>
|
||||
<set value="1" var="autopilot.launch"/> <!-- Just for simulation -->
|
||||
<go from="HOME" throttle="1.0" vmode="throttle" wp="CLIMB" pitch="25"/>
|
||||
</block>
|
||||
|
||||
<block name="Standby" key="s" strip_button="Standby" strip_icon="home.png">
|
||||
<set var="flight_altitude" value="GetAltRef() + 50"/>
|
||||
<call_once fun="NavVerticalAutoThrottleMode(0.0)"/>
|
||||
<call_once fun="NavVerticalAltitudeMode(flight_altitude, 0.0)"/>
|
||||
<call fun="nav_gvf_ellipse_wp(WP_STDBY, a_stb, b_stb, alpha_stb)"/>
|
||||
</block>
|
||||
|
||||
<!-- ## MISSION ## -->
|
||||
|
||||
<block name="ellipse_wp" strip_icon="oval.png">
|
||||
<call_once fun="NavVerticalAutoThrottleMode(0.0)"/>
|
||||
<call_once fun="NavVerticalAltitudeMode(flight_altitude, 0.0)"/>
|
||||
<call fun="nav_gvf_ellipse_wp(WP_ELL, gvf_ellipse_par.a, gvf_ellipse_par.b, gvf_ellipse_par.alpha)"/>
|
||||
</block>
|
||||
|
||||
<block name="2D_trefoil">
|
||||
<call_once fun="NavVerticalAutoThrottleMode(0.0)"/>
|
||||
<call_once fun="NavVerticalAltitudeMode(flight_altitude, 0.0)"/>
|
||||
<call fun="nav_gvf_parametric_2D_trefoil_wp(WP_TREF, gvf_parametric_2d_trefoil_par.w1, gvf_parametric_2d_trefoil_par.w2, gvf_parametric_2d_trefoil_par.ratio, gvf_parametric_2d_trefoil_par.r, gvf_parametric_2d_trefoil_par.alpha)"/>
|
||||
</block>
|
||||
|
||||
<block name="3D_ellipse">
|
||||
<call fun="nav_gvf_parametric_3D_ellipse_wp(WP_ELL, gvf_parametric_3d_ellipse_par.r, gvf_parametric_3d_ellipse_par.zl, gvf_parametric_3d_ellipse_par.zh, gvf_parametric_3d_ellipse_par.alpha)"/>
|
||||
</block>
|
||||
|
||||
<block name="3D_lissajous">
|
||||
<call fun="nav_gvf_parametric_3D_lissajous_wp_center(WP_ELL,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="segment" strip_icon="home_drop.png">
|
||||
<call fun="nav_gvf_segment_loop_wp1_wp2(WP_P0, WP_P1, gvf_segment_par.d1, gvf_segment_par.d2)"/>
|
||||
</block>
|
||||
</blocks>
|
||||
|
||||
<!-- ## AUTO LANDING ## -->
|
||||
|
||||
<!-- TODO: implement once it is ready -->
|
||||
|
||||
<!-- ########################### -->
|
||||
|
||||
</flight_plan>
|
||||
@@ -0,0 +1,124 @@
|
||||
<!DOCTYPE flight_plan SYSTEM "../flight_plan.dtd">
|
||||
|
||||
<flight_plan alt="750" ground_alt="720" lat0="37.29745306" lon0="-3.68272961" max_dist_from_home="100" name="Rover Steering" security_height="0.3">
|
||||
|
||||
<header>
|
||||
<!-- CITIC coords for bebugging: lat0="37.19762703" lon0="-3.62415667"-->
|
||||
</header>
|
||||
|
||||
<waypoints>
|
||||
<waypoint name="HOME" x="0" y="0"/>
|
||||
<waypoint name="STDBY" lat="37.2975591" lon="-3.6831233"/>
|
||||
<waypoint name="ELL" lat="37.2975632" lon="-3.6831887"/>
|
||||
<waypoint name="P0" lat="37.2978504" lon="-3.6832013"/>
|
||||
<waypoint name="P1" lat="37.2974861" lon="-3.6828853"/>
|
||||
|
||||
<!-- Bézier Waypoints. Define them in order -->
|
||||
<waypoint name="BZ0" x="1.7" y="-6.0"/>
|
||||
<waypoint name="BZ1" x="-1.4" y="-14.7"/>
|
||||
<waypoint name="BZ2" x="-0.688" y="-24.723"/>
|
||||
<waypoint name="BZ3" x="6.3" y="-26.2"/>
|
||||
<waypoint name="BZ4" x="16.9" y="-30.7"/>
|
||||
<waypoint name="BZ5" x="25.6" y="-30.0"/>
|
||||
<waypoint name="BZ6" x="31.0" y="-22.6"/>
|
||||
<waypoint name="BZ7" x="33.4" y="-14.7"/>
|
||||
<waypoint name="BZ8" x="32.3" y="-7.0"/>
|
||||
<waypoint name="BZ9" x="27.3" y="-2.0"/>
|
||||
<waypoint name="BZ10" x="19.7" y="0.6"/>
|
||||
<waypoint name="BZ11" x="12.6" y="1.6"/>
|
||||
<waypoint name="BZ12" x="8.373" y="-2.597"/>
|
||||
|
||||
</waypoints>
|
||||
|
||||
<modules>
|
||||
<module name="gvf_classic">
|
||||
<define name="GVF_OCAML_GCS" value="FALSE"/>
|
||||
<define name="GVF_ELLIPSE_KE" value="1.5"/>
|
||||
<define name="GVF_ELLIPSE_KN" value="1.5"/>
|
||||
<define name="GVF_ELLIPSE_A" value="10"/>
|
||||
<define name="GVF_ELLIPSE_B" value="10"/>
|
||||
<define name="GVF_ELLIPSE_ALPHA" value="0"/>
|
||||
|
||||
<define name="GVF_LINE_KE" value="20"/>
|
||||
<define name="GVF_LINE_KN" value="2"/>
|
||||
</module>
|
||||
|
||||
|
||||
<module name="gvf_ik">
|
||||
<define name="GVF_OCAML_GCS" value="FALSE"/>
|
||||
|
||||
<define name="GVF_IK_GAMMA_AMPLITUDE" value=".1"/>
|
||||
<define name="GVF_IK_GAMMA_OMEGA" value=".6"/>
|
||||
<define name="GVF_IK_ELLIPSE_A" value="10"/>
|
||||
<define name="GVF_IK_ELLIPSE_B" value="10"/>
|
||||
<define name="GVF_IK_ELLIPSE_ALPHA" value="0"/>
|
||||
|
||||
<define name="GVF_IK_ELLIPSE_KE" value="1"/>
|
||||
<define name="GVF_IK_ELLIPSE_KN" value="2"/>
|
||||
</module>
|
||||
|
||||
<module name="gvf_parametric">
|
||||
<define name="GVF_PARAMETRIC_2D_BEZIER_N_SEG" value="4"/>
|
||||
</module>
|
||||
</modules>
|
||||
|
||||
<!-- EXCEPTIONS ..................................................... -->
|
||||
|
||||
<!-- ................................................................ -->
|
||||
|
||||
<blocks>
|
||||
|
||||
<!-- ## INIT ## -->
|
||||
|
||||
<block name="Wait GPS" strip_icon="gps.png">
|
||||
<call_once fun="NavKillThrottle()"/>
|
||||
<while cond="!GpsFixValid()"/>
|
||||
</block>
|
||||
|
||||
<block name="Geo init" strip_icon="googleearth.png">
|
||||
<while cond="LessThan(NavBlockTime(), 10)"/>
|
||||
<!--call_once fun="NavSetGroundReferenceHere()"/-->
|
||||
<!--call_once fun="NavSetAltitudeReferenceHere()"/-->
|
||||
</block>
|
||||
|
||||
<block name="Start Engine" strip_icon="on.png">
|
||||
<while cond="LessThan(NavBlockTime(), 1)"/>
|
||||
<call_once fun="autopilot_set_motors_on(TRUE)"/>
|
||||
</block>
|
||||
|
||||
<block name="Holding Point" strip_icon="mob.png">
|
||||
<attitude roll="0" throttle="0" until="FALSE" vmode="throttle"/>
|
||||
</block>
|
||||
|
||||
<block name="Standby" strip_icon="home.png">
|
||||
<call fun="nav_gvf_ellipse_wp(WP_STDBY, 10, 10, 0)"/>
|
||||
</block>
|
||||
|
||||
<!-- ## MISSION ## -->
|
||||
|
||||
<block name="Ellipse ELL" strip_icon="oval.png">
|
||||
<call fun="nav_gvf_ellipse_wp(WP_ELL, gvf_ellipse_par.a, gvf_ellipse_par.b, gvf_ellipse_par.alpha)"/>
|
||||
</block>
|
||||
|
||||
<block name="Ellipse-IK ELL" strip_icon="oval.png">
|
||||
<call fun="nav_gvf_ik_ellipse_wp(WP_ELL, gvf_ik_ellipse_par.a, gvf_ik_ellipse_par.b, gvf_ik_ellipse_par.alpha)"/>
|
||||
</block>
|
||||
|
||||
<block name="Segment P0-P1" strip_icon="line.png">
|
||||
<call fun="nav_gvf_segment_wp1_wp2(WP_P0, WP_P1)"/>
|
||||
<deroute block="Standby"/>
|
||||
</block>
|
||||
|
||||
<block name="Line to HOME" strip_icon="home_drop.png">
|
||||
<call fun="nav_gvf_segment_XY1_XY2(GetPosX(), GetPosY(), 0.f, 0.f)"/>
|
||||
<deroute block="Standby"/>
|
||||
</block>
|
||||
|
||||
<block name="2D_bezier" strip_icon="eight.png">
|
||||
<call fun="nav_gvf_parametric_2D_bezier_wp(WP_BZ0)"/>
|
||||
</block>
|
||||
|
||||
<!-- ########################### -->
|
||||
|
||||
</blocks>
|
||||
</flight_plan>
|
||||
+193
-54
@@ -6,16 +6,23 @@
|
||||
</header>
|
||||
<waypoints>
|
||||
<waypoint name="HOME" x="0" y="0"/>
|
||||
<waypoint alt="235" name="STDBY" x="49.5" y="100.1"/>
|
||||
<waypoint alt="215.0" name="AF" x="177.4" y="45.1"/>
|
||||
<waypoint alt="185.0" name="TD" x="28.8" y="57.0"/>
|
||||
<waypoint name="AF" x="177.4" y="45.1" alt="215.0"/>
|
||||
<waypoint name="TD" x="28.8" y="57.0" alt="185.0"/>
|
||||
<waypoint name="_BASELEG" x="168.8" y="-13.8"/>
|
||||
<waypoint name="CLIMB" x="-114.5" y="162.3"/>
|
||||
<waypoint alt="235.0" name="ELLIPSE" x="0.5" y="109.0"/>
|
||||
<waypoint alt="235.0" name="S1" x="-14.8" y="157.8"/>
|
||||
<waypoint alt="235.0" name="S2" x="175.7" y="85.2"/>
|
||||
<waypoint alt="235.0" name="S3" x="140.0" y="-2.3"/>
|
||||
<waypoint alt="235.0" name="S4" x="-49.6" y="66.3"/>
|
||||
|
||||
<waypoint name="STDBY" x="49.5" y="100.1" alt="235"/>
|
||||
<waypoint name="ELL" x="0.5" y="109.0"/>
|
||||
<waypoint name="S1" x="-14.8" y="157.8"/>
|
||||
<waypoint name="S2" x="175.7" y="85.2"/>
|
||||
<waypoint name="S3" x="140.0" y="-2.3"/>
|
||||
<waypoint name="S4" x="-49.6" y="66.3"/>
|
||||
|
||||
<waypoint name="P0" x="-60.1" y="85.0"/>
|
||||
<waypoint name="P1" x="134.2" y="-14.5"/>
|
||||
<waypoint name="P2" x="-269.5" y="143.0"/>
|
||||
<waypoint name="P3" x="-245.7" y="-314.3"/>
|
||||
|
||||
</waypoints>
|
||||
|
||||
<sectors>
|
||||
@@ -28,90 +35,222 @@
|
||||
</sectors>
|
||||
|
||||
<variables>
|
||||
<variable var="a_stb" init="90.0" min="1.0" max="150.0" step="1.0"/>
|
||||
<variable var="b_stb" init="90.0" min="1.0" max="150.0" step="1.0"/>
|
||||
<variable var="alpha_stb" init="0.0" min="0.0" max="180.0" step="1.0"/>
|
||||
|
||||
<variable var="angle_ps" init="0" min="-180" max="179" step="1"/>
|
||||
<variable init="0" max="100" min="0" step="5" var="ell_delta"/>
|
||||
<variable var="ell_delta" init="0" min="0" max="100" step="5" />
|
||||
</variables>
|
||||
|
||||
<modules>
|
||||
<module name="gvf_module"/>
|
||||
<module name="gvf_parametric"/>
|
||||
<module name="gvf_classic">
|
||||
<define name="GVF_OCAML_GCS" value="FALSE"/>
|
||||
|
||||
<define name="GVF_ELLIPSE_KE" value="1.2"/>
|
||||
<define name="GVF_ELLIPSE_KN" value="1.1"/>
|
||||
|
||||
<define name="GVF_SIN_W" value="0.005"/>
|
||||
<define name="GVF_SIN_A" value="5"/>
|
||||
</module>
|
||||
|
||||
<module name="gvf_ik">
|
||||
<define name="GVF_OCAML_GCS" value="FALSE"/>
|
||||
|
||||
<define name="GVF_IK_GAMMA_AMPLITUDE" value=".3"/>
|
||||
<define name="GVF_IK_GAMMA_OMEGA" value=".6"/>
|
||||
|
||||
<define name="GVF_IK_ELLIPSE_KE" value=".6"/>
|
||||
<define name="GVF_IK_ELLIPSE_KN" value="1"/>
|
||||
|
||||
<define name="GVF_IK_LINE_KE" value="18"/>
|
||||
<define name="GVF_IK_LINE_KN" value="1"/>
|
||||
</module>
|
||||
|
||||
<module name="gvf_parametric">
|
||||
<define name="GVF_PARAMETRIC_2D_TREFOIL_R" value="50"/>
|
||||
<define name="GVF_PARAMETRIC_2D_TREFOIL_RATIO" value="160"/>
|
||||
|
||||
<define name="GVF_PARAMETRIC_3D_ELLIPSE_ZL" value="40"/>
|
||||
<define name="GVF_PARAMETRIC_3D_ELLIPSE_ZH" value="60"/>
|
||||
|
||||
<define name="GVF_PARAMETRIC_3D_LISSAJOUS_KX" value="0.01"/>
|
||||
<define name="GVF_PARAMETRIC_3D_LISSAJOUS_KY" value="0.01"/>
|
||||
<define name="GVF_PARAMETRIC_3D_LISSAJOUS_KZ" value="0.01"/>
|
||||
<define name="GVF_PARAMETRIC_3D_LISSAJOUS_CX" value="120"/>
|
||||
<define name="GVF_PARAMETRIC_3D_LISSAJOUS_CY" value="120"/>
|
||||
<define name="GVF_PARAMETRIC_3D_LISSAJOUS_CZ" value="5"/>
|
||||
<define name="GVF_PARAMETRIC_3D_LISSAJOUS_WX" value="1"/>
|
||||
<define name="GVF_PARAMETRIC_3D_LISSAJOUS_WY" value="2"/>
|
||||
<define name="GVF_PARAMETRIC_3D_LISSAJOUS_WZ" value="2"/>
|
||||
<define name="GVF_PARAMETRIC_3D_LISSAJOUS_DX" value="90"/>
|
||||
<define name="GVF_PARAMETRIC_3D_LISSAJOUS_DY" value="90"/>
|
||||
<define name="GVF_PARAMETRIC_3D_LISSAJOUS_DZ" value="-10"/>
|
||||
<define name="GVF_PARAMETRIC_3D_LISSAJOUS_ALPHA" value="105"/>
|
||||
</module>
|
||||
|
||||
</modules>
|
||||
|
||||
<!-- EXCEPTIONS ..................................................... -->
|
||||
|
||||
<exceptions>
|
||||
<!-- Takeoff - Standby -->
|
||||
<exception cond="(IndexOfBlock('Takeoff') == nav_block) @AND
|
||||
(GetPosAlt() @GT (GetAltRef() + 30))" deroute="Standby"/>
|
||||
|
||||
<!-- Block Time GT 300 -->
|
||||
<!--exception cond="!(IndexOfBlock('Standby') @GEQ nav_block) @AND
|
||||
(NavBlockTime() @GT 180)" deroute="Standby"/-->
|
||||
|
||||
<!-- Bat Low -->
|
||||
<!--exception cond="!(IndexOfBlock('Standby') @GEQ nav_block) @AND
|
||||
electrical.bat_critical" deroute="Standby"/-->
|
||||
|
||||
<!-- RC lost && Block Time GT 300 -->
|
||||
<!--exception cond="!(IndexOfBlock('Standby') @GEQ nav_block) @AND
|
||||
(radio_control.status == RC_REALLY_LOST) @AND
|
||||
(NavBlockTime() @GT 300)" deroute="Standby"/-->
|
||||
</exceptions>
|
||||
|
||||
<!-- ................................................................ -->
|
||||
|
||||
<blocks>
|
||||
<block name="Wait GPS">
|
||||
|
||||
<!-- ## INIT ## -->
|
||||
|
||||
<block name="Wait GPS" strip_icon="gps.png">
|
||||
<set value="1" var="autopilot.kill_throttle"/>
|
||||
<while cond="!GpsFixValid()"/>
|
||||
</block>
|
||||
<block name="Geo init">
|
||||
|
||||
<block name="Geo init" strip_icon="googleearth.png">
|
||||
<while cond="LessThan(NavBlockTime(), 10)"/>
|
||||
<call_once fun="NavSetGroundReferenceHere()"/>
|
||||
</block>
|
||||
<block name="Holding point">
|
||||
|
||||
<block name="Holding point" strip_icon="mob.png">
|
||||
<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() @GT GetAltRef()+25" deroute="Standby"/>
|
||||
|
||||
<!-- ## AUTO TAKEOFF ## -->
|
||||
|
||||
<block name="Takeoff" strip_button="Takeoff (wp CLIMB)" strip_icon="takeoff.png">
|
||||
<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 fun="gvf_ellipse_wp(WP_ELLIPSE, gvf_ellipse_par.a, gvf_ellipse_par.b, gvf_ellipse_par.alpha)"/>
|
||||
</block>
|
||||
<block name="segment_loop">
|
||||
<call fun="gvf_segment_loop_wp1_wp2(WP_ELLIPSE, WP_STDBY, gvf_segment_par.d1, gvf_segment_par.d2)"/>
|
||||
</block>
|
||||
<block name="segment">
|
||||
<call fun="gvf_segment_wp1_wp2(WP_ELLIPSE, WP_STDBY)"/>
|
||||
</block>
|
||||
<block name="line">
|
||||
<call fun="gvf_line_wp_heading(WP_ELLIPSE, gvf_line_par.heading)"/>
|
||||
</block>
|
||||
<block name="Sinusoidal">
|
||||
<call fun="gvf_sin_wp_alpha(WP_ELLIPSE, gvf_sin_par.alpha, gvf_sin_par.w, gvf_sin_par.off, gvf_sin_par.A)"/>
|
||||
</block>
|
||||
<block name="Poly Survey">
|
||||
<call_once fun="gvf_nav_survey_polygon_setup(WP_S1, 4, angle_ps, 30, 30, 40, flight_altitude)"/>
|
||||
<call fun="gvf_nav_survey_polygon_run()"/>
|
||||
</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)"/>
|
||||
<set value="1" var="autopilot.launch"/> <!-- Just for simulation -->
|
||||
<go from="HOME" throttle="1.0" vmode="throttle" wp="CLIMB" pitch="25"/>
|
||||
</block>
|
||||
|
||||
<block group="land" name="Land Right AF-TD" strip_button="Land right (wp AF-TD)" strip_icon="land-right.png">
|
||||
<block name="Standby" key="s" strip_button="Standby" strip_icon="home.png">
|
||||
<set var="flight_altitude" value="GetAltRef() + 50"/>
|
||||
<call_once fun="NavVerticalAutoThrottleMode(0.0)"/>
|
||||
<call_once fun="NavVerticalAltitudeMode(flight_altitude, 0.0)"/>
|
||||
<call fun="nav_gvf_ellipse_wp(WP_STDBY, a_stb, b_stb, alpha_stb)"/>
|
||||
</block>
|
||||
|
||||
<!-- ## MISSION ## -->
|
||||
|
||||
<block name="Ellipse ELL" strip_icon="oval.png">
|
||||
<call_once fun="NavVerticalAutoThrottleMode(0.0)"/>
|
||||
<call_once fun="NavVerticalAltitudeMode(flight_altitude, 0.0)"/>
|
||||
<call fun="nav_gvf_ellipse_wp(WP_ELL, gvf_ellipse_par.a, gvf_ellipse_par.b, gvf_ellipse_par.alpha)"/>
|
||||
</block>
|
||||
|
||||
<block name="Segment P0-P1" strip_icon="line.png">
|
||||
<call_once fun="NavVerticalAutoThrottleMode(0.0)"/>
|
||||
<call_once fun="NavVerticalAltitudeMode(flight_altitude, 0.0)"/>
|
||||
<call fun="nav_gvf_segment_wp1_wp2(WP_P0, WP_P1)"/>
|
||||
<deroute block="Standby"/>
|
||||
</block>
|
||||
|
||||
<block name="Segment Loop P0-P1" strip_icon="line.png">
|
||||
<call_once fun="NavVerticalAutoThrottleMode(0.0)"/>
|
||||
<call_once fun="NavVerticalAltitudeMode(flight_altitude, 0.0)"/>
|
||||
<call fun="nav_gvf_segment_loop_wp1_wp2(WP_P0, WP_P1, gvf_segment_par.d1, gvf_segment_par.d2)"/>
|
||||
</block>
|
||||
|
||||
<block name="Line P0-heading" strip_icon="line.png">
|
||||
<call_once fun="NavVerticalAutoThrottleMode(0.0)"/>
|
||||
<call_once fun="NavVerticalAltitudeMode(flight_altitude, 0.0)"/>
|
||||
<call fun="nav_gvf_line_wp_heading(WP_P0, gvf_line_par.heading)"/>
|
||||
</block>
|
||||
|
||||
<block name="Sinusoidal P0" strip_icon="eight.png">
|
||||
<call_once fun="NavVerticalAutoThrottleMode(0.0)"/>
|
||||
<call_once fun="NavVerticalAltitudeMode(flight_altitude, 0.0)"/>
|
||||
<call fun="nav_gvf_sin_wp_alpha(WP_P0, gvf_sin_par.alpha, gvf_sin_par.w, gvf_sin_par.off, gvf_sin_par.A)"/>
|
||||
</block>
|
||||
|
||||
<block name="Poly Survey S" strip_icon="survey.png">
|
||||
<call_once fun="NavVerticalAutoThrottleMode(0.0)"/>
|
||||
<call_once fun="NavVerticalAltitudeMode(flight_altitude, 0.0)"/>
|
||||
<call_once fun="nav_gvf_survey_polygon_setup(WP_S1, 4, angle_ps, 30, 30, 40, flight_altitude)"/>
|
||||
<call fun="nav_gvf_survey_polygon_run()"/>
|
||||
</block>
|
||||
|
||||
<block name="2D Trefoil ELL" strip_icon="eight.png">
|
||||
<call_once fun="NavVerticalAutoThrottleMode(0.0)"/>
|
||||
<call_once fun="NavVerticalAltitudeMode(flight_altitude, 0.0)"/>
|
||||
<call fun="nav_gvf_parametric_2D_trefoil_wp(WP_ELL, gvf_parametric_2d_trefoil_par.w1, gvf_parametric_2d_trefoil_par.w2, gvf_parametric_2d_trefoil_par.ratio, gvf_parametric_2d_trefoil_par.r, gvf_parametric_2d_trefoil_par.alpha)"/>
|
||||
</block>
|
||||
|
||||
<block name="3D Lissajous ELL" strip_icon="eight.png">
|
||||
<call fun="nav_gvf_parametric_3D_lissajous_wp_center(WP_ELL, 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="3D Ellipse ELL" strip_icon="eight.png">
|
||||
<call fun="nav_gvf_parametric_3D_ellipse_wp(WP_ELL, gvf_parametric_3d_ellipse_par.r, gvf_parametric_3d_ellipse_par.zl, gvf_parametric_3d_ellipse_par.zh, gvf_parametric_3d_ellipse_par.alpha)"/>
|
||||
</block>
|
||||
|
||||
<block name="3D Ellipse ELL-delta" strip_icon="eight.png">
|
||||
<call fun="nav_gvf_parametric_3D_ellipse_wp_delta(WP_ELL, gvf_parametric_3d_ellipse_par.r, flight_altitude-ground_alt, ell_delta,gvf_parametric_3d_ellipse_par.alpha)"/>
|
||||
</block>
|
||||
|
||||
<block name="Ellipse IK ELL" strip_icon="oval.png">
|
||||
<call_once fun="NavVerticalAutoThrottleMode(0.0)"/>
|
||||
<call_once fun="NavVerticalAltitudeMode(flight_altitude, 0.0)"/>
|
||||
<call fun="nav_gvf_ik_ellipse_wp(WP_ELL, gvf_ik_ellipse_par.a, gvf_ik_ellipse_par.b, gvf_ik_ellipse_par.alpha)"/>
|
||||
</block>
|
||||
|
||||
<block name="Segment IK P2-P3" strip_icon="line.png">
|
||||
<call_once fun="NavVerticalAutoThrottleMode(0.0)"/>
|
||||
<call_once fun="NavVerticalAltitudeMode(flight_altitude, 0.0)"/>
|
||||
<call fun="nav_gvf_ik_segment_wp1_wp2(WP_P2, WP_P3)"/>
|
||||
<deroute block="Standby"/>
|
||||
</block>
|
||||
|
||||
<!-- ## AUTO LANDING ## -->
|
||||
|
||||
<!-- Not implemented with GVF yet! -->
|
||||
|
||||
<block name="Land Right AF-TD" group="land" 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">
|
||||
|
||||
<block name="Land Left AF-TD" group="land" 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() @GT 0.5" wp="_BASELEG"/>
|
||||
<circle radius="nav_radius" until="And(NavQdrCloseTo(DegOfRad(baseleg_out_qdr)-(nav_radius/fabs(nav_radius))*10), 10 @GT fabs(GetPosAlt() - WaypointAlt(WP__BASELEG)))" wp="_BASELEG"/>
|
||||
</block>
|
||||
|
||||
<block name="final">
|
||||
<exception cond="GetAltRef() + 10 @GT 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>
|
||||
|
||||
@@ -5,26 +5,32 @@
|
||||
|
||||
<waypoints>
|
||||
<waypoint name="HOME" x="0.0" y="0.0"/>
|
||||
<waypoint name="STDBY" x="0.1" y="0.8"/>
|
||||
<waypoint name="S1_START" x="0.0" y="1.0"/>
|
||||
<waypoint name="S1_END" x="0.0" y="5.0"/>
|
||||
<waypoint name="S2_START" x="2.0" y="1.0"/>
|
||||
<waypoint name="S2_END" x="2.0" y="5.0"/>
|
||||
<waypoint name="TD" x="-1.0" y="1.5"/>
|
||||
<!--waypoint name="STDBY" x="0.0" y="0.0"/-->
|
||||
<waypoint name="S1_START" lat="37.2972243" lon="-3.6831055"/>
|
||||
<waypoint name="S1_END" lat="37.2974372" lon="-3.6825632"/>
|
||||
<!--waypoint name="TD" x="0.0" y="0.0"/-->
|
||||
</waypoints>
|
||||
|
||||
<variables>
|
||||
<variable init="0.1" var="fp_throttle"/>
|
||||
<variable var="desired_heading" init="0.0f" type="float" min="0." max="10." step="0.1"/>
|
||||
<variable var="nominal_alt" init="0.5" type="float" min="0." max="10." step="0.1"/>
|
||||
</variables>
|
||||
|
||||
<modules>
|
||||
<module name="gvf_module"/>
|
||||
<!-- module name="gvf_parametric"/--> <!-- Not implemented yet -->
|
||||
<module name="gvf_classic">
|
||||
<define name="GVF_OCAML_GCS" value="FALSE"/>
|
||||
|
||||
<define name="GVF_ELLIPSE_A" value="8"/>
|
||||
<define name="GVF_ELLIPSE_B" value="8"/>
|
||||
<define name="GVF_LINE_KE" value="15"/>
|
||||
</module>
|
||||
|
||||
<!--module name="gvf_parametric"/--> <!-- Not implemented yet -->
|
||||
</modules>
|
||||
|
||||
<blocks>
|
||||
|
||||
<!-- ## INIT ## -->
|
||||
|
||||
<block name="FPInit">
|
||||
<call_once fun="NavKillThrottle()"/>
|
||||
<while cond="!GpsFixValid()"/>
|
||||
@@ -41,48 +47,55 @@
|
||||
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
|
||||
</block>
|
||||
|
||||
<!-- ## AUTO TAKEOFF ## -->
|
||||
|
||||
<block name="Take off" strip_button="Takeoff" strip_icon="takeoff.png" group="home">
|
||||
<call_once fun="NavVerticalAltitudeMode(nominal_alt, 0.0)"/>
|
||||
</block>
|
||||
|
||||
<block name="Standby" strip_button="Standby" strip_icon="home.png" group="home">
|
||||
<stay wp="STDBY"/>
|
||||
<stay wp="HOME"/>
|
||||
</block>
|
||||
|
||||
<block name="GVF Segment 1">
|
||||
<call fun="gvf_segment_loop_wp1_wp2(WP_S1_START, WP_S1_END, gvf_segment_par.d1, gvf_segment_par.d2)"/>
|
||||
<!-- ## MISSION ## -->
|
||||
|
||||
<block name="GVF Line STBY_S1" strip_icon="line.png">
|
||||
<call_once fun="NavVerticalAltitudeMode(nominal_alt, 0.0)"/>
|
||||
<call fun="nav_gvf_line_wp1_wp2(WP_HOME, WP_S1_START)"/>
|
||||
</block>
|
||||
|
||||
<block name="GVF Segment S1" strip_icon="line.png">
|
||||
<call_once fun="NavVerticalAltitudeMode(nominal_alt, 0.0)"/>
|
||||
<call fun="nav_gvf_segment_loop_wp1_wp2(WP_S1_START, WP_S1_END, gvf_segment_par.d1, gvf_segment_par.d2)"/>
|
||||
</block>
|
||||
|
||||
<block name="GVF Segment 2">
|
||||
<call fun="gvf_segment_loop_wp1_wp2(WP_S2_START, WP_S2_END, gvf_segment_par.d1, gvf_segment_par.d2)"/>
|
||||
<block name="GVF Ellipse HOME">
|
||||
<call_once fun="NavVerticalAltitudeMode(nominal_alt, 0.0)"/>
|
||||
<call fun="nav_gvf_ellipse_wp(WP_HOME, gvf_ellipse_par.a, gvf_ellipse_par.b, gvf_ellipse_par.alpha)"/>
|
||||
</block>
|
||||
|
||||
<block name="GVF 25m Circle">
|
||||
<call fun="gvf_ellipse_wp(WP_TD, 25, 25, gvf_ellipse_par.alpha)"/>
|
||||
</block>
|
||||
|
||||
<!-- Function found in ./sw/airborne/modules/guidance/gfv/gvf.h -->
|
||||
<block name="GVF Ellipse">
|
||||
<call fun="gvf_ellipse_wp(WP_TD, gvf_ellipse_par.a, gvf_ellipse_par.b, gvf_ellipse_par.alpha)"/>
|
||||
</block>
|
||||
<!-- ## AUTO LANDING ## -->
|
||||
|
||||
<block name="Land here" key="l" strip_button="Land Here" strip_icon="land-right.png">
|
||||
<call_once fun="NavSetWaypointHere(WP_TD)"/>
|
||||
<call_once fun="NavSetWaypointHere(WP_HOME)"/>
|
||||
</block>
|
||||
|
||||
<block name="Land">
|
||||
<go wp="TD"/>
|
||||
<go wp="HOME"/>
|
||||
</block>
|
||||
|
||||
<block name="Land Target" strip_button="Land Target" group="target">
|
||||
<exception cond="!nav_is_in_flight()" deroute="Kill Engine"/>
|
||||
<stay climb="nav.descend_vspeed" vmode="climb" wp="TD"/>
|
||||
<stay climb="nav.descend_vspeed" vmode="climb" wp="HOME"/>
|
||||
</block>
|
||||
|
||||
<block name="Kill Engine" key="k">
|
||||
<call_once fun="NavKillThrottle()"/>
|
||||
<while cond="1"/>
|
||||
</block>
|
||||
|
||||
<!-- ########################### -->
|
||||
|
||||
</blocks>
|
||||
|
||||
</flight_plan>
|
||||
|
||||
@@ -28,7 +28,7 @@
|
||||
</settings>
|
||||
|
||||
<dep>
|
||||
<depends>gvf_module</depends>
|
||||
<depends>gvf_classic</depends>
|
||||
</dep>
|
||||
|
||||
<header>
|
||||
@@ -45,4 +45,3 @@
|
||||
</makefile>
|
||||
|
||||
</module>
|
||||
|
||||
|
||||
@@ -30,7 +30,7 @@
|
||||
</settings>
|
||||
|
||||
<dep>
|
||||
<depends>@navigation</depends>
|
||||
<depends>@navigation,gvf_common</depends>
|
||||
<provides>guidance,commands</provides>
|
||||
</dep>
|
||||
|
||||
|
||||
@@ -1,9 +1,10 @@
|
||||
<!DOCTYPE module SYSTEM "module.dtd">
|
||||
|
||||
<module name="gvf_module" dir="guidance/gvf">
|
||||
<module name="gvf_classic" dir="guidance/gvf">
|
||||
<doc>
|
||||
<description>Guidance algorithm for tracking smooth trajectories. The algorithm is based on the idea of stearing the vehicle to a vector field that smoothly converges to the desired trajectory.
|
||||
For more details we refer to https://wiki.paparazziuav.org/wiki/Module/guidance_vector_field .
|
||||
<description>
|
||||
Guidance algorithm for tracking smooth trajectories. The algorithm is based on the idea of stearing the vehicle to a vector field that smoothly converges to the desired trajectory.
|
||||
For more details we refer to https://wiki.paparazziuav.org/wiki/Module/guidance_vector_field .
|
||||
</description>
|
||||
<section name="Ellipse" prefix="GVF_ELLIPSE_">
|
||||
<define name="KE" value="1" description="Gain for the aggresivity of the gvf"/>
|
||||
@@ -37,21 +38,21 @@ For more details we refer to https://wiki.paparazziuav.org/wiki/Module/guidance_
|
||||
<dl_setting MAX="20" MIN="0.0" STEP="0.2" VAR="gvf_control.speed" shortname = "(ROTORCRAFT) speed"/>
|
||||
</dl_settings>
|
||||
<dl_settings NAME="Ellipse">
|
||||
<dl_setting MAX="5" MIN="0.0" STEP="0.01" VAR="gvf_ellipse_par.ke" shortname="ell_ke" param="GVF_ELLIPSE_KE"/>
|
||||
<dl_setting MAX="50" MIN="0.0" STEP="0.01" VAR="gvf_ellipse_par.ke" shortname="ell_ke" param="GVF_ELLIPSE_KE"/>
|
||||
<dl_setting MAX="5" MIN="0.0" STEP="0.01" VAR="gvf_ellipse_par.kn" shortname="ell_kn" param="GVF_ELLIPSE_KN"/>
|
||||
<dl_setting MAX="150" MIN="0.0" STEP="5" VAR="gvf_ellipse_par.a" shortname="ell_a" param="GVF_ELLIPSE_A"/>
|
||||
<dl_setting MAX="150" MIN="0.0" STEP="5" VAR="gvf_ellipse_par.b" shortname="ell_b" param="GVF_ELLIPSE_B"/>
|
||||
<dl_setting MAX="90" MIN="-90" STEP="1" VAR="gvf_ellipse_par.alpha" shortname="ell_alpha" param="GVF_ELLIPSE_ALPHA"/>
|
||||
</dl_settings>
|
||||
<dl_settings NAME="Line">
|
||||
<dl_setting MAX="5" MIN="0.0" STEP="0.01" VAR="gvf_line_par.ke" shortname="line_ke" param="GVF_LINE_KE"/>
|
||||
<dl_settings NAME="Line">
|
||||
<dl_setting MAX="50" MIN="0.0" STEP="0.01" VAR="gvf_line_par.ke" shortname="line_ke" param="GVF_LINE_KE"/>
|
||||
<dl_setting MAX="5" MIN="0.0" STEP="0.01" VAR="gvf_line_par.kn" shortname="line_kn" param="GVF_LINE_KN"/>
|
||||
<dl_setting MAX="180" MIN="-180" STEP="1" VAR="gvf_line_par.heading" shortname="line_heading" param="GVF_LINE_HEADING"/>
|
||||
<dl_setting MAX="100" MIN="0" STEP="1" VAR="gvf_segment_par.d1" shortname="d1_seg" param="GVF_SEGMENT_D1"/>
|
||||
<dl_setting MAX="100" MIN="0" STEP="1" VAR="gvf_segment_par.d2" shortname="d2_seg" param="GVF_SEGMENT_D2"/>
|
||||
</dl_settings>
|
||||
<dl_settings NAME="Sine">
|
||||
<dl_setting MAX="5" MIN="0.0" STEP="0.01" VAR="gvf_sin_par.ke" shortname="sin_ke" param="GVF_SIN_KE"/>
|
||||
<dl_setting MAX="50" MIN="0.0" STEP="0.01" VAR="gvf_sin_par.ke" shortname="sin_ke" param="GVF_SIN_KE"/>
|
||||
<dl_setting MAX="5" MIN="0.0" STEP="0.01" VAR="gvf_sin_par.kn" shortname="sin_kn" param="GVF_SIN_KN"/>
|
||||
<dl_setting MAX="180" MIN="-180" STEP="1" VAR="gvf_sin_par.alpha" shortname="sin_alpha" param="GVF_SIN_ALPHA"/>
|
||||
<dl_setting MAX="0.01" MIN="0" STEP="0.0001" VAR="gvf_sin_par.w" shortname="sin_w" param="GVF_SIN_W"/>
|
||||
@@ -68,10 +69,9 @@ For more details we refer to https://wiki.paparazziuav.org/wiki/Module/guidance_
|
||||
|
||||
<header>
|
||||
<file name="gvf.h"/>
|
||||
<file name="gvf_low_level_control.h"/>
|
||||
<file name="trajectories/gvf_line.h"/>
|
||||
<file name="trajectories/gvf_sin.h"/>
|
||||
<file name="trajectories/gvf_ellipse.h"/>
|
||||
<file name="nav/nav_line.h"/>
|
||||
<file name="nav/nav_sin.h"/>
|
||||
<file name="nav/nav_ellipse.h"/>
|
||||
<file name="nav/nav_survey_polygon_gvf.h"/>
|
||||
</header>
|
||||
|
||||
@@ -79,28 +79,24 @@ For more details we refer to https://wiki.paparazziuav.org/wiki/Module/guidance_
|
||||
|
||||
<makefile firmware="fixedwing">
|
||||
<file name="gvf.c"/>
|
||||
<file name="gvf_low_level_control.c"/>
|
||||
<file name="trajectories/gvf_line.c"/>
|
||||
<file name="trajectories/gvf_sin.c"/>
|
||||
<file name="trajectories/gvf_ellipse.c"/>
|
||||
<file name="nav/nav_line.c"/>
|
||||
<file name="nav/nav_sin.c"/>
|
||||
<file name="nav/nav_ellipse.c"/>
|
||||
<file name="nav/nav_survey_polygon_gvf.c"/>
|
||||
</makefile>
|
||||
|
||||
<makefile firmware="rotorcraft">
|
||||
<file name="gvf.c"/>
|
||||
<file name="gvf_low_level_control.c"/>
|
||||
<file name="trajectories/gvf_line.c"/>
|
||||
<file name="trajectories/gvf_sin.c"/>
|
||||
<file name="trajectories/gvf_ellipse.c"/>
|
||||
<file name="nav/nav_line.c"/>
|
||||
<file name="nav/nav_sin.c"/>
|
||||
<file name="nav/nav_ellipse.c"/>
|
||||
</makefile>
|
||||
|
||||
<makefile firmware="rover">
|
||||
<file name="gvf.c"/>
|
||||
<file name="gvf_low_level_control.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"/-->
|
||||
<file name="nav/nav_line.c"/>
|
||||
<file name="nav/nav_sin.c"/>
|
||||
<file name="nav/nav_ellipse.c"/>
|
||||
</makefile>
|
||||
|
||||
</module>
|
||||
@@ -3,22 +3,36 @@
|
||||
<module name="gvf_common" dir="guidance/">
|
||||
<doc>
|
||||
<description>
|
||||
Common file to allow both gvf to work together.
|
||||
Still requires at least one module providing the actual gvf implementation
|
||||
A common module that enables all GVF variants to operate within a unified framework.
|
||||
It also provides a shared low-level control system.
|
||||
However, at least one additional module is required to implement the actual GVF functionality.
|
||||
</description>
|
||||
</doc>
|
||||
|
||||
<header>
|
||||
<file name="gvf_common.h"/>
|
||||
<file name="trajectories/gvf_traj.h"/>
|
||||
<file name="trajectories/gvf_param_traj.h"/>
|
||||
</header>
|
||||
|
||||
<makefile firmware="fixedwing">
|
||||
|
||||
<makefile firmware="fixedwing">
|
||||
<file name="gvf_common.c"/>
|
||||
</makefile>
|
||||
|
||||
<makefile firmware="rover">
|
||||
<file name="trajectories/gvf_traj.c"/>
|
||||
<file name="trajectories/gvf_param_traj.c"/>
|
||||
</makefile>
|
||||
|
||||
<makefile firmware="rotorcraft">
|
||||
<define name="ROTORCRAFT_BASE_SEND_TRAJECTORY" value="FALSE"/>
|
||||
<file name="gvf_common.c"/>
|
||||
</makefile>
|
||||
<file name="trajectories/gvf_traj.c"/>
|
||||
<file name="trajectories/gvf_param_traj.c"/>
|
||||
</makefile>
|
||||
|
||||
<makefile firmware="rover">
|
||||
<define name="ROVER_BASE_SEND_TRAJECTORY" value="FALSE"/>
|
||||
<file name="gvf_common.c"/>
|
||||
<file name="trajectories/gvf_traj.c"/>
|
||||
<file name="trajectories/gvf_param_traj.c"/>
|
||||
</makefile>
|
||||
|
||||
</module>
|
||||
|
||||
|
||||
@@ -0,0 +1,99 @@
|
||||
<!DOCTYPE module SYSTEM "module.dtd">
|
||||
|
||||
<module name="gvf_ik" dir="guidance/gvf_ik">
|
||||
<doc>
|
||||
<description>
|
||||
Guidance algorithm based on inverse kinematics for path following using guiding vector fields.
|
||||
The path is defined implicitly by level sets, enabling precise transient control and feed-forward motion shaping.
|
||||
For more details, see https://arxiv.org/pdf/2502.17313 .
|
||||
</description>
|
||||
<section name="IK" prefix="GVF_IK_">
|
||||
<define name="GAMMA_AMPLITUDE" value="0" description="Error feedforward amplitude"/>
|
||||
<define name="GAMMA_OMEGA" value="0" description="Error feedforward angular velocity" unit="rad/s"/>
|
||||
</section>
|
||||
<section name="Ellipse" prefix="GVF_IK_ELLIPSE_">
|
||||
<define name="KE" value="1" description="Gain for the aggresivity of the gvf"/>
|
||||
<define name="KN" value="1" description="Gain for the alignment of the vehicle with the gvf"/>
|
||||
<define name="A" value="80" description="Horizontal axis length of the ellipse" unit="m"/>
|
||||
<define name="B" value="80" description="Vertical axis length of the ellipse" unit="m"/>
|
||||
<define name="ALPHA" value="0" description="Rotation of the horizontal axis" unit="deg"/>
|
||||
</section>
|
||||
<section name="Line" prefix="GVF_IK_LINE_">
|
||||
<define name="KE" value="1" description="Gain for the aggresivity of the gvf"/>
|
||||
<define name="KN" value="1" description="Gain for the alignment of the vehicle with the gvf"/>
|
||||
<define name="HEADING" value="0" description="Desired heading for the line (0 is North, 90 is East)" unit="deg"/>
|
||||
<define name="D1" value="0" description="Extra distance (w.r.t. the 1st point) to be travelled before turning around for the segment_loop" unit="m"/>
|
||||
<define name="D2" value="0" description="Extra distance (w.r.t. the 2nd point) to be travelled before turning around for the segment_loop" unit="m"/>
|
||||
</section>
|
||||
<section name="Sinusoidal" prefix="GVF_IK_SIN_">
|
||||
<define name="KE" value="1" description="Gain for the aggresivity of the gvf"/>
|
||||
<define name="KN" value="1" description="Gain for the alignment of the vehicle with the gvf"/>
|
||||
<define name="ALPHA" value="0" description="Desired heading for the line (0 is East, 90 is North)" unit="deg"/>
|
||||
<define name="W" value="0" description="Frequency for the sinusoidal y=Asin(Wx + OFF)" unit="rad/m"/>
|
||||
<define name="OFF" value="0" description="Offset for the sinusoidal y=Asin(Wx + OFF)" unit="rad"/>
|
||||
<define name="A" value="0" description="Amplitude for the sinusoidal y=Asin(Wx + OFF)" unit="m"/>
|
||||
</section>
|
||||
</doc>
|
||||
|
||||
<settings name="GVF_IK">
|
||||
<dl_settings>
|
||||
<dl_settings NAME="GVF_IK">
|
||||
<dl_settings NAME="Control">
|
||||
<dl_setting MAX="1" MIN="-1" STEP="2" VAR="gvf_ik_control.s" shortname = "direction"/>
|
||||
<dl_setting MAX="100" MIN="0" STEP="0.01" VAR="gvf_ik_control.gamma_amplitude" shortname = "gamma_amplitude (phi)" param="GVF_IK_GAMMA_AMPLITUDE"/>
|
||||
<dl_setting MAX="10" MIN="0" STEP="0.01" VAR="gvf_ik_control.gamma_omega" shortname = "gamma_omega (rad/s)" param="GVF_IK_GAMMA_OMEGA"/>
|
||||
</dl_settings>
|
||||
<dl_settings NAME="Ellipse">
|
||||
<dl_setting MAX="5" MIN="0.0" STEP="0.01" VAR="gvf_ik_ellipse_par.ke" shortname="ell_ke" param="GVF_IK_ELLIPSE_KE"/>
|
||||
<dl_setting MAX="5" MIN="0.0" STEP="0.01" VAR="gvf_ik_ellipse_par.kn" shortname="ell_kn" param="GVF_IK_ELLIPSE_KN"/>
|
||||
<dl_setting MAX="150" MIN="0.0" STEP="10" VAR="gvf_ik_ellipse_par.a" shortname="ell_a" param="GVF_IK_ELLIPSE_A"/>
|
||||
<dl_setting MAX="150" MIN="0.0" STEP="10" VAR="gvf_ik_ellipse_par.b" shortname="ell_b" param="GVF_IK_ELLIPSE_B"/>
|
||||
<dl_setting MAX="90" MIN="-90" STEP="1" VAR="gvf_ik_ellipse_par.alpha" shortname="ell_alpha" param="GVF_IK_ELLIPSE_ALPHA"/>
|
||||
</dl_settings>
|
||||
<dl_settings NAME="Line">
|
||||
<dl_setting MAX="100" MIN="0.0" STEP="0.01" VAR="gvf_ik_line_par.ke" shortname="line_ke" param="GVF_IK_LINE_KE"/>
|
||||
<dl_setting MAX="100" MIN="0.0" STEP="0.01" VAR="gvf_ik_line_par.kn" shortname="line_kn" param="GVF_IK_LINE_KN"/>
|
||||
<dl_setting MAX="180" MIN="-180" STEP="1" VAR="gvf_ik_line_par.heading" shortname="line_heading" param="GVF_IK_LINE_HEADING"/>
|
||||
<dl_setting MAX="100" MIN="0" STEP="1" VAR="gvf_ik_segment_par.d1" shortname="d1_seg" param="GVF_IK_SEGMENT_D1"/>
|
||||
<dl_setting MAX="100" MIN="0" STEP="1" VAR="gvf_ik_segment_par.d2" shortname="d2_seg" param="GVF_IK_SEGMENT_D2"/>
|
||||
</dl_settings>
|
||||
<dl_settings NAME="Sine">
|
||||
<dl_setting MAX="5" MIN="0.0" STEP="0.01" VAR="gvf_ik_sin_par.ke" shortname="sin_ke" param="GVF_IK_SIN_KE"/>
|
||||
<dl_setting MAX="5" MIN="0.0" STEP="0.01" VAR="gvf_ik_sin_par.kn" shortname="sin_kn" param="GVF_IK_SIN_KN"/>
|
||||
<dl_setting MAX="180" MIN="-180" STEP="1" VAR="gvf_ik_sin_par.alpha" shortname="sin_alpha" param="GVF_IK_SIN_ALPHA"/>
|
||||
<dl_setting MAX="0.01" MIN="0" STEP="0.0001" VAR="gvf_ik_sin_par.w" shortname="sin_w" param="GVF_IK_SIN_W"/>
|
||||
<dl_setting MAX="6.2" MIN="0" STEP="0.002" VAR="gvf_ik_sin_par.off" shortname="sin_off" param="GVF_IK_SIN_OFF"/>
|
||||
<dl_setting MAX="100" MIN="0" STEP="1" VAR="gvf_ik_sin_par.A" shortname="sin_amplitude" param="GVF_IK_SIN_A"/>
|
||||
</dl_settings>
|
||||
</dl_settings>
|
||||
</dl_settings>
|
||||
</settings>
|
||||
|
||||
<dep>
|
||||
<depends>gvf_common</depends>
|
||||
</dep>
|
||||
|
||||
<header>
|
||||
<file name="gvf_ik.h"/>
|
||||
<file name="nav/nav_line.h"/>
|
||||
<file name="nav/nav_sin.h"/>
|
||||
<file name="nav/nav_ellipse.h"/>
|
||||
</header>
|
||||
|
||||
<init fun = "gvf_ik_init()"/>
|
||||
|
||||
<makefile firmware="fixedwing">
|
||||
<file name="gvf_ik.c"/>
|
||||
<file name="nav/nav_line.c"/>
|
||||
<file name="nav/nav_sin.c"/>
|
||||
<file name="nav/nav_ellipse.c"/>
|
||||
</makefile>
|
||||
|
||||
<makefile firmware="rover">
|
||||
<file name="gvf_ik.c"/>
|
||||
<file name="nav/nav_line.c"/>
|
||||
<file name="nav/nav_sin.c"/>
|
||||
<file name="nav/nav_ellipse.c"/>
|
||||
</makefile>
|
||||
|
||||
</module>
|
||||
@@ -2,7 +2,8 @@
|
||||
|
||||
<module name="gvf_parametric" dir="guidance/gvf_parametric">
|
||||
<doc>
|
||||
<description>Guidance algorithm for tracking 2D and 3D smooth trajectories that are defined by a parametric equation.
|
||||
<description>
|
||||
Guidance algorithm for tracking 2D and 3D smooth trajectories that are defined by a parametric equation.
|
||||
|
||||
For more details we refer to https://wiki.paparazziuav.org/wiki/Module/guidance_vector_field .
|
||||
This algorithm needs the Eigen library, be sure you have it installed, e.g., git submodule update --init --recursive sw/ext/eigen
|
||||
@@ -14,8 +15,8 @@
|
||||
<dl_settings NAME="GVF_PARAMETRIC">
|
||||
<dl_settings NAME="Control">
|
||||
<dl_setting MAX="1" MIN="-1" STEP="2" VAR="gvf_parametric_control.s" shortname = "direction"/>
|
||||
<dl_setting MAX="1.25" MIN="0.75" STEP="0.05" VAR="gvf_parametric_control.k_roll" shortname="k_roll" param="GVF_PARAMETRIC_CONTROL_KROLL"/>
|
||||
<dl_setting MAX="1.25" MIN="0.75" STEP="0.05" VAR="gvf_parametric_control.k_climb" shortname="k_climb" param="GVF_PARAMETRIC_CONTROL_KCLIMB"/>
|
||||
<dl_setting MAX="1.25" MIN="0.75" STEP="0.05" VAR="gvf_c_params.k_roll" shortname="k_roll" param="GVF_PARAMETRIC_CONTROL_KROLL"/>
|
||||
<dl_setting MAX="1.25" MIN="0.75" STEP="0.05" VAR="gvf_c_params.k_climb" shortname="k_climb" param="GVF_PARAMETRIC_CONTROL_KCLIMB"/>
|
||||
<dl_setting MAX="10" MIN="0.01" STEP="0.05" VAR="gvf_parametric_control.L" shortname="L" param="GVF_PARAMETRIC_CONTROL_L"/>
|
||||
<dl_setting MAX="1" MIN="0.001" STEP="0.005" VAR="gvf_parametric_control.beta" shortname="beta" param="GVF_PARAMETRIC_CONTROL_BETA"/>
|
||||
<dl_setting MAX="1.5" MIN="0.5" STEP="0.01" VAR="gvf_parametric_control.k_psi" shortname="k_psi" param="GVF_PARAMETRIC_CONTROL_KPSI"/>
|
||||
@@ -43,8 +44,8 @@
|
||||
<dl_setting MAX="180" MIN="-180" STEP="1" VAR="gvf_parametric_3d_ellipse_par.alpha" shortname="3d_ell_alpha" param="GVF_PARAMETRIC_3D_ELLIPSE_ALPHA"/>
|
||||
</dl_settings>
|
||||
<dl_settings NAME="3D_lissajous">
|
||||
<dl_setting MAX="0.01" MIN="0.0" STEP="0.0005" VAR="gvf_parametric_3d_lissajous_par.kx" shortname="3d_lis_kx" param="GVF_PARAMETRIC_3D_LISSAJOUS_KX"/>
|
||||
<dl_setting MAX="0.01" MIN="0.0" STEP="0.0005" VAR="gvf_parametric_3d_lissajous_par.ky" shortname="3d_lis_ky" param="GVF_PARAMETRIC_3D_LISSAJOUS_KY"/>
|
||||
<dl_setting MAX="0.1" MIN="0.0" STEP="0.0005" VAR="gvf_parametric_3d_lissajous_par.kx" shortname="3d_lis_kx" param="GVF_PARAMETRIC_3D_LISSAJOUS_KX"/>
|
||||
<dl_setting MAX="0.1" MIN="0.0" STEP="0.0005" VAR="gvf_parametric_3d_lissajous_par.ky" shortname="3d_lis_ky" param="GVF_PARAMETRIC_3D_LISSAJOUS_KY"/>
|
||||
<dl_setting MAX="0.1" MIN="0.0" STEP="0.0005" VAR="gvf_parametric_3d_lissajous_par.kz" shortname="3d_lis_kz" param="GVF_PARAMETRIC_3D_LISSAJOUS_KZ"/>
|
||||
<dl_setting MAX="250" MIN="-250" STEP="5" VAR="gvf_parametric_3d_lissajous_par.cx" shortname="3d_lis_cx" param="GVF_PARAMETRIC_3D_LISSAJOUS_CX"/>
|
||||
<dl_setting MAX="250" MIN="-250" STEP="5" VAR="gvf_parametric_3d_lissajous_par.cy" shortname="3d_lis_cy" param="GVF_PARAMETRIC_3D_LISSAJOUS_CY"/>
|
||||
@@ -68,11 +69,10 @@
|
||||
|
||||
<header>
|
||||
<file name="gvf_parametric.h"/>
|
||||
<file name="gvf_parametric_low_level_control.h"/>
|
||||
<file name="trajectories/gvf_parametric_3d_ellipse.h"/>
|
||||
<file name="trajectories/gvf_parametric_3d_lissajous.h"/>
|
||||
<file name="trajectories/gvf_parametric_2d_trefoil.h"/>
|
||||
<file name="trajectories/gvf_parametric_2d_bezier_splines.h"/>
|
||||
<file name="nav/nav_parametric_3d_ellipse.h"/>
|
||||
<file name="nav/nav_parametric_3d_lissajous.h"/>
|
||||
<file name="nav/nav_parametric_2d_trefoil.h"/>
|
||||
<file name="nav/nav_parametric_2d_bezier_splines.h"/>
|
||||
</header>
|
||||
|
||||
<init fun = "gvf_parametric_init()"/>
|
||||
@@ -84,11 +84,10 @@
|
||||
<define name="EIGEN_NO_MALLOC"/>
|
||||
<define name="EIGEN_NO_AUTOMATIC_RESIZING"/>
|
||||
<file name="gvf_parametric.cpp"/>
|
||||
<file name="gvf_parametric_low_level_control.c"/>
|
||||
<file name="trajectories/gvf_parametric_3d_ellipse.c"/>
|
||||
<file name="trajectories/gvf_parametric_3d_lissajous.c"/>
|
||||
<file name="trajectories/gvf_parametric_2d_trefoil.c"/>
|
||||
<file name="trajectories/gvf_parametric_2d_bezier_splines.c"/>
|
||||
<file name="nav/nav_parametric_3d_ellipse.c"/>
|
||||
<file name="nav/nav_parametric_3d_lissajous.c"/>
|
||||
<file name="nav/nav_parametric_2d_trefoil.c"/>
|
||||
<file name="nav/nav_parametric_2d_bezier_splines.c"/>
|
||||
<flag name="LDFLAGS" value="lstdc++" />
|
||||
</makefile>
|
||||
|
||||
@@ -99,11 +98,10 @@
|
||||
<define name="EIGEN_NO_MALLOC"/>
|
||||
<define name="EIGEN_NO_AUTOMATIC_RESIZING"/>
|
||||
<file name="gvf_parametric.cpp"/>
|
||||
<file name="gvf_parametric_low_level_control.c"/>
|
||||
<file name="trajectories/gvf_parametric_3d_ellipse.c"/>
|
||||
<file name="trajectories/gvf_parametric_3d_lissajous.c"/>
|
||||
<file name="trajectories/gvf_parametric_2d_trefoil.c"/>
|
||||
<file name="trajectories/gvf_parametric_2d_bezier_splines.c"/>
|
||||
<file name="nav/nav_parametric_3d_ellipse.c"/>
|
||||
<file name="nav/nav_parametric_3d_lissajous.c"/>
|
||||
<file name="nav/nav_parametric_2d_trefoil.c"/>
|
||||
<file name="nav/nav_parametric_2d_bezier_splines.c"/>
|
||||
<flag name="LDFLAGS" value="lstdc++" />
|
||||
</makefile>
|
||||
|
||||
|
||||
@@ -0,0 +1,11 @@
|
||||
<!DOCTYPE radio SYSTEM "radio.dtd">
|
||||
|
||||
<radio name="Futaba T7C 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="2000" max="2000" average="0" reverse="1"/>
|
||||
<channel function="YAW" min="1000" neutral="1500" max="2000" average="0"/>
|
||||
<channel function="MODE" max="2000" neutral="1520" min="1000" average="1"/>
|
||||
<channel function="GAIN1" min="1000" neutral="1520" max="2000" average="0"/>
|
||||
<channel function="GAIN2" min="1000" neutral="1520" max="2000" average="0"/>
|
||||
</radio>
|
||||
@@ -0,0 +1,54 @@
|
||||
<?xml version="1.0"?>
|
||||
<!--
|
||||
(c) 2023 Jesus Bautista Villar <jesbauti20@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, 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="FrSky X20 + FrSky ACCESS RX6R receiver" data_min="900" data_max="2100" sync_min="9000" sync_max="18000" pulse_type="POSITIVE">
|
||||
<channel function="THROTTLE" min="991" neutral="1496" max="1993" average="0"/>
|
||||
<channel function="YAW" min="998" neutral="1501" max="2005" average="0"/>
|
||||
<channel function="PITCH" min="987" neutral="1505" max="2011" average="0"/>
|
||||
<channel function="ROLL" min="1001" neutral="1500" max="2003" average="0" reverse="1"/>
|
||||
<channel function="MODE" min="987" neutral="1500" max="2011" average="0"/>
|
||||
<channel function="GAIN1" min="987" neutral="1500" max="1998" average="0"/>
|
||||
<channel function="GAIN2" min="987" neutral="1500" max="2011" average="0"/>
|
||||
<channel function="CRUISE_MODE" min="987" neutral="1500" max="2011" average="0" reverse="1"/>
|
||||
</radio>
|
||||
@@ -0,0 +1,52 @@
|
||||
<?xml version="1.0"?>
|
||||
<!--
|
||||
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="1100" neutral="1520" max="1940" average="0"/>
|
||||
<channel function="PITCH" min="1100" neutral="1520" max="1940" average="0"/>
|
||||
<channel function="THROTTLE" min="1100" neutral="1520" max="1940" average="0"/>
|
||||
<channel function="YAW" min="1100" neutral="1500" max="1940" average="0"/>
|
||||
<channel function="MODE" min="1100" neutral="1520" max="1940" average="1" reverse="1"/>
|
||||
<channel function="CRUISE_MODE" min="1100" neutral="1520" max="1940" average="0" reverse="1"/>
|
||||
<channel function="GAIN1" min="1100" neutral="1520" max="1940" average="0"/>
|
||||
<!--channel function="GAIN2" min="1100" neutral="1520" max="1940" average="0"/-->
|
||||
</radio>
|
||||
@@ -0,0 +1,60 @@
|
||||
<?xml version="1.0"?>
|
||||
<!--
|
||||
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="1100" neutral="1100" max="1940" average="0"/>
|
||||
<channel function="YAW" min="1000" neutral="1500" max="2000" average="0"/>
|
||||
<channel function="MODE" max="2000" neutral="1520" min="1000" 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" max="2000" neutral="1520" min="1000" 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,31 @@
|
||||
<!DOCTYPE settings SYSTEM "../settings.dtd">
|
||||
|
||||
<!-- A conf to use for standard operation (no tuning) -->
|
||||
|
||||
<settings>
|
||||
<dl_settings>
|
||||
<dl_settings NAME="flight params">
|
||||
<dl_setting MAX="1000" MIN="0" STEP="5" VAR="flight_altitude" shortname="altitude"/>
|
||||
<dl_setting MAX="360" MIN="0" STEP="1" VAR="nav_course"/>
|
||||
<dl_setting MAX="10" MIN="-10" STEP="5" VAR="nav_shift" module="firmwares/fixedwing/nav" handler="IncreaseShift" shortname="inc. shift"/>
|
||||
<dl_setting MAX="0" MIN="0" STEP="1" VAR="autopilot.flight_time" shortname="flight time" module="autopilot" handler="ResetFlightTimeAndLaunch"/>
|
||||
<!-- <dl_setting MAX="200" MIN="-200" STEP="10" VAR="nav_radius" module="firmwares/fixedwing/nav" handler="SetNavRadius">
|
||||
<strip_button icon="circle-right.png" name="Circle right" value="1" group="circle"/>
|
||||
<strip_button icon="circle-left.png" name="Circle left" value="-1" group="circle"/>
|
||||
<key_press key="greater" value="1"/>
|
||||
<key_press key="less" value="-1"/>
|
||||
</dl_setting> -->
|
||||
</dl_settings>
|
||||
|
||||
<dl_settings NAME="mode">
|
||||
<dl_setting MAX="2" MIN="0" STEP="1" VAR="autopilot.mode" module="autopilot" values="MANUAL|AUTO1|AUTO2|HOME|NOGPS|FAILSAFE"/>
|
||||
<dl_setting MAX="1" MIN="0" STEP="1" VAR="autopilot.launch"/>
|
||||
<dl_setting MAX="1" MIN="0" STEP="1" VAR="autopilot.kill_throttle"/>
|
||||
</dl_settings>
|
||||
|
||||
<dl_settings NAME="mcu">
|
||||
<dl_setting var="mcu_reboot" shortname="reboot" min="0" step="1" max="3" values="NORMAL|POWEROFF|FAST|BOOTLOADER" handler="reboot" module="mcu" type="fun"/>
|
||||
</dl_settings>
|
||||
|
||||
</dl_settings>
|
||||
</settings>
|
||||
@@ -0,0 +1,108 @@
|
||||
<?xml version="1.0"?>
|
||||
<!DOCTYPE telemetry SYSTEM "../telemetry.dtd">
|
||||
<telemetry>
|
||||
|
||||
<!-- ## AP ## -->
|
||||
|
||||
<process name="Ap">
|
||||
<mode name="default">
|
||||
<message name="AUTOPILOT_VERSION" period="11.1"/>
|
||||
<message name="AIRSPEED" period="1"/>
|
||||
<message name="ALIVE" period="5"/>
|
||||
<message name="GPS" period="0.25"/>
|
||||
<message name="ESTIMATOR" period="0.5"/>
|
||||
<message name="ENERGY" period="1.1"/>
|
||||
<message name="WP_MOVED" period="1.0"/>
|
||||
<message name="DESIRED" period="1.05"/>
|
||||
<message name="CALIBRATION" period="2.1"/>
|
||||
<message name="NAVIGATION_REF" period="9."/>
|
||||
<message name="PPRZ_MODE" period="5."/>
|
||||
<message name="SETTINGS" period="5."/>
|
||||
<message name="STATE_FILTER_STATUS" period="5."/>
|
||||
<message name="DATALINK_REPORT" period="5.1"/>
|
||||
<message name="DL_VALUE" period="1.5"/>
|
||||
<message name="SURVEY" period="2.1"/>
|
||||
<message name="GPS_SOL" period="2.0"/>
|
||||
<message name="COMMANDS" period="5"/>
|
||||
<message name="FBW_STATUS" period="2"/>
|
||||
<message name="AIR_DATA" period="1.3"/>
|
||||
<message name="VECTORNAV_INFO" period="0.5"/>
|
||||
|
||||
<message name="ATTITUDE" period="0.5"/>
|
||||
<message name="NAVIGATION" period=".1"/>
|
||||
|
||||
<!-- GNC ..................................... -->
|
||||
<message name="GVF" period=".1"/>
|
||||
<message name="GVF_PARAMETRIC" period="3."/>
|
||||
<message name="DCF" period=".25"/>
|
||||
<!-- ......................................... -->
|
||||
</mode>
|
||||
|
||||
<mode name="minimal">
|
||||
<message name="ALIVE" period="5"/>
|
||||
<message name="ATTITUDE" period="4"/>
|
||||
<message name="GPS" period="1.05"/>
|
||||
<message name="ESTIMATOR" period="1.3"/>
|
||||
<message name="WP_MOVED" period="1.4"/>
|
||||
<message name="DESIRED" period="4.05"/>
|
||||
<message name="ENERGY" period="1.1"/>
|
||||
<message name="CALIBRATION" period="5.1"/>
|
||||
<message name="NAVIGATION_REF" period="9."/>
|
||||
<message name="NAVIGATION" period="3."/>
|
||||
<message name="PPRZ_MODE" period="5."/>
|
||||
<message name="STATE_FILTER_STATUS" period="5."/>
|
||||
<message name="DATALINK_REPORT" period="5.1"/>
|
||||
<message name="DL_VALUE" period="1.5"/>
|
||||
<message name="IMU_GYRO" period="10.1"/>
|
||||
<message name="SURVEY" period="2.1"/>
|
||||
<message name="GPS_SOL" period="5.0"/>
|
||||
|
||||
<!-- GNC ..................................... -->
|
||||
<message name="GVF" period=".1"/>
|
||||
<message name="GVF_PARAMETRIC" period="3."/>
|
||||
<message name="DCF" period=".25"/>
|
||||
<!-- ......................................... -->
|
||||
</mode>
|
||||
|
||||
<mode name="extremal">
|
||||
<message name="ALIVE" period="5"/>
|
||||
<message name="GPS" period="5.1"/>
|
||||
<message name="ESTIMATOR" period="5.3"/>
|
||||
<message name="ENERGY" period="10.1"/>
|
||||
<message name="DESIRED" period="10.2"/>
|
||||
<message name="NAVIGATION" period="5.4"/>
|
||||
<message name="PPRZ_MODE" period="5.5"/>
|
||||
<message name="STATE_FILTER_STATUS" period="7."/>
|
||||
<message name="DATALINK_REPORT" period="5.7"/>
|
||||
</mode>
|
||||
|
||||
<mode name="raw_sensors">
|
||||
<message name="DL_VALUE" period="0.5"/>
|
||||
<message name="ALIVE" period="2.1"/>
|
||||
<message name="BARO_RAW" period="0.5"/>
|
||||
<message name="IMU_ACCEL_RAW" period="0.1"/>
|
||||
<message name="IMU_GYRO_RAW" period="0.1"/>
|
||||
<message name="IMU_MAG_RAW" period="0.1"/>
|
||||
</mode>
|
||||
</process>
|
||||
|
||||
<!-- ## FBW (debugging without radio) ## -->
|
||||
|
||||
<process name="Fbw">
|
||||
<mode name="default">
|
||||
<message name="COMMANDS" period="5"/>
|
||||
<message name="FBW_STATUS" period="2"/>
|
||||
<message name="ACTUATORS_RAW" period="5"/> <!-- For trimming -->
|
||||
</mode>
|
||||
<mode name="debug">
|
||||
<message name="PPM" period="0.5"/>
|
||||
<message name="RC" period="0.5"/>
|
||||
<message name="COMMANDS" period="0.5"/>
|
||||
<message name="FBW_STATUS" period="1"/>
|
||||
<message name="ACTUATORS_RAW" period="5"/> <!-- For trimming -->
|
||||
</mode>
|
||||
</process>
|
||||
|
||||
<!-- ########################### -->
|
||||
|
||||
</telemetry>
|
||||
@@ -7,7 +7,7 @@
|
||||
telemetry="telemetry/GVF/gvf_default_fixedwing.xml"
|
||||
flight_plan="flight_plans/UCM/fixedwing_gvfMission.xml"
|
||||
settings="settings/fixedwing_basic.xml"
|
||||
settings_modules="modules/ahrs_float_dcm.xml modules/electrical.xml modules/gps.xml modules/gps_ublox.xml modules/guidance_full_pid_fw.xml modules/gvf_module.xml~GVF~ modules/gvf_parametric.xml~GVF_PARAMETRIC~ modules/imu_common.xml modules/nav_basic_fw.xml modules/stabilization_adaptive_fw.xml"
|
||||
settings_modules="modules/ahrs_float_dcm.xml modules/electrical.xml modules/gps.xml modules/gps_ublox.xml modules/guidance_full_pid_fw.xml modules/gvf_classic.xml~GVF~ modules/gvf_parametric.xml~GVF_PARAMETRIC~ modules/imu_common.xml modules/nav_basic_fw.xml modules/stabilization_adaptive_fw.xml"
|
||||
gui_color="blue"
|
||||
/>
|
||||
<aircraft
|
||||
@@ -18,7 +18,7 @@
|
||||
telemetry="telemetry/GVF/gvf_rover.xml"
|
||||
flight_plan="flight_plans/UCM/steering_rover_gvfMission.xml"
|
||||
settings=""
|
||||
settings_modules="modules/ahrs_float_dcm.xml modules/electrical.xml modules/gps.xml modules/gps_ublox.xml modules/gps_ubx_ucenter.xml modules/guidance_rover_steering.xml~SR Guidance~ modules/gvf_module.xml~GVF~ modules/gvf_parametric.xml~GVF_PARAMETRIC~ modules/imu_common.xml modules/nav_rover_base.xml"
|
||||
settings_modules="modules/ahrs_float_dcm.xml modules/electrical.xml modules/gps.xml modules/gps_ublox.xml modules/gps_ubx_ucenter.xml modules/guidance_rover_steering.xml~SR Guidance~ modules/gvf_classic.xml~GVF~ modules/gvf_parametric.xml~GVF_PARAMETRIC~ modules/imu_common.xml modules/nav_rover_base.xml"
|
||||
gui_color="blue"
|
||||
/>
|
||||
</conf>
|
||||
@@ -0,0 +1,57 @@
|
||||
<conf>
|
||||
<aircraft
|
||||
name="gvf_sonic1"
|
||||
ac_id="5"
|
||||
airframe="airframes/UGR/sonic_n1.xml"
|
||||
radio="radios/UGR/T16SZ_SBUS_sonic.xml"
|
||||
telemetry="telemetry/UGR/gvf_fixedwing.xml"
|
||||
flight_plan="flight_plans/UGR/fixedwing_clubalhambra_gvf.xml"
|
||||
settings="settings/UGR/fixedwing_basic.xml"
|
||||
settings_modules="modules/ahrs_float_dcm.xml modules/electrical.xml modules/gps.xml modules/gps_ublox.xml modules/gps_ubx_ucenter.xml modules/guidance_full_pid_fw.xml modules/gvf_classic.xml~GVF~ modules/gvf_ik.xml~GVF_IK~ modules/gvf_parametric.xml~GVF_PARAMETRIC~ modules/imu_common.xml modules/nav_basic_fw.xml modules/power_switch.xml modules/stabilization_adaptive_fw.xml"
|
||||
gui_color="#6200a000ea00"
|
||||
/>
|
||||
<aircraft
|
||||
name="gvf_sonic2"
|
||||
ac_id="6"
|
||||
airframe="airframes/UGR/sonic_n2.xml"
|
||||
radio="radios/T7C_SBUS.xml"
|
||||
telemetry="telemetry/UGR/gvf_fixedwing.xml"
|
||||
flight_plan="flight_plans/UGR/fixedwing_clubalhambra_gvf.xml"
|
||||
settings="settings/UGR/fixedwing_basic.xml"
|
||||
settings_modules="modules/ahrs_float_dcm.xml modules/electrical.xml modules/gps.xml modules/gps_ublox.xml modules/gps_ubx_ucenter.xml modules/guidance_full_pid_fw.xml modules/gvf_classic.xml~GVF~ modules/gvf_ik.xml~GVF_IK~ modules/gvf_parametric.xml~GVF_PARAMETRIC~ modules/imu_common.xml modules/nav_basic_fw.xml modules/power_switch.xml modules/stabilization_adaptive_fw.xml"
|
||||
gui_color="#ed0033003b00"
|
||||
/>
|
||||
<aircraft
|
||||
name="st_rover_n1"
|
||||
ac_id="1"
|
||||
airframe="airframes/UGR/steering_rover_n1.xml"
|
||||
radio="radios/UGR/FrSkyX20.xml"
|
||||
telemetry="telemetry/GVF/gvf_rover.xml"
|
||||
flight_plan="flight_plans/UGR/steering_rover_clubalhambra.xml"
|
||||
settings=""
|
||||
settings_modules="modules/ahrs_float_dcm.xml modules/electrical.xml modules/gps.xml modules/gps_ublox.xml modules/guidance_rover_steering.xml~SR Guidance~ modules/gvf_classic.xml~GVF~ modules/gvf_ik.xml~GVF_IK~ modules/gvf_parametric.xml~GVF_PARAMETRIC~ modules/imu_common.xml modules/nav_rover_base.xml"
|
||||
gui_color="#35008400e400"
|
||||
/>
|
||||
<aircraft
|
||||
name="st_rover_n2"
|
||||
ac_id="2"
|
||||
airframe="airframes/UGR/steering_rover_n2.xml"
|
||||
radio="radios/UGR/T16SZ_SBUS_rover.xml"
|
||||
telemetry="telemetry/GVF/gvf_rover.xml"
|
||||
flight_plan="flight_plans/UGR/steering_rover_clubalhambra.xml"
|
||||
settings=""
|
||||
settings_modules="modules/ahrs_float_dcm.xml modules/electrical.xml modules/gps.xml modules/gps_ublox.xml modules/guidance_rover_steering.xml~SR Guidance~ modules/gvf_classic.xml~GVF~ modules/gvf_ik.xml~GVF_IK~ modules/gvf_parametric.xml~GVF_PARAMETRIC~ modules/imu_common.xml modules/nav_rover_base.xml"
|
||||
gui_color="#3300d1007a00"
|
||||
/>
|
||||
<aircraft
|
||||
name="st_rover_n3"
|
||||
ac_id="3"
|
||||
airframe="airframes/UGR/steering_rover_n3.xml"
|
||||
radio="radios/UGR/FrSkyX20.xml"
|
||||
telemetry="telemetry/GVF/gvf_rover.xml"
|
||||
flight_plan="flight_plans/UGR/steering_rover_clubalhambra.xml"
|
||||
settings=""
|
||||
settings_modules="modules/ahrs_float_dcm.xml modules/electrical.xml modules/gps.xml modules/gps_ublox.xml modules/guidance_rover_steering.xml~SR Guidance~ modules/gvf_classic.xml~GVF~ modules/gvf_ik.xml~GVF_IK~ modules/gvf_parametric.xml~GVF_PARAMETRIC~ modules/imu_common.xml modules/nav_rover_base.xml"
|
||||
gui_color="#e0001b002400"
|
||||
/>
|
||||
</conf>
|
||||
@@ -0,0 +1,209 @@
|
||||
<control_panel name="paparazzi control panel">
|
||||
<section name="programs">
|
||||
|
||||
</section>
|
||||
<section name="sessions">
|
||||
<session name="Flight UDP/WiFi">
|
||||
<program name="Server"/>
|
||||
<program name="PprgGCS"/>
|
||||
<program name="Data Link">
|
||||
<arg flag="-udp"/>
|
||||
</program>
|
||||
</session>
|
||||
<session name="Flight USB-serial@9600">
|
||||
<program name="Data Link">
|
||||
<arg flag="-d" constant="/dev/ttyUSB0"/>
|
||||
</program>
|
||||
<program name="Server"/>
|
||||
<program name="PprzGCS"/>
|
||||
</session>
|
||||
<session name="Flight USB-serial@57600">
|
||||
<program name="Data Link">
|
||||
<arg flag="-d" constant="/dev/ttyUSB0"/>
|
||||
<arg flag="-s" constant="57600"/>
|
||||
</program>
|
||||
<program name="Server"/>
|
||||
<program name="PprzGCS"/>
|
||||
</session>
|
||||
<session name="Flight USB-XBee-API@57600">
|
||||
<program name="Data Link">
|
||||
<arg flag="-d" constant="/dev/ttyUSB0"/>
|
||||
<arg flag="-transport" constant="xbee"/>
|
||||
<arg flag="-s" constant="57600"/>
|
||||
</program>
|
||||
<program name="Server"/>
|
||||
<program name="PprzGCS"/>
|
||||
</session>
|
||||
<session name="Flight USB-serial Redundant">
|
||||
<program name="Server"/>
|
||||
<program name="PprzGCS"/>
|
||||
<program name="Data Link">
|
||||
<arg flag="-d" constant="/dev/ttyUSB0"/>
|
||||
<arg flag="-id" constant="1"/>
|
||||
<arg flag="-redlink"/>
|
||||
</program>
|
||||
<program name="Data Link">
|
||||
<arg flag="-d" constant="/dev/ttyUSB1"/>
|
||||
<arg flag="-id" constant="2"/>
|
||||
<arg flag="-redlink"/>
|
||||
</program>
|
||||
<program name="Link Combiner"/>
|
||||
</session>
|
||||
<session name="SupperBitRF">
|
||||
<program name="Data Link">
|
||||
<arg flag="-d" constant="/dev/ttyACM0"/>
|
||||
</program>
|
||||
<program name="Server"/>
|
||||
<program name="PprzGCS"/>
|
||||
<program name="Messages"/>
|
||||
</session>
|
||||
<session name="SupperBitRF cable telemetry">
|
||||
<program name="Data Link">
|
||||
<arg flag="-d" constant="/dev/ttyACM1"/>
|
||||
<arg flag="-s" constant="57600"/>
|
||||
</program>
|
||||
<program name="Server"/>
|
||||
<program name="PprzGCS"/>
|
||||
<program name="Messages"/>
|
||||
<program name="Messages"/>
|
||||
</session>
|
||||
<session name="Messages and Settings">
|
||||
<program name="Data Link">
|
||||
<arg flag="-d" constant="/dev/ttyUSB0"/>
|
||||
<arg flag="-s" constant="57600"/>
|
||||
</program>
|
||||
<program name="Server">
|
||||
<arg flag="-n"/>
|
||||
</program>
|
||||
<program name="Messages"/>
|
||||
<program name="Settings">
|
||||
<arg flag="-ac" constant="@AIRCRAFT"/>
|
||||
</program>
|
||||
</session>
|
||||
<session name="Raw Sensors">
|
||||
<program name="Data Link">
|
||||
<arg flag="-d" constant="/dev/ttyUSB0"/>
|
||||
<arg flag="-s" constant="57600"/>
|
||||
</program>
|
||||
<program name="Server">
|
||||
<arg flag="-n"/>
|
||||
</program>
|
||||
<program name="Messages">
|
||||
<arg flag="-g" constant="300x400+0-220"/>
|
||||
</program>
|
||||
<program name="Settings">
|
||||
<arg flag="-ac" constant="@AIRCRAFT"/>
|
||||
<arg flag="-g" constant="800x200+0-0"/>
|
||||
</program>
|
||||
<program name="Real-time Plotter">
|
||||
<arg flag="-g" constant="1000x250-0+0"/>
|
||||
<arg flag="-t" constant="ACC"/>
|
||||
<arg flag="-u" constant="0.05"/>
|
||||
<arg flag="-c" constant="0.00"/>
|
||||
<arg flag="-c" constant="*:telemetry:IMU_ACCEL_RAW:ax"/>
|
||||
<arg flag="-c" constant="*:telemetry:IMU_ACCEL_RAW:ay"/>
|
||||
<arg flag="-c" constant="*:telemetry:IMU_ACCEL_RAW:az"/>
|
||||
<arg flag="-n"/>
|
||||
<arg flag="-g" constant="1000x250-0+250"/>
|
||||
<arg flag="-t" constant="GYRO"/>
|
||||
<arg flag="-u" constant="0.05"/>
|
||||
<arg flag="-c" constant="0.00"/>
|
||||
<arg flag="-c" constant="*:telemetry:IMU_GYRO_RAW:gp"/>
|
||||
<arg flag="-c" constant="*:telemetry:IMU_GYRO_RAW:gq"/>
|
||||
<arg flag="-c" constant="*:telemetry:IMU_GYRO_RAW:gr"/>
|
||||
<arg flag="-n"/>
|
||||
<arg flag="-g" constant="1000x250-0+500"/>
|
||||
<arg flag="-t" constant="MAG"/>
|
||||
<arg flag="-u" constant="0.05"/>
|
||||
<arg flag="-c" constant="0.00"/>
|
||||
<arg flag="-c" constant="*:telemetry:IMU_MAG_RAW:mx"/>
|
||||
<arg flag="-c" constant="*:telemetry:IMU_MAG_RAW:my"/>
|
||||
<arg flag="-c" constant="*:telemetry:IMU_MAG_RAW:mz"/>
|
||||
<arg flag="-n"/>
|
||||
<arg flag="-g" constant="1000x250-0+750"/>
|
||||
<arg flag="-t" constant="BARO"/>
|
||||
<arg flag="-u" constant="0.05"/>
|
||||
<arg flag="-c" constant="101325.0"/>
|
||||
<arg flag="-c" constant="*:telemetry:BARO_RAW:abs"/>
|
||||
</program>
|
||||
</session>
|
||||
<session name="Scaled Sensors">
|
||||
<program name="Data Link">
|
||||
<arg flag="-d" constant="/dev/ttyUSB0"/>
|
||||
<arg flag="-s" constant="57600"/>
|
||||
</program>
|
||||
<program name="Server">
|
||||
<arg flag="-n"/>
|
||||
</program>
|
||||
<program name="Messages">
|
||||
<arg flag="-g" constant="300x400+0-220"/>
|
||||
</program>
|
||||
<program name="Settings">
|
||||
<arg flag="-ac" constant="@AIRCRAFT"/>
|
||||
<arg flag="-g" constant="800x200+0-0"/>
|
||||
</program>
|
||||
<program name="Real-time Plotter">
|
||||
<arg flag="-g" constant="1000x250-0+0"/>
|
||||
<arg flag="-t" constant="ACC"/>
|
||||
<arg flag="-u" constant="0.05"/>
|
||||
<arg flag="-c" constant="9.81"/>
|
||||
<arg flag="-c" constant="0.00"/>
|
||||
<arg flag="-c" constant="-9.81"/>
|
||||
<arg flag="-c" constant="*:telemetry:IMU_ACCEL_SCALED:ax:0.0009766"/>
|
||||
<arg flag="-c" constant="*:telemetry:IMU_ACCEL_SCALED:ay:0.0009766"/>
|
||||
<arg flag="-c" constant="*:telemetry:IMU_ACCEL_SCALED:az:0.0009766"/>
|
||||
<arg flag="-n"/>
|
||||
<arg flag="-g" constant="1000x250-0+250"/>
|
||||
<arg flag="-t" constant="GYRO"/>
|
||||
<arg flag="-u" constant="0.05"/>
|
||||
<arg flag="-c" constant="0.00"/>
|
||||
<arg flag="-c" constant="*:telemetry:IMU_GYRO_SCALED:gp:0.0139882"/>
|
||||
<arg flag="-c" constant="*:telemetry:IMU_GYRO_SCALED:gq:0.0139882"/>
|
||||
<arg flag="-c" constant="*:telemetry:IMU_GYRO_SCALED:gr:0.0139882"/>
|
||||
<arg flag="-n"/>
|
||||
<arg flag="-g" constant="1000x250-0+500"/>
|
||||
<arg flag="-t" constant="MAG"/>
|
||||
<arg flag="-u" constant="0.05"/>
|
||||
<arg flag="-c" constant="0.00"/>
|
||||
<arg flag="-c" constant="*:telemetry:IMU_MAG_SCALED:mx:0.0004883"/>
|
||||
<arg flag="-c" constant="*:telemetry:IMU_MAG_SCALED:my:0.0004883"/>
|
||||
<arg flag="-c" constant="*:telemetry:IMU_MAG_SCALED:mz:0.0004883"/>
|
||||
</program>
|
||||
</session>
|
||||
<session name="_Multi_sim_fixedwing">
|
||||
<program name="Data Link">
|
||||
<arg flag="-udp"/>
|
||||
<arg flag="-udp_broadcast"/>
|
||||
</program>
|
||||
<program name="Server">
|
||||
<arg flag="-n"/>
|
||||
</program>
|
||||
<program name="PprzGCS"/>
|
||||
<program name="Simulator">
|
||||
<arg flag="-a" constant="gvf_sonic1"/>
|
||||
<arg flag="-t" constant="nps"/>
|
||||
<arg flag="--time_factor" constant="1"/>
|
||||
</program>
|
||||
<program name="Simulator">
|
||||
<arg flag="-a" constant="gvf_sonic2"/>
|
||||
<arg flag="-t" constant="nps"/>
|
||||
<arg flag="--time_factor" constant="1"/>
|
||||
</program>
|
||||
<program name="Messages"/>
|
||||
</session>
|
||||
<session name="Simulation Fast">
|
||||
<program name="Simulator">
|
||||
<arg flag="-a" constant="st_rover_n3"/>
|
||||
<arg flag="-t" constant="nps"/>
|
||||
</program>
|
||||
<program name="Data Link">
|
||||
<arg flag="-udp"/>
|
||||
<arg flag="-udp_broadcast"/>
|
||||
</program>
|
||||
<program name="Server">
|
||||
<arg flag="-n"/>
|
||||
</program>
|
||||
<program name="PprzGCS"/>
|
||||
</session>
|
||||
</section>
|
||||
</control_panel>
|
||||
@@ -0,0 +1,35 @@
|
||||
<conf>
|
||||
<aircraft
|
||||
name="sonic_fixedwing"
|
||||
ac_id="1"
|
||||
airframe="airframes/UGR/sonic_n1.xml"
|
||||
radio="radios/T7C_SBUS.xml"
|
||||
telemetry="telemetry/UGR/gvf_fixedwing.xml"
|
||||
flight_plan="flight_plans/demo_gvf.xml"
|
||||
settings="settings/UGR/fixedwing_basic.xml"
|
||||
settings_modules="modules/ahrs_float_dcm.xml modules/electrical.xml modules/gps.xml modules/gps_ublox.xml modules/gps_ubx_ucenter.xml modules/guidance_full_pid_fw.xml modules/gvf_classic.xml~GVF~ modules/gvf_ik.xml~GVF_IK~ modules/gvf_parametric.xml~GVF_PARAMETRIC~ modules/imu_common.xml modules/nav_basic_fw.xml modules/power_switch.xml modules/stabilization_adaptive_fw.xml"
|
||||
gui_color="#6200a000ea00"
|
||||
/>
|
||||
<aircraft
|
||||
name="st_rover"
|
||||
ac_id="2"
|
||||
airframe="airframes/UGR/steering_rover_n2.xml"
|
||||
radio="radios/UGR/T16SZ_SBUS_rover.xml"
|
||||
telemetry="telemetry/GVF/gvf_rover.xml"
|
||||
flight_plan="flight_plans/UGR/steering_rover_clubalhambra.xml"
|
||||
settings=""
|
||||
settings_modules="modules/ahrs_float_dcm.xml modules/electrical.xml modules/gps.xml modules/gps_ublox.xml modules/guidance_rover_steering.xml~SR Guidance~ modules/gvf_classic.xml~GVF~ modules/gvf_ik.xml~GVF_IK~ modules/gvf_parametric.xml~GVF_PARAMETRIC~ modules/imu_common.xml modules/nav_rover_base.xml"
|
||||
gui_color="#35008400e400"
|
||||
/>
|
||||
<aircraft
|
||||
name="bebop_rotorcraft"
|
||||
ac_id="3"
|
||||
airframe="airframes/examples/bebop2_gvf.xml"
|
||||
radio="radios/dummy.xml"
|
||||
telemetry="telemetry/GVF/gvf_default_rotorcraft.xml"
|
||||
flight_plan="flight_plans/demo_gvf_rotorcraft.xml"
|
||||
settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_speed.xml"
|
||||
settings_modules="modules/air_data.xml modules/electrical.xml modules/gps.xml modules/gps_ublox.xml modules/gps_ubx_ucenter.xml modules/guidance_pid_rotorcraft.xml modules/guidance_rotorcraft.xml modules/gvf_classic.xml~GVF~ modules/imu_common.xml modules/ins_ekf2.xml modules/nav_rotorcraft.xml modules/stabilization_indi_simple.xml"
|
||||
gui_color="#6200a000ea00"
|
||||
/>
|
||||
</conf>
|
||||
@@ -56,7 +56,7 @@ The GVF Viewer tool is built inside the GCS map widget but will be deactivated u
|
||||
</gcsconf>
|
||||
|
||||
|
||||
Once we loaded GVF Viewer, we need an aircraft working with GVF. In order to test some features of this tool, the user can load with ``start.py`` the GVF configurations located in ``conf/userconf/GVF`` and compile the simulation target.
|
||||
Once we loaded GVF Viewer, we need an aircraft working with GVF. In order to test some features of this tool, the user can load with ``start.py`` the GVF configurations located in ``conf/userconf/conf_example_gvf.xml`` and compile the simulation target.
|
||||
|
||||
How GVF Viewer works
|
||||
--------------------
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -28,119 +28,55 @@
|
||||
#ifndef GVF_H
|
||||
#define GVF_H
|
||||
|
||||
#define GVF_GRAVITY 9.806
|
||||
|
||||
/*! Default GCS trajectory painter */
|
||||
#ifndef GVF_OCAML_GCS
|
||||
#define GVF_OCAML_GCS true
|
||||
#endif
|
||||
|
||||
#include "std.h"
|
||||
#include "modules/guidance/gvf_common.h"
|
||||
#include "modules/guidance/trajectories/gvf_traj.h"
|
||||
|
||||
/** STRUCTS ---------------------------------------------------------------- **/
|
||||
|
||||
/** @typedef gvf_con
|
||||
* @brief Control parameters for the GVF
|
||||
* @param ke Gain defining how agressive is the vector field
|
||||
* @param kn Gain for making converge the vehile to the vector field
|
||||
* @param error Error signal. It does not have any specific units. It depends on how the trajectory has been implemented. Check the specific wiki entry for each trajectory.
|
||||
* @param omega Angular velocity of the vehicle
|
||||
* @param error_n Error of alignment with the GVF
|
||||
* @param speed Speed of the vehicle. It is only used in rotorcrafts for now.
|
||||
* @param s Defines the direction to be tracked. Its meaning depends on the trajectory and its implementation. Check the wiki entry of the GVF. It takes the values -1 or 1.
|
||||
* @param align Align the vehicle with the direction of the vector field. Can only used in rotorcrafts.
|
||||
*/
|
||||
|
||||
typedef struct {
|
||||
float ke;
|
||||
float kn;
|
||||
float error;
|
||||
float omega;
|
||||
float error_n;
|
||||
float speed;
|
||||
int8_t s;
|
||||
bool align;
|
||||
} gvf_con;
|
||||
|
||||
typedef struct {
|
||||
float n_norm;
|
||||
float t_norm;
|
||||
float omega_d;
|
||||
float omega;
|
||||
} gvf_tel;
|
||||
|
||||
// Extern structs
|
||||
extern gvf_con gvf_control;
|
||||
|
||||
typedef struct {
|
||||
float course;
|
||||
float px_dot;
|
||||
float py_dot;
|
||||
} gvf_st;
|
||||
|
||||
extern gvf_st gvf_state;
|
||||
|
||||
enum trajectories {
|
||||
LINE = 0,
|
||||
ELLIPSE,
|
||||
SIN,
|
||||
NONE = 255,
|
||||
};
|
||||
|
||||
typedef struct {
|
||||
enum trajectories type;
|
||||
float p[16];
|
||||
} gvf_tra;
|
||||
|
||||
/** @typedef gvf_seg
|
||||
* @brief Struct employed by the LINE trajectory for the special case of trackinga segment, which is described by the coordinates x1, y1, x2, y2
|
||||
* @param seg Tracking a segment or not
|
||||
* @param x1 coordinate w.r.t. HOME
|
||||
* @param y1 coordinate w.r.t. HOME
|
||||
* @param x2 coordinate w.r.t. HOME
|
||||
* @param y2 coordinate w.r.t. HOME
|
||||
*/
|
||||
typedef struct {
|
||||
int seg;
|
||||
float x1;
|
||||
float y1;
|
||||
float x2;
|
||||
float y2;
|
||||
} gvf_seg;
|
||||
|
||||
extern gvf_tra gvf_trajectory;
|
||||
|
||||
struct gvf_grad {
|
||||
float nx;
|
||||
float ny;
|
||||
float nz;
|
||||
};
|
||||
|
||||
struct gvf_Hess {
|
||||
float H11;
|
||||
float H12;
|
||||
float H13;
|
||||
float H21;
|
||||
float H22;
|
||||
float H23;
|
||||
float H31;
|
||||
float H32;
|
||||
float H33;
|
||||
};
|
||||
/** EXTERN FUNCTIONS ------------------------------------------------------- **/
|
||||
|
||||
extern void gvf_init(void);
|
||||
void gvf_control_2D(float ke, float kn, float e,
|
||||
extern void gvf_control_2D(float ke, float kn, float e,
|
||||
struct gvf_grad *, struct gvf_Hess *);
|
||||
extern void gvf_set_speed(float speed); // Rotorcraft only (for now)
|
||||
extern void gvf_set_align(bool align); // Rotorcraft only
|
||||
extern void gvf_set_direction(int8_t s);
|
||||
|
||||
// Straight line
|
||||
extern bool gvf_line_XY_heading(float x, float y, float heading);
|
||||
extern bool gvf_line_XY1_XY2(float x1, float y1, float x2, float y2);
|
||||
extern bool gvf_line_wp1_wp2(uint8_t wp1, uint8_t wp2);
|
||||
extern bool gvf_segment_loop_XY1_XY2(float x1, float y1, float x2, float y2, float d1, float d2);
|
||||
extern bool gvf_segment_loop_wp1_wp2(uint8_t wp1, uint8_t wp2, float d1, float d2);
|
||||
extern bool gvf_segment_XY1_XY2(float x1, float y1, float x2, float y2);
|
||||
extern bool gvf_segment_wp1_wp2(uint8_t wp1, uint8_t wp2);
|
||||
extern bool gvf_line_wp_heading(uint8_t wp, float heading);
|
||||
|
||||
// Ellipse
|
||||
extern bool gvf_ellipse_wp(uint8_t wp, float a, float b, float alpha);
|
||||
extern bool gvf_ellipse_XY(float x, float y, float a, float b, float alpha);
|
||||
|
||||
// Sinusoidal
|
||||
extern bool gvf_sin_XY_alpha(float x, float y, float alpha, float w, float off, float A);
|
||||
extern bool gvf_sin_wp1_wp2(uint8_t wp1, uint8_t wp2, float w, float off,
|
||||
float A);
|
||||
extern bool gvf_sin_wp_alpha(uint8_t wp, float alpha, float w, float off,
|
||||
float A);
|
||||
|
||||
#endif // GVF_H
|
||||
@@ -1,72 +0,0 @@
|
||||
/*
|
||||
* Copyright (C) 2020 Hector Garcia de Marina <hgarciad@ucm.es>
|
||||
*
|
||||
* This file is part of paparazzi.
|
||||
*
|
||||
* paparazzi is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 2, or (at your option)
|
||||
* any later version.
|
||||
*
|
||||
* paparazzi is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with paparazzi; see the file COPYING. If not, see
|
||||
* <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
/**
|
||||
* @file modules/guidance/gvf/gvf_low_level_control.c
|
||||
*
|
||||
* Firmware dependent file for the guiding vector field algorithm for 2D trajectories.
|
||||
*/
|
||||
|
||||
#include "autopilot.h"
|
||||
#include "modules/guidance/gvf/gvf_low_level_control.h"
|
||||
#include "modules/guidance/gvf/gvf.h"
|
||||
|
||||
#if defined(FIXEDWING_FIRMWARE)
|
||||
#include "firmwares/fixedwing/stabilization/stabilization_attitude.h"
|
||||
#endif
|
||||
|
||||
void gvf_low_level_getState(void)
|
||||
{
|
||||
#if defined(FIXEDWING_FIRMWARE)
|
||||
float ground_speed = stateGetHorizontalSpeedNorm_f();
|
||||
gvf_state.course = stateGetHorizontalSpeedDir_f();
|
||||
gvf_state.px_dot = ground_speed * sinf(gvf_state.course);
|
||||
gvf_state.py_dot = ground_speed * cosf(gvf_state.course);
|
||||
|
||||
#elif defined(ROVER_FIRMWARE) || defined(ROTORCRAFT_FIRMWARE)
|
||||
// We assume that the course and psi
|
||||
// of the rover (steering wheel) are the same
|
||||
gvf_state.course = stateGetNedToBodyEulers_f()->psi;
|
||||
gvf_state.px_dot = stateGetSpeedEnu_f()->x;
|
||||
gvf_state.py_dot = stateGetSpeedEnu_f()->y;
|
||||
#endif
|
||||
}
|
||||
|
||||
void gvf_low_level_control_2D(float omega)
|
||||
{
|
||||
|
||||
#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);
|
||||
}
|
||||
#endif
|
||||
|
||||
(void)(omega); // Avoid unused parameter warning
|
||||
}
|
||||
|
||||
@@ -1,67 +0,0 @@
|
||||
/*
|
||||
* Copyright (C) 2020 Hector Garcia de Marina <hgarciad@ucm.es>
|
||||
*
|
||||
* This file is part of paparazzi.
|
||||
*
|
||||
* paparazzi is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 2, or (at your option)
|
||||
* any later version.
|
||||
*
|
||||
* paparazzi is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with paparazzi; see the file COPYING. If not, see
|
||||
* <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
/**
|
||||
* @file modules/guidance/gvf/gvf_low_level_control.h
|
||||
*
|
||||
* Firmware dependent file for the guiding vector field algorithm for 2D trajectories.
|
||||
*/
|
||||
|
||||
#ifndef GVF_LOW_LEVEL_CONTROL_H
|
||||
#define GVF_LOW_LEVEL_CONTROL_H
|
||||
|
||||
#ifdef FIXEDWING_FIRMWARE
|
||||
#include "firmwares/fixedwing/nav.h"
|
||||
#include "modules/nav/common_nav.h"
|
||||
#include "firmwares/fixedwing/stabilization/stabilization_attitude.h"
|
||||
#define gvf_setNavMode(_navMode) (horizontal_mode = _navMode)
|
||||
#define GVF_MODE_ROUTE HORIZONTAL_MODE_ROUTE
|
||||
#define GVF_MODE_WAYPOINT HORIZONTAL_MODE_WAYPOINT
|
||||
#define GVF_MODE_CIRCLE HORIZONTAL_MODE_CIRCLE
|
||||
|
||||
#elif defined(ROVER_FIRMWARE)
|
||||
#include "state.h"
|
||||
#include "firmwares/rover/navigation.h"
|
||||
#define gvf_setNavMode(_navMode) (nav.mode = _navMode)
|
||||
#define GVF_MODE_ROUTE NAV_MODE_ROUTE
|
||||
#define GVF_MODE_WAYPOINT NAV_MODE_WAYPOINT
|
||||
#define GVF_MODE_CIRCLE NAV_MODE_CIRCLE
|
||||
|
||||
#elif defined(ROTORCRAFT_FIRMWARE)
|
||||
#include "firmwares/rotorcraft/navigation.h"
|
||||
#define gvf_setNavMode(_navMode) (nav.horizontal_mode = _navMode)
|
||||
#define GVF_MODE_ROUTE NAV_HORIZONTAL_MODE_ROUTE
|
||||
#define GVF_MODE_WAYPOINT NAV_HORIZONTAL_MODE_WAYPOINT
|
||||
#define GVF_MODE_CIRCLE NAV_HORIZONTAL_MODE_CIRCLE
|
||||
|
||||
//NAV_SETPOINT_MODE_POS
|
||||
//NAV_SETPOINT_MODE_SPEED
|
||||
//NAV_SETPOINT_MODE_ACCEL
|
||||
|
||||
#else
|
||||
#error "GVF does not support your firmware yet!"
|
||||
#endif
|
||||
|
||||
// Low level control functions
|
||||
extern void gvf_low_level_getState(void);
|
||||
extern void gvf_low_level_control_2D(float);
|
||||
|
||||
#endif // GVF_LOW_LEVEL_CONTROL_H
|
||||
|
||||
+49
-34
@@ -20,16 +20,10 @@
|
||||
*
|
||||
*/
|
||||
|
||||
/** \file gvf_ellipse.c
|
||||
*
|
||||
* Guidance algorithm based on vector fields
|
||||
* 2D Ellipse trajectory
|
||||
*/
|
||||
#include "nav_ellipse.h"
|
||||
|
||||
|
||||
#include "modules/nav/common_nav.h"
|
||||
#include "gvf_ellipse.h"
|
||||
#include "generated/airframe.h"
|
||||
#include "modules/guidance/gvf/gvf.h"
|
||||
|
||||
/*! Default gain ke for the ellipse trajectory */
|
||||
#ifndef GVF_ELLIPSE_KE
|
||||
@@ -60,36 +54,57 @@ gvf_ell_par gvf_ellipse_par = {GVF_ELLIPSE_KE, GVF_ELLIPSE_KN,
|
||||
GVF_ELLIPSE_A, GVF_ELLIPSE_B, GVF_ELLIPSE_ALPHA
|
||||
};
|
||||
|
||||
void gvf_ellipse_info(float *phi, struct gvf_grad *grad,
|
||||
struct gvf_Hess *hess)
|
||||
// Param array lenght
|
||||
static int gvf_p_len_wps = 0;
|
||||
|
||||
/** ------------------------------------------------------------------------ **/
|
||||
|
||||
// ELLIPSE
|
||||
|
||||
bool nav_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;
|
||||
|
||||
struct EnuCoor_f *p = stateGetPositionEnu_f();
|
||||
float px = p->x;
|
||||
float py = p->y;
|
||||
float wx = gvf_trajectory.p[0];
|
||||
float wy = gvf_trajectory.p[1];
|
||||
float a = gvf_trajectory.p[2];
|
||||
float b = gvf_trajectory.p[3];
|
||||
float alpha = gvf_trajectory.p[4];
|
||||
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;
|
||||
gvf_trajectory.p_len= 5 + gvf_p_len_wps;
|
||||
gvf_p_len_wps = 0;
|
||||
|
||||
float cosa = cosf(alpha);
|
||||
float sina = sinf(alpha);
|
||||
// SAFE MODE
|
||||
if (a < 1 || b < 1) {
|
||||
gvf_trajectory.p[2] = 60;
|
||||
gvf_trajectory.p[3] = 60;
|
||||
}
|
||||
|
||||
// Phi(x,y)
|
||||
float xel = (px - wx) * cosa - (py - wy) * sina;
|
||||
float yel = (px - wx) * sina + (py - wy) * cosa;
|
||||
*phi = (xel / a) * (xel / a) + (yel / b) * (yel / b) - 1;
|
||||
if ((int)gvf_trajectory.p[2] == (int)gvf_trajectory.p[3]) {
|
||||
gvf_setNavMode(GVF_MODE_CIRCLE);
|
||||
|
||||
// grad Phi
|
||||
grad->nx = (2 * xel / (a * a)) * cosa + (2 * yel / (b * b)) * sina;
|
||||
grad->ny = (2 * yel / (b * b)) * cosa - (2 * xel / (a * a)) * sina;
|
||||
} else {
|
||||
gvf_setNavMode(GVF_MODE_WAYPOINT);
|
||||
}
|
||||
|
||||
// Hessian Phi
|
||||
hess->H11 = 2 * (cosa * cosa / (a * a)
|
||||
+ sina * sina / (b * b));
|
||||
hess->H12 = 2 * sina * cosa * (1 / (b * b) - 1 / (a * a));
|
||||
hess->H21 = hess->H12;
|
||||
hess->H22 = 2 * (sina * sina / (a * a)
|
||||
+ cosa * cosa / (b * b));
|
||||
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 nav_gvf_ellipse_wp(uint8_t wp, float a, float b, float alpha)
|
||||
{
|
||||
gvf_trajectory.p[5] = wp;
|
||||
gvf_p_len_wps = 1;
|
||||
|
||||
nav_gvf_ellipse_XY(WaypointX(wp), WaypointY(wp), a, b, alpha);
|
||||
return true;
|
||||
}
|
||||
+6
-8
@@ -20,16 +20,10 @@
|
||||
*
|
||||
*/
|
||||
|
||||
/** @file gvf_ellipse.h
|
||||
*
|
||||
* Guidance algorithm based on vector fields
|
||||
* 2D Ellipse trajectory
|
||||
*/
|
||||
|
||||
#ifndef GVF_ELLIPSE_H
|
||||
#define GVF_ELLIPSE_H
|
||||
|
||||
#include "modules/guidance/gvf/gvf.h"
|
||||
#include "modules/guidance/trajectories/gvf_traj.h"
|
||||
|
||||
/** @typedef gvf_ell_par
|
||||
* @brief Parameters for the GVF line trajectory
|
||||
@@ -47,8 +41,12 @@ typedef struct {
|
||||
float alpha;
|
||||
} gvf_ell_par;
|
||||
|
||||
/** ------------------------------------------------------------------------ **/
|
||||
|
||||
extern gvf_ell_par gvf_ellipse_par;
|
||||
|
||||
extern void gvf_ellipse_info(float *phi, struct gvf_grad *, struct gvf_Hess *);
|
||||
// Ellipse
|
||||
extern bool nav_gvf_ellipse_wp(uint8_t wp, float a, float b, float alpha);
|
||||
extern bool nav_gvf_ellipse_XY(float x, float y, float a, float b, float alpha);
|
||||
|
||||
#endif // GVF_ELLIPSE_H
|
||||
@@ -0,0 +1,256 @@
|
||||
/*
|
||||
* Copyright (C) 2016 Hector Garcia de Marina
|
||||
*
|
||||
* 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 "nav_line.h"
|
||||
|
||||
#include "generated/airframe.h"
|
||||
#include "modules/guidance/gvf/gvf.h"
|
||||
|
||||
/*! Gain ke for the line trajectory*/
|
||||
#ifndef GVF_LINE_KE
|
||||
#define GVF_LINE_KE 1
|
||||
#endif
|
||||
|
||||
/*! Gain kn for the line trajectory*/
|
||||
#ifndef GVF_LINE_KN
|
||||
#define GVF_LINE_KN 1
|
||||
#endif
|
||||
|
||||
/*! Default heading in degrees for a trajectory called from gvf_line_**_HEADING */
|
||||
#ifndef GVF_LINE_HEADING
|
||||
#define GVF_LINE_HEADING 0
|
||||
#endif
|
||||
|
||||
/*! In case of tracking a segment, how much distance in meters will go the vehicle beyond the point x1,y1 before turning back */
|
||||
#ifndef GVF_SEGMENT_D1
|
||||
#define GVF_SEGMENT_D1 0
|
||||
#endif
|
||||
|
||||
/*! In case of tracking a segment, how much distance in meters will go the vehicle beyond the point x2,y2 before turning back */
|
||||
#ifndef GVF_SEGMENT_D2
|
||||
#define GVF_SEGMENT_D2 0
|
||||
#endif
|
||||
|
||||
gvf_li_par gvf_line_par = {GVF_LINE_KE, GVF_LINE_KN, GVF_LINE_HEADING};
|
||||
gvf_seg_par gvf_segment_par = {GVF_SEGMENT_D1, GVF_SEGMENT_D2};
|
||||
|
||||
// Param array lenght
|
||||
static int gvf_p_len_wps = 0;
|
||||
|
||||
/** ------------------------------------------------------------------------ **/
|
||||
|
||||
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_trajectory.p_len= 3 + gvf_p_len_wps;
|
||||
gvf_p_len_wps = 0;
|
||||
|
||||
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;
|
||||
|
||||
gvf_setNavMode(GVF_MODE_WAYPOINT);
|
||||
|
||||
gvf_segment.seg = 0;
|
||||
}
|
||||
|
||||
static int out_of_segment_area(float x1, float y1, float x2, float y2, float d1, float d2)
|
||||
{
|
||||
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 alpha = atan2f(zy, zx);
|
||||
|
||||
float cosa = cosf(-alpha);
|
||||
float sina = sinf(-alpha);
|
||||
|
||||
float pxr = px * cosa - py * sina;
|
||||
float zxr = zx * cosa - zy * sina;
|
||||
|
||||
int s = 0;
|
||||
|
||||
if (pxr < -d1) {
|
||||
s = 1;
|
||||
} else if (pxr > (zxr + d2)) {
|
||||
s = -1;
|
||||
}
|
||||
|
||||
if (zy < 0) {
|
||||
s *= -1;
|
||||
}
|
||||
|
||||
return s;
|
||||
}
|
||||
|
||||
/** ------------------------------------------------------------------------ **/
|
||||
|
||||
// STRAIGHT LINE
|
||||
|
||||
bool nav_gvf_line_XY_heading(float a, float b, float heading)
|
||||
{
|
||||
gvf_set_direction(1);
|
||||
gvf_line(a, b, heading);
|
||||
return true;
|
||||
}
|
||||
|
||||
bool nav_gvf_line_wp_heading(uint8_t wp, float heading)
|
||||
{
|
||||
heading = RadOfDeg(heading);
|
||||
|
||||
float a = WaypointX(wp);
|
||||
float b = WaypointY(wp);
|
||||
|
||||
return nav_gvf_line_XY_heading(a, b, heading);
|
||||
}
|
||||
|
||||
bool nav_gvf_line_XY1_XY2(float x1, float y1, float x2, float y2)
|
||||
{
|
||||
if (gvf_p_len_wps != 2) {
|
||||
gvf_trajectory.p[3] = x2;
|
||||
gvf_trajectory.p[4] = y2;
|
||||
gvf_trajectory.p[5] = 0;
|
||||
gvf_p_len_wps = 3;
|
||||
}
|
||||
|
||||
float zx = x2 - x1;
|
||||
float zy = y2 - y1;
|
||||
|
||||
nav_gvf_line_XY_heading(x1, y1, atan2f(zx, zy));
|
||||
|
||||
gvf_setNavMode(GVF_MODE_ROUTE);
|
||||
gvf_segment.seg = 1;
|
||||
gvf_segment.x1 = x1;
|
||||
gvf_segment.y1 = y1;
|
||||
gvf_segment.x2 = x2;
|
||||
gvf_segment.y2 = y2;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool nav_gvf_line_wp1_wp2(uint8_t wp1, uint8_t wp2)
|
||||
{
|
||||
gvf_trajectory.p[3] = wp1;
|
||||
gvf_trajectory.p[4] = wp2;
|
||||
gvf_p_len_wps = 2;
|
||||
|
||||
float x1 = WaypointX(wp1);
|
||||
float y1 = WaypointY(wp1);
|
||||
float x2 = WaypointX(wp2);
|
||||
float y2 = WaypointY(wp2);
|
||||
|
||||
return nav_gvf_line_XY1_XY2(x1, y1, x2, y2);
|
||||
}
|
||||
|
||||
// SEGMENT
|
||||
|
||||
bool nav_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_set_direction(s);
|
||||
}
|
||||
|
||||
float zx = x2 - x1;
|
||||
float zy = y2 - y1;
|
||||
float alpha = atanf(zx / zy);
|
||||
|
||||
gvf_line(x1, y1, alpha);
|
||||
|
||||
gvf_setNavMode(GVF_MODE_ROUTE);
|
||||
|
||||
gvf_segment.seg = 1;
|
||||
gvf_segment.x1 = x1;
|
||||
gvf_segment.y1 = y1;
|
||||
gvf_segment.x2 = x2;
|
||||
gvf_segment.y2 = y2;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool nav_gvf_segment_loop_wp1_wp2(uint8_t wp1, uint8_t wp2, float d1, float d2)
|
||||
{
|
||||
gvf_trajectory.p[3] = wp1;
|
||||
gvf_trajectory.p[4] = wp2;
|
||||
gvf_trajectory.p[5] = d1;
|
||||
gvf_trajectory.p[6] = d2;
|
||||
gvf_p_len_wps = 4;
|
||||
|
||||
float x1 = WaypointX(wp1);
|
||||
float y1 = WaypointY(wp1);
|
||||
float x2 = WaypointX(wp2);
|
||||
float y2 = WaypointY(wp2);
|
||||
|
||||
return nav_gvf_segment_loop_XY1_XY2(x1, y1, x2, y2, d1, d2);
|
||||
}
|
||||
|
||||
bool nav_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 nav_gvf_line_XY1_XY2(x1, y1, x2, y2);
|
||||
}
|
||||
|
||||
bool nav_gvf_segment_wp1_wp2(uint8_t wp1, uint8_t wp2)
|
||||
{
|
||||
gvf_trajectory.p[3] = wp1;
|
||||
gvf_trajectory.p[4] = wp2;
|
||||
gvf_p_len_wps = 2;
|
||||
|
||||
float x1 = WaypointX(wp1);
|
||||
float y1 = WaypointY(wp1);
|
||||
float x2 = WaypointX(wp2);
|
||||
float y2 = WaypointY(wp2);
|
||||
|
||||
return nav_gvf_segment_XY1_XY2(x1, y1, x2, y2);
|
||||
}
|
||||
|
||||
bool nav_gvf_segment_points(struct FloatVect2 start, struct FloatVect2 end)
|
||||
{
|
||||
return nav_gvf_segment_XY1_XY2(start.x, start.y, end.x, end.y);
|
||||
}
|
||||
+14
-2
@@ -29,7 +29,8 @@
|
||||
#ifndef GVF_LINE_H
|
||||
#define GVF_LINE_H
|
||||
|
||||
#include "modules/guidance/gvf/gvf.h"
|
||||
#include "modules/guidance/trajectories/gvf_traj.h"
|
||||
#include "math/pprz_algebra_float.h"
|
||||
|
||||
/** @typedef gvf_li_par
|
||||
* @brief Parameters for the GVF line trajectory
|
||||
@@ -53,9 +54,20 @@ typedef struct {
|
||||
float d2;
|
||||
} gvf_seg_par;
|
||||
|
||||
/** ------------------------------------------------------------------------ **/
|
||||
|
||||
extern gvf_li_par gvf_line_par;
|
||||
extern gvf_seg_par gvf_segment_par;
|
||||
|
||||
extern void gvf_line_info(float *phi, struct gvf_grad *, struct gvf_Hess *);
|
||||
// Straight line
|
||||
extern bool nav_gvf_line_XY_heading(float x, float y, float heading);
|
||||
extern bool nav_gvf_line_wp_heading(uint8_t wp, float heading);
|
||||
extern bool nav_gvf_line_XY1_XY2(float x1, float y1, float x2, float y2);
|
||||
extern bool nav_gvf_line_wp1_wp2(uint8_t wp1, uint8_t wp2);
|
||||
extern bool nav_gvf_segment_loop_XY1_XY2(float x1, float y1, float x2, float y2, float d1, float d2);
|
||||
extern bool nav_gvf_segment_loop_wp1_wp2(uint8_t wp1, uint8_t wp2, float d1, float d2);
|
||||
extern bool nav_gvf_segment_XY1_XY2(float x1, float y1, float x2, float y2);
|
||||
extern bool nav_gvf_segment_wp1_wp2(uint8_t wp1, uint8_t wp2);
|
||||
extern bool nav_gvf_segment_points(struct FloatVect2 start, struct FloatVect2 end);
|
||||
|
||||
#endif // GVF_LINE_H
|
||||
@@ -0,0 +1,131 @@
|
||||
/*
|
||||
* Copyright (C) 2016 Hector Garcia de Marina
|
||||
*
|
||||
* 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 "nav_sin.h"
|
||||
|
||||
#include "generated/airframe.h"
|
||||
#include "modules/guidance/gvf/gvf.h"
|
||||
|
||||
/*! Default gain ke for the sin trajectory*/
|
||||
#ifndef GVF_SIN_KE
|
||||
#define GVF_SIN_KE 1
|
||||
#endif
|
||||
|
||||
/*! Default gain kn for the sin trajectory*/
|
||||
#ifndef GVF_SIN_KN
|
||||
#define GVF_SIN_KN 1
|
||||
#endif
|
||||
|
||||
/*! Default orientation in rads for the sin trajectory function gvf_sin_**_alpha*/
|
||||
#ifndef GVF_SIN_ALPHA
|
||||
#define GVF_SIN_ALPHA 0
|
||||
#endif
|
||||
|
||||
/*! Default frequency for the sin trajectory in rads*/
|
||||
#ifndef GVF_SIN_W
|
||||
#define GVF_SIN_W 0
|
||||
#endif
|
||||
|
||||
/*! Default off-set in rads for the sin trajectory in rads*/
|
||||
#ifndef GVF_SIN_OFF
|
||||
#define GVF_SIN_OFF 0
|
||||
#endif
|
||||
|
||||
/*! Default amplitude for the sin trajectory in meters*/
|
||||
#ifndef GVF_SIN_A
|
||||
#define GVF_SIN_A 0
|
||||
#endif
|
||||
|
||||
gvf_s_par gvf_sin_par = {GVF_SIN_KE, GVF_SIN_KN,
|
||||
GVF_SIN_ALPHA, GVF_SIN_W, GVF_SIN_OFF, GVF_SIN_A
|
||||
};
|
||||
|
||||
// Param array lenght
|
||||
static int gvf_p_len_wps = 0;
|
||||
|
||||
/** ------------------------------------------------------------------------ **/
|
||||
|
||||
// SINUSOIDAL (if w = 0 and off = 0, then we just have the straight line case)
|
||||
|
||||
bool nav_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_trajectory.p_len= 6 + gvf_p_len_wps;
|
||||
gvf_p_len_wps = 0;
|
||||
|
||||
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 nav_gvf_sin_wp1_wp2(uint8_t wp1, uint8_t wp2, float w, float off, float A)
|
||||
{
|
||||
w = 2 * M_PI * w;
|
||||
|
||||
gvf_trajectory.p[6] = wp1;
|
||||
gvf_trajectory.p[7] = wp2;
|
||||
gvf_p_len_wps = 2;
|
||||
|
||||
float x1 = WaypointX(wp1);
|
||||
float y1 = WaypointY(wp1);
|
||||
float x2 = WaypointX(wp2);
|
||||
float y2 = WaypointY(wp2);
|
||||
|
||||
float zx = x1 - x2;
|
||||
float zy = y1 - y2;
|
||||
|
||||
float alpha = atanf(zy / zx);
|
||||
|
||||
nav_gvf_sin_XY_alpha(x1, y1, alpha, w, off, A);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool nav_gvf_sin_wp_alpha(uint8_t wp, float alpha, float w, float off, float A)
|
||||
{
|
||||
w = 2 * M_PI * w;
|
||||
alpha = RadOfDeg(alpha);
|
||||
|
||||
gvf_trajectory.p[6] = wp;
|
||||
gvf_p_len_wps = 1;
|
||||
|
||||
float x = WaypointX(wp);
|
||||
float y = WaypointY(wp);
|
||||
|
||||
nav_gvf_sin_XY_alpha(x, y, alpha, w, off, A);
|
||||
|
||||
return true;
|
||||
}
|
||||
+7
-2
@@ -29,7 +29,7 @@
|
||||
#ifndef GVF_SIN_H
|
||||
#define GVF_SIN_H
|
||||
|
||||
#include "modules/guidance/gvf/gvf.h"
|
||||
#include "modules/guidance/trajectories/gvf_traj.h"
|
||||
|
||||
/** @typedef gvf_s_par
|
||||
* @brief Parameters for the GVF line trajectory
|
||||
@@ -49,8 +49,13 @@ typedef struct {
|
||||
float A;
|
||||
} gvf_s_par;
|
||||
|
||||
/** ------------------------------------------------------------------------ **/
|
||||
|
||||
extern gvf_s_par gvf_sin_par;
|
||||
|
||||
extern void gvf_sin_info(float *phi, struct gvf_grad *, struct gvf_Hess *);
|
||||
// Sinusoidal
|
||||
extern bool nav_gvf_sin_XY_alpha(float x, float y, float alpha, float w, float off, float A);
|
||||
extern bool nav_gvf_sin_wp1_wp2(uint8_t wp1, uint8_t wp2, float w, float off, float A);
|
||||
extern bool nav_gvf_sin_wp_alpha(uint8_t wp, float alpha, float w, float off, float A);
|
||||
|
||||
#endif // GVF_SIN_H
|
||||
@@ -27,6 +27,8 @@
|
||||
*
|
||||
*/
|
||||
|
||||
#include "nav_survey_polygon_gvf.h"
|
||||
|
||||
#include "firmwares/fixedwing/nav.h"
|
||||
#include "state.h"
|
||||
#include "autopilot.h"
|
||||
@@ -39,19 +41,25 @@
|
||||
|
||||
struct gvf_SurveyPolyAdv gvf_survey;
|
||||
|
||||
static void gvf_nav_points(struct FloatVect2 start, struct FloatVect2 end)
|
||||
/** ------------------------------------------------------------------------ **/
|
||||
|
||||
static void gvf_circle_direction(float rad)
|
||||
{
|
||||
gvf_segment_XY1_XY2(start.x, start.y, end.x, end.y);
|
||||
if (rad > 0) {
|
||||
gvf_set_direction(-1);
|
||||
} else {
|
||||
gvf_set_direction(1);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* intercept two lines and give back the point of intersection
|
||||
* @return FALSE if no intersection can be found or intersection does not lie between points a and b
|
||||
* else TRUE
|
||||
* Intercept two lines and give back the point of intersection
|
||||
* @return FALSE if no intersection can be found or intersection does not lie
|
||||
* between points a and b else TRUE
|
||||
* @param p returns intersection
|
||||
* @param x, y first line is defined by point x and y (goes through this points)
|
||||
* @param a1, a2, b1, b2 second line by coordinates a1/a2, b1/b2
|
||||
*/
|
||||
**/
|
||||
static bool gvf_intercept_two_lines(struct FloatVect2 *p, struct FloatVect2 x, struct FloatVect2 y, float a1, float a2,
|
||||
float b1, float b2)
|
||||
{
|
||||
@@ -70,11 +78,11 @@ static bool gvf_intercept_two_lines(struct FloatVect2 *p, struct FloatVect2 x, s
|
||||
}
|
||||
|
||||
/**
|
||||
* intersects a line with the polygon and gives back the two intersection points
|
||||
* @return TRUE if two intersection can be found, else FALSE
|
||||
* Intersects a line with the polygon and gives back the two intersection points
|
||||
* @return TRUE if two intersection can be found, else FALSE
|
||||
* @param x, y intersection points
|
||||
* @param a, b define the line to intersection
|
||||
*/
|
||||
**/
|
||||
static bool gvf_get_two_intersects(struct FloatVect2 *x, struct FloatVect2 *y, struct FloatVect2 a, struct FloatVect2 b)
|
||||
{
|
||||
int i, count = 0;
|
||||
@@ -122,8 +130,10 @@ static bool gvf_get_two_intersects(struct FloatVect2 *x, struct FloatVect2 *y, s
|
||||
return true;
|
||||
}
|
||||
|
||||
/** ------------------------------------------------------------------------ **/
|
||||
|
||||
/**
|
||||
* initializes the variables needed for the survey to start
|
||||
* Initializes the variables needed for the survey to start
|
||||
* @param first_wp the first Waypoint of the polygon
|
||||
* @param size the number of points that make up the polygon
|
||||
* @param angle angle in which to do the flyovers
|
||||
@@ -132,7 +142,7 @@ static bool gvf_get_two_intersects(struct FloatVect2 *x, struct FloatVect2 *y, s
|
||||
* @param min_rad minimal radius when navigating
|
||||
* @param altitude the altitude that must be reached before the flyover starts
|
||||
**/
|
||||
void gvf_nav_survey_polygon_setup(uint8_t first_wp, uint8_t size, float angle, float sweep_width, float shot_dist,
|
||||
void nav_gvf_survey_polygon_setup(uint8_t first_wp, uint8_t size, float angle, float sweep_width, float shot_dist,
|
||||
float min_rad, float altitude)
|
||||
{
|
||||
int i;
|
||||
@@ -229,28 +239,19 @@ void gvf_nav_survey_polygon_setup(uint8_t first_wp, uint8_t size, float angle, f
|
||||
}
|
||||
|
||||
/**
|
||||
* main navigation routine. This is called periodically evaluates the current
|
||||
* Main navigation routine. This is called periodically evaluates the current
|
||||
* Position and stage and navigates accordingly.
|
||||
* @returns True until the survey is finished
|
||||
*/
|
||||
void gvf_nav_direction_circle(float rad)
|
||||
{
|
||||
if (rad > 0) {
|
||||
gvf_set_direction(-1);
|
||||
} else {
|
||||
gvf_set_direction(1);
|
||||
}
|
||||
}
|
||||
|
||||
bool gvf_nav_survey_polygon_run(void)
|
||||
**/
|
||||
bool nav_gvf_survey_polygon_run(void)
|
||||
{
|
||||
NavVerticalAutoThrottleMode(0.0);
|
||||
NavVerticalAltitudeMode(gvf_survey.psa_altitude, 0.0);
|
||||
|
||||
//entry circle around entry-center until the desired altitude is reached
|
||||
if (gvf_survey.stage == gENTRY) {
|
||||
gvf_nav_direction_circle(gvf_survey.psa_min_rad);
|
||||
gvf_ellipse_XY(gvf_survey.entry_center.x, gvf_survey.entry_center.y, gvf_survey.psa_min_rad, gvf_survey.psa_min_rad, 0);
|
||||
gvf_circle_direction(gvf_survey.psa_min_rad);
|
||||
nav_gvf_ellipse_XY(gvf_survey.entry_center.x, gvf_survey.entry_center.y, gvf_survey.psa_min_rad, gvf_survey.psa_min_rad, 0);
|
||||
if (NavCourseCloseTo(gvf_survey.segment_angle)
|
||||
&& nav_approaching_xy(gvf_survey.seg_start.x, gvf_survey.seg_start.y, last_x, last_y, CARROT)
|
||||
&& fabs(stateGetPositionUtm_f()->alt - gvf_survey.psa_altitude) <= 20) {
|
||||
@@ -264,7 +265,7 @@ bool gvf_nav_survey_polygon_run(void)
|
||||
}
|
||||
//fly the segment until seg_end is reached
|
||||
if (gvf_survey.stage == gSEG) {
|
||||
gvf_nav_points(gvf_survey.seg_start, gvf_survey.seg_end);
|
||||
nav_gvf_segment_points(gvf_survey.seg_start, gvf_survey.seg_end);
|
||||
//calculate all needed points for the next flyover
|
||||
if (nav_approaching_xy(gvf_survey.seg_end.x, gvf_survey.seg_end.y, gvf_survey.seg_start.x, gvf_survey.seg_start.y, 0)) {
|
||||
#ifdef DIGITAL_CAM
|
||||
@@ -295,15 +296,15 @@ bool gvf_nav_survey_polygon_run(void)
|
||||
}
|
||||
//turn from stage to return
|
||||
else if (gvf_survey.stage == gTURN1) {
|
||||
gvf_nav_direction_circle(gvf_survey.psa_min_rad);
|
||||
gvf_ellipse_XY(gvf_survey.seg_center1.x, gvf_survey.seg_center1.y, gvf_survey.psa_min_rad, gvf_survey.psa_min_rad, 0);
|
||||
gvf_circle_direction(gvf_survey.psa_min_rad);
|
||||
nav_gvf_ellipse_XY(gvf_survey.seg_center1.x, gvf_survey.seg_center1.y, gvf_survey.psa_min_rad, gvf_survey.psa_min_rad, 0);
|
||||
if (NavCourseCloseTo(gvf_survey.return_angle)) {
|
||||
gvf_survey.stage = gRET;
|
||||
nav_init_stage();
|
||||
}
|
||||
//return
|
||||
} else if (gvf_survey.stage == gRET) {
|
||||
gvf_nav_points(gvf_survey.ret_start, gvf_survey.ret_end);
|
||||
nav_gvf_segment_points(gvf_survey.ret_start, gvf_survey.ret_end);
|
||||
if (nav_approaching_xy(gvf_survey.ret_end.x, gvf_survey.ret_end.y, gvf_survey.ret_start.x, gvf_survey.ret_start.y, 0)) {
|
||||
gvf_survey.stage = gTURN2;
|
||||
nav_init_stage();
|
||||
@@ -311,8 +312,8 @@ bool gvf_nav_survey_polygon_run(void)
|
||||
//turn from return to stage
|
||||
} else if (gvf_survey.stage == gTURN2) {
|
||||
float rad_sur = (2 * gvf_survey.psa_min_rad + gvf_survey.psa_sweep_width) * 0.5;
|
||||
gvf_nav_direction_circle(rad_sur);
|
||||
gvf_ellipse_XY(gvf_survey.seg_center2.x, gvf_survey.seg_center2.y, rad_sur, rad_sur, 0);
|
||||
gvf_circle_direction(rad_sur);
|
||||
nav_gvf_ellipse_XY(gvf_survey.seg_center2.x, gvf_survey.seg_center2.y, rad_sur, rad_sur, 0);
|
||||
if (NavCourseCloseTo(gvf_survey.segment_angle)) {
|
||||
gvf_survey.stage = gSEG;
|
||||
nav_init_stage();
|
||||
|
||||
@@ -27,8 +27,8 @@
|
||||
*
|
||||
*/
|
||||
|
||||
#ifndef NAV_SURVEY_POLYGON_GVF_H
|
||||
#define NAV_SURVEY_POLYGON_GVF_H
|
||||
#ifndef SURVEY_POLYGON_GVF_H
|
||||
#define SURVEY_POLYGON_GVF_H
|
||||
|
||||
#include "std.h"
|
||||
#include "math/pprz_algebra_float.h"
|
||||
@@ -83,11 +83,11 @@ struct gvf_SurveyPolyAdv {
|
||||
struct FloatVect2 ret_end;
|
||||
};
|
||||
|
||||
// external setting
|
||||
extern void gvf_nav_survey_polygon_setup(uint8_t first_wp, uint8_t size, float angle, float sweep_width, float shot_dist,
|
||||
/** ------------------------------------------------------------------------ **/
|
||||
|
||||
// Polygon survey
|
||||
extern void nav_gvf_survey_polygon_setup(uint8_t first_wp, uint8_t size, float angle, float sweep_width, float shot_dist,
|
||||
float min_rad, float altitude);
|
||||
extern bool nav_gvf_survey_polygon_run(void);
|
||||
|
||||
void gvf_nav_direction_circle(float rad);
|
||||
extern bool gvf_nav_survey_polygon_run(void);
|
||||
|
||||
#endif
|
||||
#endif // SURVEY_POLYGON_GVF_H
|
||||
|
||||
@@ -1,85 +0,0 @@
|
||||
/*
|
||||
* Copyright (C) 2016 Hector Garcia de Marina
|
||||
*
|
||||
* This file is part of paparazzi.
|
||||
*
|
||||
* paparazzi is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 2, or (at your option)
|
||||
* any later version.
|
||||
*
|
||||
* paparazzi is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with paparazzi; see the file COPYING. If not, write to
|
||||
* the Free Software Foundation, 59 Temple Place - Suite 330,
|
||||
* Boston, MA 02111-1307, USA.
|
||||
*
|
||||
*/
|
||||
|
||||
/** \file gvf_line.c
|
||||
*
|
||||
* Guidance algorithm based on vector fields
|
||||
* 2D straight line trajectory
|
||||
*/
|
||||
|
||||
|
||||
#include "modules/nav/common_nav.h"
|
||||
#include "gvf_line.h"
|
||||
#include "generated/airframe.h"
|
||||
|
||||
/*! Gain ke for the line trajectory*/
|
||||
#ifndef GVF_LINE_KE
|
||||
#define GVF_LINE_KE 1
|
||||
#endif
|
||||
|
||||
/*! Gain kn for the line trajectory*/
|
||||
#ifndef GVF_LINE_KN
|
||||
#define GVF_LINE_KN 1
|
||||
#endif
|
||||
|
||||
/*! Default heading in degrees for a trajectory called from gvf_line_**_HEADING */
|
||||
#ifndef GVF_LINE_HEADING
|
||||
#define GVF_LINE_HEADING 0
|
||||
#endif
|
||||
|
||||
/*! In case of tracking a segment, how much distance in meters will go the vehicle beyond the point x1,y1 before turning back */
|
||||
#ifndef GVF_SEGMENT_D1
|
||||
#define GVF_SEGMENT_D1 0
|
||||
#endif
|
||||
|
||||
/*! In case of tracking a segment, how much distance in meters will go the vehicle beyond the point x2,y2 before turning back */
|
||||
#ifndef GVF_SEGMENT_D2
|
||||
#define GVF_SEGMENT_D2 0
|
||||
#endif
|
||||
|
||||
gvf_li_par gvf_line_par = {GVF_LINE_KE, GVF_LINE_KN, GVF_LINE_HEADING};
|
||||
gvf_seg_par gvf_segment_par = {GVF_SEGMENT_D1, GVF_SEGMENT_D2};
|
||||
|
||||
void gvf_line_info(float *phi, struct gvf_grad *grad,
|
||||
struct gvf_Hess *hess)
|
||||
{
|
||||
|
||||
struct EnuCoor_f *p = stateGetPositionEnu_f();
|
||||
float px = p->x;
|
||||
float py = p->y;
|
||||
float a = gvf_trajectory.p[0];
|
||||
float b = gvf_trajectory.p[1];
|
||||
float alpha = gvf_trajectory.p[2];
|
||||
|
||||
// Phi(x,y)
|
||||
*phi = -(px - a) * cosf(alpha) + (py - b) * sinf(alpha);
|
||||
|
||||
// grad Phi
|
||||
grad->nx = -cosf(alpha);
|
||||
grad->ny = sinf(alpha);
|
||||
|
||||
// Hessian Phi
|
||||
hess->H11 = 0;
|
||||
hess->H12 = 0;
|
||||
hess->H21 = 0;
|
||||
hess->H22 = 0;
|
||||
}
|
||||
@@ -18,7 +18,107 @@
|
||||
* <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include "./gvf_common.h"
|
||||
#include "gvf_common.h"
|
||||
|
||||
gvf_common_omega gvf_c_omega;
|
||||
gvf_common_params gvf_c_info;
|
||||
// Get MODULE_H from loaded modules
|
||||
#include "generated/modules.h"
|
||||
|
||||
/** ------------------------------------------------------------------------ **/
|
||||
|
||||
gvf_common_ctrl gvf_c_ctrl;
|
||||
gvf_common_info gvf_c_info;
|
||||
gvf_common_params gvf_c_params = {1,1};
|
||||
|
||||
// State
|
||||
gvf_common_state gvf_c_state;
|
||||
|
||||
/** ------------------------------------------------------------------------ **/
|
||||
|
||||
void gvf_low_level_getState(void)
|
||||
{
|
||||
|
||||
gvf_c_state.px = stateGetPositionEnu_f()->x;
|
||||
gvf_c_state.py = stateGetPositionEnu_f()->y;
|
||||
|
||||
#if defined(FIXEDWING_FIRMWARE)
|
||||
float ground_speed = stateGetHorizontalSpeedNorm_f();
|
||||
gvf_c_state.course = stateGetHorizontalSpeedDir_f();
|
||||
gvf_c_state.px_dot = ground_speed * sinf(gvf_c_state.course);
|
||||
gvf_c_state.py_dot = ground_speed * cosf(gvf_c_state.course);
|
||||
|
||||
#elif defined(ROVER_FIRMWARE) || defined(ROTORCRAFT_FIRMWARE)
|
||||
// We assume that the course and psi
|
||||
// of the rover (steering wheel) are the same
|
||||
gvf_c_state.course = stateGetNedToBodyEulers_f()->psi;
|
||||
gvf_c_state.px_dot = stateGetSpeedEnu_f()->x;
|
||||
gvf_c_state.py_dot = stateGetSpeedEnu_f()->y;
|
||||
#endif
|
||||
}
|
||||
|
||||
void gvf_low_level_control_2D(float omega)
|
||||
{
|
||||
|
||||
#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 =
|
||||
- gvf_c_params.k_roll * atanf(omega * ground_speed / GVF_GRAVITY / cosf(att->theta));
|
||||
BoundAbs(h_ctl_roll_setpoint, h_ctl_roll_max_setpoint);
|
||||
}
|
||||
#endif
|
||||
|
||||
gvf_c_ctrl.omega = omega;
|
||||
}
|
||||
|
||||
void gvf_low_level_control_3D(float heading_rate, float climbing_rate)
|
||||
{
|
||||
#if defined(FIXEDWING_FIRMWARE)
|
||||
if (autopilot_get_mode() == AP_MODE_AUTO2) {
|
||||
// Vertical Z coordinate
|
||||
v_ctl_mode = V_CTL_MODE_AUTO_CLIMB;
|
||||
v_ctl_speed_mode = V_CTL_SPEED_THROTTLE;
|
||||
|
||||
v_ctl_climb_setpoint = gvf_c_params.k_climb * climbing_rate; // Setting point for vertical speed
|
||||
|
||||
// Lateral XY coordinates
|
||||
lateral_mode = LATERAL_MODE_ROLL;
|
||||
|
||||
struct FloatEulers *att = stateGetNedToBodyEulers_f();
|
||||
float ground_speed = stateGetHorizontalSpeedNorm_f();
|
||||
|
||||
h_ctl_roll_setpoint =
|
||||
-gvf_c_params.k_roll * atanf(heading_rate * ground_speed / GVF_GRAVITY / cosf(att->theta));
|
||||
BoundAbs(h_ctl_roll_setpoint, h_ctl_roll_max_setpoint); // Setting point for roll angle
|
||||
}
|
||||
#endif
|
||||
|
||||
gvf_low_level_control_2D(heading_rate);
|
||||
|
||||
(void)(climbing_rate); // Avoid unused parameter warning
|
||||
}
|
||||
|
||||
bool gvf_nav_approaching(float wp_x, float wp_y, float from_x, float from_y, float t)
|
||||
{
|
||||
|
||||
#if defined(FIXEDWING_FIRMWARE)
|
||||
return nav_approaching_xy(wp_x, wp_y, from_x, from_y, t);
|
||||
|
||||
#else // FIXEDWING & ROVER FIRMWARE
|
||||
struct EnuCoor_f wp, from;
|
||||
|
||||
wp.x = wp_x;
|
||||
wp.y = wp_y;
|
||||
from.x = from_x;
|
||||
from.y = from_y;
|
||||
|
||||
return nav.nav_approaching(&wp, &from, t);
|
||||
#endif
|
||||
|
||||
// Avoid unused parameter warning
|
||||
}
|
||||
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user