Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-22 12:28:03 +08:00
Code Issues Actions 25 Packages Projects Releases Wiki Activity

Fix some errors, added two modules and improved OSD files (#2614)

Browse Source
This commit is contained in:
hendrixgr
2020-11-15 23:22:19 +02:00
committed by GitHub
parent 03a937c2d8
commit ce5d9680e6
18 changed files with 2037 additions and 353 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/matek_f405_wing/Easyglider_matek_f405_autoload.xml
+76 -31
View File
@@ -51,13 +51,39 @@
<module name="navigation"/>
<module name="ahrs" type="float_dcm"/>
<module name="ins" type="alt_float">
<configure name="USE_MAGNETOMETER" value="0"/>
<define name="AHRS_USE_GPS_HEADING" value="1"/>
<configure name="USE_MAGNETOMETER" value="true"/>
<define name="AHRS_USE_GPS_HEADING" value="true"/>
<define name="AHRS_GRAVITY_UPDATE_COORDINATED_TURN"/>
</module>
<module name="nav_line"/>
<module name="gps_ubx_ucenter"/>
<module name="mag" type="hmc58xx">
<!-- If you mag somehow does not work, Enable this line or increase the delay if it still does not work -->
<!--<define name="HMC58XX_STARTUP_DELAY" value="1.4"/>-->
<configure name="MAG_HMC58XX_I2C_DEV" value="i2c2"/>
<!-- temporary for debugging external magneto and setup orientation sign and Body to Magneto angles-->
<!-- <define name="MODULE_HMC58XX_SYNC_SEND" value="true"/> -->
<!-- When all calibration is done and everything works, set the below definition to true -->
<define name="MODULE_HMC58XX_UPDATE_AHRS" value="true"/>
<define name="HMC58XX_CHAN_X" value="0"/>
<define name="HMC58XX_CHAN_Y" value="1"/>
<define name="HMC58XX_CHAN_Z" value="2"/>
<define name="HMC58XX_CHAN_X_SIGN" value="+"/>
<define name="HMC58XX_CHAN_Y_SIGN" value="+"/>
<define name="HMC58XX_CHAN_Z_SIGN" value="+"/>
</module>
<module name="uav_recovery">
<define name="USE_PARACHUTE" value="true"/>
<define name="PARACHUTE_SERVO_CHANNEL" value="PARACHUTE" />
<define name="AIRBORNE_WIND_CORRECTION" value="1.0" />
<define name="PARACHUTE_WIND_CORRECTION" value="0.5" />
<define name="PARACHUTE_DESCENT_RATE" value="3.0" />
<define name="PARACHUTE_LINE_LENGTH" value="2.0" />
<define name="FIXED_WIND_DIRECTION_FOR_TESTING" value="330.0" />
<define name="FIXED_WIND_SPEED_FOR_TESTING" value="10.0" />
</module>
</firmware>
<firmware name="setup">
@@ -69,12 +95,13 @@
<servos>
<servo name="MOTOR" no="0" min="1100" neutral="1100" max="2000"/>
<servo name="AILERON" no="2" min="2000" neutral="1500" max="1000"/>
<servo name="ELEVATOR" no="3" min="2000" neutral="1500" max="1000"/>
<servo name="PARACHUTE" no="1" min="950" neutral="950" max="2050"/>
<servo name="AILERON_L" no="2" min="1000" neutral="1500" max="2000"/>
<servo name="ELEVATOR" no="3" min="1000" neutral="1500" max="2000"/>
<servo name="RUDDER" no="4" min="1000" neutral="1500" max="2000"/>
<servo name="CAM_PAN" no="5" min="943" neutral="1520" max="2056"/>
<servo name="CAM_TILT" no="6" min="996" neutral="1221" max="2004"/>
<servo name="PARACHUTE" no="7" min="950" neutral="950" max="2050"/>
<servo name="AILERON_R" no="5" min="1100" neutral="1100" max="2000"/>
<servo name="CAM_PAN" no="6" min="943" neutral="1520" max="2056"/>
<servo name="CAM_TILT" no="7" min="996" neutral="1221" max="2004"/>
</servos>
<commands>
@@ -82,6 +109,7 @@
<axis name="PITCH" failsafe_value="0"/>
<axis name="THROTTLE" failsafe_value="0"/>
<axis name="YAW" failsafe_value="2000"/>
<axis name="FLAPS" failsafe_value="0"/>
<axis name="CAM_PAN" failsafe_value="0"/>
<axis name="CAM_TILT" failsafe_value="0"/>
<axis name="PARACHUTE" failsafe_value="0"/>
@@ -92,21 +120,27 @@
<set command="PITCH" value="@PITCH"/>
<set command="THROTTLE" value="@THROTTLE"/>
<set command="YAW" value="@YAW"/>
<set command="FLAPS" value="@AUX1"/>
<set command="PARACHUTE" value="@AUX3"/>
</rc_commands>
<section name="MIXER">
<define name="AILERON_DIFF" value="0.66"/>
<define name="COMBI_SWITCH" value="0.3"/>
<define name="COMBI_SWITCH" value="0.5"/>
</section>
<command_laws>
<let var="roll" value="@ROLL"/>
<let var="flaps" value="@FLAPS"/>
<set servo="MOTOR" value="@THROTTLE"/>
<set servo="AILERON" value="@ROLL"/>
<set servo="PARACHUTE" value="@PARACHUTE"/>
<set servo="AILERON_L" value="$roll + $flaps"/>
<set servo="AILERON_R" value="$roll - $flaps"/>
<set servo="ELEVATOR" value="@PITCH"/>
<set servo="RUDDER" value="@YAW + @ROLL*COMBI_SWITCH"/>
<set servo="CAM_PAN" value="@CAM_PAN"/>
<set servo="CAM_TILT" value="@CAM_TILT"/>
<set servo="PARACHUTE" value="@PARACHUTE"/>
</command_laws>
<!-- EMPTY "auto_rc_commands" block means NO RC RUDDER CONTROL IN AUTO2 AND AUTO1 -->
@@ -126,27 +160,28 @@
Normalized Local magnetic field obtained from http://www.ngdc.noaa.gov/geomag-web/#igrfwmm
Magnetic field intensity (North-East-vertical) / total field strength
H_x = INTENSITY_x / Total Magnetic Field
Total Magnetic Field = (intensityEAST^2+intensityNORTH^2+intensityVERTICAL^2)
Calculated for LAGONISI ATTIKI 21 AUGUST 2020
Total Magnetic Field = (intensityEAST^2+intensityNORTH^2+intensityVERTICAL^2)
Calculated for LAGONISI ATTIKI 23 October 2020
-->
<section name="AHRS" prefix="AHRS_" >
<define name="MAG_DECLINATION" value="5.3" />
<define name="H_X" value="(25242.1/47035.5)" />
<define name="H_Y" value="(2224.0/47035.5)" />
<define name="H_Z" value="(39626.2/47035.5) " />
<define name="MAG_DECLINATION" value="5.3" unit="deg" />
<define name="H_X" value="(25243.2/47047.8)" />
<define name="H_Y" value="(2232.5/47047.8)" />
<define name="H_Z" value="(39639.5/47047.8) " />
</section>
<section name="IMU" prefix="IMU_">
<define name="GYRO_P_SIGN" value="1"/>
<define name="GYRO_Q_SIGN" value="-1"/>
<define name="GYRO_R_SIGN" value="-1"/>
<!-- Calibration Neutral -->
<define name="GYRO_P_NEUTRAL" value="0"/>
<define name="GYRO_Q_NEUTRAL" value="0"/>
<define name="GYRO_R_NEUTRAL" value="0"/>
<!-- SENS = 14.375 LSB/(deg/sec) * 57.6 deg/rad = 828 LSB/rad/sec / 12bit FRAC: 4096 / 828 -->
<define name="GYRO_P_SENS" value="4.947" integer="16"/>
<define name="GYRO_Q_SENS" value="4.947" integer="16"/>
<define name="GYRO_R_SENS" value="4.947" integer="16"/>
<define name="GYRO_P_Q" value="0."/>
<define name="GYRO_P_R" value="0"/>
<define name="GYRO_Q_P" value="0."/>
@@ -154,23 +189,31 @@
<define name="GYRO_R_P" value="0."/>
<define name="GYRO_R_Q" value="0."/>
<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"/>
<!-- Found with calibrate.py script and visual observation of the RAW values -->
<define name="ACCEL_X_NEUTRAL" value="63"/>
<define name="ACCEL_Y_NEUTRAL" value="-55"/>
<define name="ACCEL_Z_NEUTRAL" value="-363"/>
<define name="ACCEL_X_SENS" value="4.92325848105" integer="16"/>
<define name="ACCEL_Y_SENS" value="4.91672766019" integer="16"/>
<define name="ACCEL_Z_SENS" value="4.85646198137" integer="16"/>
<define name="MAG_X_SIGN" value="1"/>
<define name="MAG_Y_SIGN" value="1"/>
<define name="MAG_Z_SIGN" value="1"/>
<!-- Found with calibrate.py script -->
<define name="MAG_X_NEUTRAL" value="127"/>
<define name="MAG_Y_NEUTRAL" value="-72"/>
<define name="MAG_Z_NEUTRAL" value="-24"/>
<define name="MAG_X_SENS" value="5.62922068812" integer="16"/>
<define name="MAG_Y_SENS" value="5.80146852641" integer="16"/>
<define name="MAG_Z_SENS" value="6.19489790035" integer="16"/>
<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"/> <!-- YAW -->
</section>
<section name="INS" prefix="INS_">
<define name="ROLL_NEUTRAL_DEFAULT" value="RadOfDeg(0)" unit="radians"/>
<define name="PITCH_NEUTRAL_DEFAULT" value="RadOfDeg(0)" unit="radians"/>
@@ -243,9 +286,11 @@
<section name="BAT">
<!-- FOR USE WITH A CURRENT SENSOR -->
<define name="BAT_CAPACITY" value="5000" unit="mah"/>
<define name="LOITER_BAT_CURRENT" value="10" unit="Amps"/>
<define name="CATASTROPHIC_BAT_LEVEL" value="7.5" unit="V"/>
<define name="CRITIC_BAT_LEVEL" value="8.5" unit="V"/>
<define name="LOW_BAT_LEVEL" value="9.3" unit="V"/>
<define name="CRITIC_BAT_LEVEL" value="9." unit="V"/>
<define name="LOW_BAT_LEVEL" value="10." unit="V"/>
<define name="MAX_BAT_LEVEL" value="12.6" unit="V"/>
</section>
@@ -263,16 +308,16 @@
<define name="KILL_MODE_DISTANCE" value="(1.5*MAX_DIST_FROM_HOME)"/>
<define name="CONTROL_FREQUENCY" value="60" unit="Hz"/>
<define name="XBEE_INIT" value="\"ATPL2\rATRN1\rATTT80\r\""/>
<define name="NO_XBEE_API_INIT" value="TRUE"/>
<define name="NO_XBEE_API_INIT" value="true"/>
<define name="TRIGGER_DELAY" value="1."/>
<define name="DEFAULT_CIRCLE_RADIUS" value="60."/>
<!-- MIN_CIRCLE_RADIUS used and needed for spiral navigation function and panic autolanding turns-->
<define name="MIN_CIRCLE_RADIUS" value="40."/>
<!--UNLOCKED_HOME_MODE if set to TRUE means that HOME mode does not get stuck.
<!--UNLOCKED_HOME_MODE if set to true means that HOME mode does not get stuck.
If not set before when you would enter home mode you had to flip a bit via the GCS to get out. -->
<define name="UNLOCKED_HOME_MODE" value="TRUE"/>
<define name="UNLOCKED_HOME_MODE" value="true"/>
<!-- RC_LOST_MODE means that if your RC Transmitter signal is not received anymore in the autopilot, e.g. you switch it off
or fly a long range mission you define the wanted mode behaviour here.
@@ -290,7 +335,7 @@
<define name="CLIMB_NAV_RATIO" value="0.8"/> <!-- Percent Navigation for Altitude Error Equal to Start Altitude -->
<define name="DESCENT_NAV_RATIO" value="1.0"/>
</section>
<!--
<section name="NAV">
<define name="NAV_PITCH" value="0."/>
<define name="NAV_GLIDE_PITCH_TRIM" value="0"/>
@@ -303,7 +348,7 @@
<define name="INTERCEPT_AF_TOD" value="10"/>
<define name="TARGET_SPEED" value="13"/>
</section>
-->
<section name="FAILSAFE" prefix="FAILSAFE_">
<define name="DELAY_WITHOUT_GPS" value="1" unit="s"/>
<define name="DEFAULT_THROTTLE" value="0.8" unit="%"/>
conf/airframes/matek_f405_wing/Easyglider_matek_f405_wing.xml
+90 -43
View File
@@ -27,11 +27,9 @@
<define name="USE_ADC_4"/>
<define name="AHRS_TRIGGERED_ATTITUDE_LOOP"/>
<define name="USE_AHRS_GPS_ACCELERATIONS"/>
<define name="MAGNETOMETER_AVAILABLE" value="0" />
<define name="BAROMETER_AVAILABLE" value="0" />
<module name="telemetry" type="transparent">
<configure name="MODEM_BAUD" value="B57600"/>
<configure name="MODEM_BAUD" value="B19200"/>
<configure name="MODEM_PORT" value="UART6 "/>
</module>
<module name="gps" type="ublox" >
@@ -47,28 +45,53 @@
<module name="imu" type="mpu6000" />
<module name="ahrs" type="float_dcm"/>
<module name="ins" type="alt_float">
<configure name="USE_MAGNETOMETER" value="0"/>
<define name="AHRS_USE_GPS_HEADING" value="1"/>
<configure name="USE_MAGNETOMETER" value="true"/>
<define name="AHRS_USE_GPS_HEADING" value="true"/>
<define name="AHRS_GRAVITY_UPDATE_COORDINATED_TURN"/>
</module>
<module name="spi" type="master"/>
<module name="nav_line"/>
<module name="gps_ubx_ucenter"/>
<module name="nav_line"/>
<module name="baro_bmp280_i2c">
<define name="BMP280_SYNC_SEND"/>
<!-- <define name="BMP280_SYNC_SEND"/> -->
<configure name="BMP280_I2C_DEV" value="i2c1" />
<configure name="BMP280_SLAVE_ADDR" value="BMP280_I2C_ADDR" />
</module>
<module name="osd_max7456">
<configure name="MAX7456_SPI_DEV" value="spi2" />
<configure name="MAX7456_SLAVE_IDX" value="SPI_SLAVE1" />
<define name="USE_MATEK_TYPE_OSD_CHIP" value="1" />
<define name="USE_PAL_FOR_OSD_VIDEO" value="1" />
<define name="OSD_USE_BARO_ALTITUDE" value="0" />
<define name="BARO_ALTITUDE_VAR" value="baro_alt" /> <!-- if non defined the default var is ''baro_alt'' -->
<define name="OSD_USE_MAG_COMPASS" value="0" />
<define name="USE_MATEK_TYPE_OSD_CHIP" value="true" />
<define name="USE_PAL_FOR_OSD_VIDEO" value="true" />
<define name="BARO_ALTITUDE_VAR" value="baro_alt" />
</module>
<module name="mag" type="hmc58xx">
<!-- If you mag somehow does not work, Enable this line or increase the delay if it still does not work -->
<!--<define name="HMC58XX_STARTUP_DELAY" value="1.4"/>-->
<configure name="MAG_HMC58XX_I2C_DEV" value="i2c2"/>
<!-- temporary for debugging external magneto and setup orientation sign and Body to Magneto angles-->
<!-- <define name="MODULE_HMC58XX_SYNC_SEND" value="true"/> -->
<!-- When all calibration is done and everything works, set the below definition to true -->
<define name="MODULE_HMC58XX_UPDATE_AHRS" value="true"/>
<define name="HMC58XX_CHAN_X" value="0"/>
<define name="HMC58XX_CHAN_Y" value="1"/>
<define name="HMC58XX_CHAN_Z" value="2"/>
<define name="HMC58XX_CHAN_X_SIGN" value="+"/>
<define name="HMC58XX_CHAN_Y_SIGN" value="+"/>
<define name="HMC58XX_CHAN_Z_SIGN" value="+"/>
</module>
<module name="uav_recovery">
<define name="USE_PARACHUTE" value="true"/>
<define name="PARACHUTE_SERVO_CHANNEL" value="PARACHUTE" />
<define name="AIRBORNE_WIND_CORRECTION" value="1.0" />
<define name="PARACHUTE_WIND_CORRECTION" value="0.5" />
<define name="PARACHUTE_DESCENT_RATE" value="3.0" />
<define name="PARACHUTE_LINE_LENGTH" value="2.0" />
<define name="FIXED_WIND_DIRECTION_FOR_TESTING" value="330.0" />
<define name="FIXED_WIND_SPEED_FOR_TESTING" value="10.0" />
</module>
<module name="fixed_wing_cam_v1"/>
</firmware>
<firmware name="setup">
@@ -80,12 +103,13 @@
<servos>
<servo name="MOTOR" no="0" min="1100" neutral="1100" max="2000"/>
<servo name="AILERON" no="2" min="2000" neutral="1500" max="1000"/>
<servo name="ELEVATOR" no="3" min="2000" neutral="1500" max="1000"/>
<servo name="PARACHUTE" no="1" min="950" neutral="950" max="2050"/>
<servo name="AILERON_L" no="2" min="1000" neutral="1500" max="2000"/>
<servo name="ELEVATOR" no="3" min="1000" neutral="1500" max="2000"/>
<servo name="RUDDER" no="4" min="1000" neutral="1500" max="2000"/>
<servo name="CAM_PAN" no="5" min="943" neutral="1520" max="2056"/>
<servo name="CAM_TILT" no="6" min="996" neutral="1221" max="2004"/>
<servo name="PARACHUTE" no="7" min="950" neutral="950" max="2050"/>
<servo name="AILERON_R" no="5" min="1100" neutral="1100" max="2000"/>
<servo name="CAM_PAN" no="6" min="943" neutral="1520" max="2056"/>
<servo name="CAM_TILT" no="7" min="996" neutral="1221" max="2004"/>
</servos>
<commands>
@@ -93,6 +117,7 @@
<axis name="PITCH" failsafe_value="0"/>
<axis name="THROTTLE" failsafe_value="0"/>
<axis name="YAW" failsafe_value="2000"/>
<axis name="FLAPS" failsafe_value="0"/>
<axis name="CAM_PAN" failsafe_value="0"/>
<axis name="CAM_TILT" failsafe_value="0"/>
<axis name="PARACHUTE" failsafe_value="0"/>
@@ -103,22 +128,27 @@
<set command="PITCH" value="@PITCH"/>
<set command="THROTTLE" value="@THROTTLE"/>
<set command="YAW" value="@YAW"/>
<!-- <set command="PARACHUTE" value="@PARACHUTE"/> -->
<set command="FLAPS" value="@AUX1"/>
<set command="PARACHUTE" value="@AUX3"/>
</rc_commands>
<section name="MIXER">
<define name="AILERON_DIFF" value="0.66"/>
<define name="COMBI_SWITCH" value="0.3"/>
<define name="COMBI_SWITCH" value="0.5"/>
</section>
<command_laws>
<let var="roll" value="@ROLL"/>
<let var="flaps" value="@FLAPS"/>
<set servo="MOTOR" value="@THROTTLE"/>
<set servo="AILERON" value="@ROLL"/>
<set servo="PARACHUTE" value="@PARACHUTE"/>
<set servo="AILERON_L" value="$roll + $flaps"/>
<set servo="AILERON_R" value="$roll - $flaps"/>
<set servo="ELEVATOR" value="@PITCH"/>
<set servo="RUDDER" value="@YAW + @ROLL*COMBI_SWITCH"/>
<set servo="CAM_PAN" value="@CAM_PAN"/>
<set servo="CAM_TILT" value="@CAM_TILT"/>
<set servo="PARACHUTE" value="@PARACHUTE"/>
</command_laws>
<!-- EMPTY "auto_rc_commands" block means NO RC RUDDER CONTROL IN AUTO2 AND AUTO1 -->
@@ -128,7 +158,6 @@
-->
</auto_rc_commands>
<ap_only_commands>
<copy command="CAM_PAN"/>
<copy command="CAM_TILT"/>
@@ -139,26 +168,27 @@
Magnetic field intensity (North-East-vertical) / total field strength
H_x = INTENSITY_x / Total Magnetic Field
Total Magnetic Field = (intensityEAST^2+intensityNORTH^2+intensityVERTICAL^2)
Calculated for LAGONISI ATTIKI 21 AUGUST 2020
Calculated for LAGONISI ATTIKI 23 October 2020
-->
<section name="AHRS" prefix="AHRS_" >
<define name="MAG_DECLINATION" value="5.3" />
<define name="H_X" value="(25242.1/47035.5)" />
<define name="H_Y" value="(2224.0/47035.5)" />
<define name="H_Z" value="(39626.2/47035.5) " />
<define name="MAG_DECLINATION" value="5.3" unit="deg" />
<define name="H_X" value="(25243.2/47047.8)" />
<define name="H_Y" value="(2232.5/47047.8)" />
<define name="H_Z" value="(39639.5/47047.8) " />
</section>
<section name="IMU" prefix="IMU_">
<define name="GYRO_P_SIGN" value="1"/>
<define name="GYRO_Q_SIGN" value="-1"/>
<define name="GYRO_R_SIGN" value="-1"/>
<!-- Calibration Neutral -->
<define name="GYRO_P_NEUTRAL" value="0"/>
<define name="GYRO_Q_NEUTRAL" value="0"/>
<define name="GYRO_R_NEUTRAL" value="0"/>
<!-- SENS = 14.375 LSB/(deg/sec) * 57.6 deg/rad = 828 LSB/rad/sec / 12bit FRAC: 4096 / 828 -->
<define name="GYRO_P_SENS" value="4.947" integer="16"/>
<define name="GYRO_Q_SENS" value="4.947" integer="16"/>
<define name="GYRO_R_SENS" value="4.947" integer="16"/>
<define name="GYRO_P_Q" value="0."/>
<define name="GYRO_P_R" value="0"/>
<define name="GYRO_Q_P" value="0."/>
@@ -166,22 +196,37 @@
<define name="GYRO_R_P" value="0."/>
<define name="GYRO_R_Q" value="0."/>
<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"/>
<!-- Found with calibrate.py script -->
<!-- SENS = 2048 LSB/g / 9.81 ms2/g = 208.766564729 LSB/ms2 / 10bit FRAC: 1024 / 208.7665 -->
<define name="ACCEL_X_NEUTRAL" value="63"/>
<define name="ACCEL_Y_NEUTRAL" value="-55"/>
<define name="ACCEL_Z_NEUTRAL" value="-363"/>
<define name="ACCEL_X_SENS" value="4.92325848105" integer="16"/>
<define name="ACCEL_Y_SENS" value="4.91672766019" integer="16"/>
<define name="ACCEL_Z_SENS" value="4.85646198137" integer="16"/>
<define name="MAG_X_SIGN" value="1"/>
<define name="MAG_Y_SIGN" value="1"/>
<define name="MAG_Z_SIGN" value="1"/>
<!-- Found with calibrate.py script -->
<define name="MAG_X_NEUTRAL" value="127"/>
<define name="MAG_Y_NEUTRAL" value="-72"/>
<define name="MAG_Z_NEUTRAL" value="-24"/>
<define name="MAG_X_SENS" value="5.62922068812" integer="16"/>
<define name="MAG_Y_SENS" value="5.80146852641" integer="16"/>
<define name="MAG_Z_SENS" value="6.19489790035" integer="16"/>
<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"/> <!-- YAW -->
</section>
<section name="INS" prefix="INS_">
<define name="ROLL_NEUTRAL_DEFAULT" value="RadOfDeg(0)" unit="radians"/>
<define name="PITCH_NEUTRAL_DEFAULT" value="RadOfDeg(0)" unit="radians"/>
</section>
<section name="INS" prefix="INS_">
<define name="ROLL_NEUTRAL_DEFAULT" value="RadOfDeg(0)" unit="radians"/>
@@ -198,7 +243,7 @@
<!-- The below definition affect the throttle percentage shown on the GCS. -->
<define name="POWER_CTL_BAT_NOMINAL" value="12.6" unit="volt"/>
<!-- outer loop ALTITUDE proportional gain -->
<define name="ALTITUDE_PGAIN" value="0.07" unit="(m/s)/m"/>
<define name="ALTITUDE_PGAIN" value="0.08" unit="(m/s)/m"/>
<!-- outer loop ALTITUDE LIMIT (saturation) -->
<define name="ALTITUDE_MAX_CLIMB" value="3" unit="m/s"/>
<!-- outer loop AIRSPEED proportional gain -->
@@ -255,9 +300,11 @@
<section name="BAT">
<!-- FOR USE WITH A CURRENT SENSOR -->
<define name="BAT_CAPACITY" value="5000" unit="mah"/>
<define name="LOITER_BAT_CURRENT" value="10" unit="Amps"/>
<define name="CATASTROPHIC_BAT_LEVEL" value="7.5" unit="V"/>
<define name="CRITIC_BAT_LEVEL" value="8.5" unit="V"/>
<define name="LOW_BAT_LEVEL" value="9.3" unit="V"/>
<define name="CRITIC_BAT_LEVEL" value="9.0" unit="V"/>
<define name="LOW_BAT_LEVEL" value="10.0" unit="V"/>
<define name="MAX_BAT_LEVEL" value="12.6" unit="V"/>
</section>
@@ -275,16 +322,16 @@
<define name="KILL_MODE_DISTANCE" value="(1.5*MAX_DIST_FROM_HOME)"/>
<define name="CONTROL_FREQUENCY" value="60" unit="Hz"/>
<define name="XBEE_INIT" value="\"ATPL2\rATRN1\rATTT80\r\""/>
<define name="NO_XBEE_API_INIT" value="TRUE"/>
<define name="NO_XBEE_API_INIT" value="true"/>
<define name="TRIGGER_DELAY" value="1."/>
<define name="DEFAULT_CIRCLE_RADIUS" value="60."/>
<!-- MIN_CIRCLE_RADIUS used and needed for spiral navigation function and panic autolanding turns-->
<define name="MIN_CIRCLE_RADIUS" value="40."/>
<!--UNLOCKED_HOME_MODE if set to TRUE means that HOME mode does not get stuck.
<!--UNLOCKED_HOME_MODE if set to true means that HOME mode does not get stuck.
If not set before when you would enter home mode you had to flip a bit via the GCS to get out. -->
<define name="UNLOCKED_HOME_MODE" value="TRUE"/>
<define name="UNLOCKED_HOME_MODE" value="true"/>
<!-- RC_LOST_MODE means that if your RC Transmitter signal is not received anymore in the autopilot, e.g. you switch it off
or fly a long range mission you define the wanted mode behaviour here.
@@ -302,7 +349,7 @@
<define name="CLIMB_NAV_RATIO" value="0.8"/> <!-- Percent Navigation for Altitude Error Equal to Start Altitude -->
<define name="DESCENT_NAV_RATIO" value="1.0"/>
</section>
<!--
<section name="NAV">
<define name="NAV_PITCH" value="0."/>
<define name="NAV_GLIDE_PITCH_TRIM" value="0"/>
@@ -315,7 +362,7 @@
<define name="INTERCEPT_AF_TOD" value="10"/>
<define name="TARGET_SPEED" value="13"/>
</section>
-->
<section name="FAILSAFE" prefix="FAILSAFE_">
<define name="DELAY_WITHOUT_GPS" value="1" unit="s"/>
<define name="DEFAULT_THROTTLE" value="0.8" unit="%"/>
@@ -335,7 +382,7 @@
<define name="ALT_SHIFT_PLUS_PLUS" value="100"/>
<define name="ALT_SHIFT_PLUS" value="10"/>
<define name="ALT_SHIFT_MINUS" value="-10"/>
<define name="SPEECH_NAME" value="DART"/>
<define name="SPEECH_NAME" value="Easyglider"/>
</section>
<section name="SIMU">
conf/flight_plans/lagonisi_beach_survey.xml
+155
View File
@@ -0,0 +1,155 @@
<!DOCTYPE flight_plan SYSTEM "flight_plan.dtd">
<flight_plan alt="200." ground_alt="42" lat0="37.7776225" lon0="23.9184623" max_dist_from_home="4000" name="lagonisi_beach_survey" qfu="180" security_height="100.">
<header>
</header>
<waypoints>
<waypoint height="0" name="HOME" x="14.2" y="3.2"/>
<waypoint height="0" lat="37.777399" lon="23.918696" name="TD"/>
<waypoint height="0" name="CAM_POINT" x="11.6" y="34.3"/>
<waypoint height="50" name="STANDBY" x="23.3" y="-67.7"/>
<waypoint height="200" name="RIGHT_HERE" x="138.7" y="-28.0"/>
<waypoint height="30" name="AF" x="36.5" y="-265.3"/>
<waypoint height="30" name="BASELEG" x="101.5" y="246.0"/>
<waypoint height="200" name="MOB" x="137.3" y="3.5"/>
<waypoint height="30" lat="37.774872" lon="23.918757" name="AF_SOUTH"/>
<waypoint height="30" lat="37.779789" lon="23.918472" name="AF_NORTH"/>
<waypoint height="30" name="RELEASE" x="78.7" y="-22.2"/>
<waypoint alt="200" lat="37.772633" lon="23.906762" name="WP1"/>
<waypoint alt="200" lat="37.770300" lon="23.899190" name="WP2"/>
<waypoint alt="200" lat="37.774123" lon="23.895975" name="WP3"/>
<waypoint alt="200" lat="37.771894" lon="23.885380" name="WP4"/>
<waypoint alt="200" lat="37.774628" lon="23.882086" name="WP5"/>
<waypoint alt="200" lat="37.783286" lon="23.884851" name="WP6"/>
<waypoint alt="200" lat="37.786916" lon="23.880351" name="TARGET"/>
</waypoints>
<sectors/>
<variables/>
<exceptions>
<exception cond="(imcu_get_radio(RADIO_YAW) > MAX_PPRZ/2)&&(autopilot.mode == AP_MODE_AUTO2) && (GetPosAlt() > GetAltRef()+20)" deroute="Target"/>
<exception cond="(MIN_PPRZ/2 > imcu_get_radio(RADIO_YAW))&&(autopilot.mode == AP_MODE_AUTO2) && (GetPosAlt() > GetAltRef()+20)" deroute="Return Home"/>
</exceptions>
<blocks>
<block name="Wait Gps">
<set value="false" var="h_ctl_disabled"/>
<set value="1" var="autopilot.kill_throttle"/>
<call_once fun="imcu_set_command(COMMAND_FLAPS, 0)" />
<while cond="!GpsFixValid()"/>
</block>
<block name="Geo Init">
<while cond="LessThan(NavBlockTime(), 10)"/>
<call_once fun="NavSetGroundReferenceHere()"/>
<call_once fun="NavSetWaypointHere(WP_HOME)"/>
</block>
<block name="Holding Point">
<exception cond="((imcu_get_radio(RADIO_THROTTLE) > (MAX_PPRZ-(MAX_PPRZ/10)))&&(autopilot.mode == AP_MODE_AUTO2))" deroute="Takeoff"/>
<set value="1" var="autopilot.kill_throttle"/>
<call fun="imcu_set_command(COMMAND_FLAPS, MAX_PPRZ/2)"/>
<call_once fun="imcu_set_command(COMMAND_FLAPS, MAX_PPRZ/2)" />
<attitude roll="0" throttle="0" until="((sqrt(dist2_to_home) > 10)&& (700 > gps.pdop))" vmode="throttle"/>
<deroute block="Takeoff"/>
</block>
<block name="Takeoff" strip_button="Takeoff (wp AF_NORTH)" strip_icon="takeoff.png">
<exception cond="GetPosAlt() > GetAltRef()+25" deroute="Standby"/>
<set value="true" var="autopilot.launch"/>
<set value="false" var="h_ctl_disabled"/>
<set value="false" var="autopilot.kill_throttle"/>
<call_once fun="imcu_set_command(COMMAND_FLAPS, MAX_PPRZ/2)" />
<attitude pitch="20" roll="0.0" throttle="1.0" until="GetPosAlt() > GetAltRef()+20" vmode="throttle"/>
<deroute block="Standby"/>
</block>
<block name="Standby" strip_button="Standby" strip_icon="home.png">
<exception cond="electrical.bat_low&&(autopilot.mode == AP_MODE_AUTO2)&&(GetPosAlt() > GetAltRef()+20)" deroute="Land"/>
<exception cond="radio_control.status == RC_REALLY_LOST&&(autopilot.mode == AP_MODE_AUTO2)&&(GetPosAlt() > GetAltRef()+20)" deroute="Land"/>
<set value="false" var="h_ctl_disabled"/>
<set value="false" var="autopilot.kill_throttle"/>
<set value="DEFAULT_CIRCLE_RADIUS" var="nav_radius"/>
<call_once fun="imcu_set_command(COMMAND_FLAPS, 0)" />
<circle height="30" radius="nav_radius" wp="STANDBY"/>
</block>
<block name="Target" strip_button="Go to TARGET" strip_icon="kill.png">
<exception cond="electrical.bat_low&&(autopilot.mode == AP_MODE_AUTO2)&&(GetPosAlt() > GetAltRef()+20)" deroute="Return Home NOW"/>
<call_once fun="imcu_set_command(COMMAND_FLAPS, 0)" />
<go from="HOME" height="200" hmode="route" wp="WP1"/>
<go from="WP1" height="200" hmode="route" wp="WP2"/>
<go from="WP2" height="200" hmode="route" wp="WP3"/>
<go from="WP3" height="200" hmode="route" wp="WP4"/>
<go from="WP4" height="200" hmode="route" wp="WP5"/>
<go from="WP5" height="200" hmode="route" wp="WP6"/>
<go alt="200+ground_alt" from="WP6" hmode="route" wp="TARGET"/>
<circle height="200" radius="(nav_radius*2)" until="NavCircleCount() > 2" wp="TARGET"/>
<deroute block="Return Home"/>
</block>
<block name="Return Home" strip_button="Return Home" strip_icon="home.png">
<call_once fun="imcu_set_command(COMMAND_FLAPS, 0)" />
<go from="TARGET" height="200" hmode="route" wp="WP6"/>
<go from="WP6" height="200" hmode="route" wp="WP5"/>
<go from="WP5" height="200" hmode="route" wp="WP4"/>
<go from="WP4" height="200" hmode="route" wp="WP3"/>
<go from="WP3" height="200" hmode="route" wp="WP2"/>
<go from="WP2" height="200" hmode="route" wp="WP1"/>
<go from="WP1" height="200" hmode="route" wp="HOME"/>
<deroute block="Standby"/>
</block>
<block name="Return Home NOW" strip_button="Return Home" strip_icon="home.png">
<call_once fun="imcu_set_command(COMMAND_FLAPS, 0)" />
<call fun="NavSetWaypointHere(WP_RIGHT_HERE)"/>
<set value="GetPosAlt()" var="WaypointAlt(WP_RIGHT_HERE)"/>
<go from="RIGHT_HERE" height="200" hmode="route" wp="HOME"/>
<deroute block="Standby"/>
</block>
<block name="Land" strip_button="AUTO LAND">
<call fun="calculate_wind_no_airspeed(WP_TD, (nav_radius*2), 50)"/>
<deroute block="Land1"/>
</block>
<block name="Land1">
<exception cond="airborne_wind_dir > 270. || 90. > airborne_wind_dir" deroute="Land From South"/>
<exception cond="airborne_wind_dir > 90. && 270. > airborne_wind_dir" deroute="Land From North"/>
</block>
<block name="Land From North" strip_button="Land FROM NORTH (wp AF_NORTH-TD)" strip_icon="land-right.png">
<call_once fun="imcu_set_command(COMMAND_FLAPS, 0)" />
<call_once fun="nav_compute_baseleg(WP_AF_NORTH, WP_TD, WP_BASELEG, nav_radius)"/>
<circle radius="nav_radius" until="NavCircleCount() > 0.5" wp="BASELEG"/>
<set value="V_CTL_AUTO_THROTTLE_MIN_CRUISE_THROTTLE" var="v_ctl_auto_throttle_cruise_throttle"/>
<circle radius="nav_radius" until="NavQdrCloseTo(DegOfRad(baseleg_out_qdr)-(nav_radius/fabs(nav_radius))*10) && 10 > fabs(GetPosAlt() - WaypointAlt(WP_BASELEG))" wp="BASELEG"/>
</block>
<block name="Final North">
<call_once fun="imcu_set_command(COMMAND_FLAPS, MAX_PPRZ/2)" />
<exception cond="GetAltRef() + 10 > GetPosAlt()" deroute="Flare North"/>
<go from="AF_NORTH" hmode="route" vmode="glide" wp="TD"/>
</block>
<block name="Flare North">
<go approaching_time="0" from="AF_NORTH" hmode="route" throttle="0.0" vmode="throttle" wp="TD"/>
<attitude roll="0.0" throttle="0.0" until="false" vmode="throttle"/>
</block>
<block name="Land From South" strip_button="Land FROM SOUTH (wp AF_SOUTH-TD)" strip_icon="land-left.png">
<call_once fun="imcu_set_command(COMMAND_FLAPS, 0)" />
<call_once fun="nav_compute_baseleg(WP_AF_SOUTH, WP_TD, WP_BASELEG, nav_radius)"/>
<circle radius="nav_radius" until="NavCircleCount() > 0.5" wp="BASELEG"/>
<set value="V_CTL_AUTO_THROTTLE_MIN_CRUISE_THROTTLE" var="v_ctl_auto_throttle_cruise_throttle"/>
<circle radius="nav_radius" until="NavQdrCloseTo(DegOfRad(baseleg_out_qdr)-(nav_radius/fabs(nav_radius))*10) && 10>fabs(GetPosAlt()-WaypointAlt(WP_BASELEG))" wp="BASELEG"/>
</block>
<block name="final South">
<call_once fun="imcu_set_command(COMMAND_FLAPS, MAX_PPRZ/2)" />
<exception cond="GetAltRef() + 10 > GetPosAlt()" deroute="Flare South"/>
<go from="AF_SOUTH" hmode="route" vmode="glide" wp="TD"/>
</block>
<block name="Flare South">
<go approaching_time="0" from="AF_SOUTH" hmode="route" throttle="0.0" vmode="throttle" wp="TD"/>
<attitude roll="0.0" throttle="0.0" until="false" vmode="throttle"/>
</block>
<block name="Recover Uav" strip_button="Recover" strip_icon="parachute.png">
<call fun="LockParachute()"/>
<call fun="calculate_wind_no_airspeed(WP_TD, (nav_radius*2), 50)"/>
<call fun="ParachuteComputeApproach(WP_BASELEG, WP_RELEASE, WP_TD)"/>
<circle alt="(ground_alt+30)" radius="nav_radius" until="NavQdrCloseTo(DegOfRad(parachute_start_qdr)-(nav_radius/fabs(nav_radius))*10) && 10>fabs(GetPosAlt()-(ground_alt+30))" wp="BASELEG"/>
<call fun="NavSetWaypointHere(WP_MOB)"/>
<call_once fun="imcu_set_command(COMMAND_FLAPS, MAX_PPRZ/2)" />
<go from="MOB" hmode="route" wp="RELEASE"/>
<attitude pitch="10" roll="0.0" throttle="0" until="MINIMUM_AIRSPEED > stateGetHorizontalSpeedNorm_f()" vmode="throttle"/>
<call fun="DeployParachute()"/>
<set value="1" var="autopilot.kill_throttle"/>
<circle height="70." radius="nav_radius" wp="HOME"/>
</block>
</blocks>
</flight_plan>
conf/gcs/gcs_with_video.xml
+132
View File
@@ -0,0 +1,132 @@
<!DOCTYPE layout SYSTEM "layout.dtd">
<layout width="1362" height="1008">
<rows>
<widget NAME="map2d" size="654">
<papget type="message_field" display="text" x="1159" y="168">
<property name="scale" value="1."/>
<property name="field" value="WEATHER:windspeed"/>
<property name="format" value="%.1f Wind speed"/>
<property name="size" value="15."/>
<property name="color" value="yellow"/>
</papget>
<papget type="message_field" display="text" x="1151" y="85">
<property name="scale" value="1."/>
<property name="field" value="AIRBORNE_ANTENNA:mag_heading_deg"/>
<property name="format" value="%.0f Mag"/>
<property name="size" value="25."/>
<property name="color" value="#00ff00"/>
</papget>
<papget type="message_field" display="text" x="18" y="510">
<property name="scale" value="1."/>
<property name="field" value="ENERGY:voltage"/>
<property name="format" value="%.2f BAT"/>
<property name="size" value="24."/>
<property name="color" value="#00ff00"/>
</papget>
<papget type="message_field" display="text" x="1153" y="10">
<property name="scale" value="0.036"/>
<property name="field" value="GPS:speed"/>
<property name="format" value="%.0f Km/h"/>
<property name="size" value="25."/>
<property name="color" value="yellow"/>
</papget>
<papget type="message_field" display="text" x="16" y="286">
<property name="scale" value="1."/>
<property name="field" value="BAT:energy"/>
<property name="format" value="%.0f mah"/>
<property name="size" value="25."/>
<property name="color" value="green"/>
</papget>
<papget type="message_field" display="text" x="17" y="157">
<property name="scale" value="1."/>
<property name="field" value="CAM_POINT:cam_point_lon"/>
<property name="format" value="%.4f E"/>
<property name="size" value="25."/>
<property name="color" value="yellow"/>
</papget>
<papget type="message_field" display="text" x="19" y="195">
<property name="scale" value="10."/>
<property name="field" value="CAM_POINT:cam_point_distance_from_home"/>
<property name="format" value="%.0f meters"/>
<property name="size" value="15."/>
<property name="color" value="yellow"/>
</papget>
<papget type="message_field" display="text" x="1159" y="191">
<property name="scale" value="1."/>
<property name="field" value="WEATHER:wind_from"/>
<property name="format" value="%.0f wind from"/>
<property name="size" value="15."/>
<property name="color" value="yellow"/>
</papget>
<papget type="message_field" display="text" x="18" y="121">
<property name="scale" value="1."/>
<property name="field" value="CAM_POINT:cam_point_lat"/>
<property name="format" value="%.4f N"/>
<property name="size" value="25."/>
<property name="color" value="yellow"/>
</papget>
<papget type="message_field" display="text" x="1164" y="264">
<property name="scale" value=".01"/>
<property name="field" value="BMP_STATUS:temp"/>
<property name="format" value="%.1f C"/>
<property name="size" value="15."/>
<property name="color" value="#00ff00"/>
</papget>
<papget type="message_field" display="text" x="21" y="323">
<property name="scale" value="0.01"/>
<property name="field" value="BAT:amps"/>
<property name="format" value="%.1f A"/>
<property name="size" value="15."/>
<property name="color" value="green"/>
</papget>
<papget type="message_field" display="text" x="24" y="558">
<property name="scale" value="1."/>
<property name="field" value="ENERGY:current"/>
<property name="format" value="%.1f Amps"/>
<property name="size" value="25."/>
<property name="color" value="#00ff00"/>
</papget>
<papget type="message_field" display="text" x="1161" y="215">
<property name="scale" value=".01"/>
<property name="field" value="BMP_STATUS:UP"/>
<property name="format" value="%.1f Baro Alt m"/>
<property name="size" value="15."/>
<property name="color" value="#00ff00"/>
</papget>
<papget type="message_field" display="text" x="1163" y="239">
<property name="scale" value="1."/>
<property name="field" value="BMP_STATUS:press"/>
<property name="format" value="%.0f pascal"/>
<property name="size" value="15."/>
<property name="color" value="#00ff00"/>
</papget>
<papget type="message_field" display="text" x="1156" y="57">
<property name="scale" value="0.1"/>
<property name="field" value="GPS:course"/>
<property name="format" value="%.0f° HDG"/>
<property name="size" value="25."/>
<property name="color" value="orange"/>
</papget>
<papget type="message_field" display="text" x="1162" y="102">
<property name="scale" value="1."/>
<property name="field" value="IMU_MAG:mx"/>
<property name="format" value="%.0f Mag Course"/>
<property name="size" value="15."/>
<property name="color" value="#00ff00"/>
</papget>
</widget>
<columns>
<rows SIZE="374">
<widget NAME="strips" size="220"/>
<widget NAME="alarms" size="122"/>
</rows>
<rows SIZE="350">
<widget NAME="aircraft" size="347"/>
</rows>
<rows SIZE="300">
<widget NAME="plugin" size="347"/>
</rows>
</columns>
</rows>
</layout>
conf/modules/board_matek_wing.xml
+2 -4
View File
@@ -27,10 +27,8 @@
<!-- OSD CONFIGURATION -->
<configure name="MAX7456_SPI_DEV" value="spi2" />
<configure name="MAX7456_SLAVE_IDX" value="SPI_SLAVE1" />
<define name="USE_MATEK_TYPE_OSD_CHIP" value="1" />
<define name="USE_PAL_FOR_OSD_VIDEO" value="1" />
<define name="OSD_USE_BARO_ALTITUDE" value="0" />
<define name="USE_MATEK_TYPE_OSD_CHIP" value="true" />
<define name="USE_PAL_FOR_OSD_VIDEO" value="true" />
<define name="BARO_ALTITUDE_VAR" value="baro_alt" /> <!-- if non defined the default var is ''baro_alt'' -->
<define name="OSD_USE_MAG_COMPASS" value="0" />
</makefile>
</module>
conf/modules/pca9685.xml
+22
View File
@@ -0,0 +1,22 @@
<!DOCTYPE module SYSTEM "module.dtd">
<module name="pca9685" dir="pca9685">
<doc>
<description>
pca9685 additional servo driver
in a c code file write pca9865_set_servo(12, 1500); to set servo number 12.
In the flight plan you can use call_once fun="pca9865_set_servo(12, 1500)"
</description>
</doc>
<settings>
</settings>
<header>
<file name="pca9685_i2c.h"/>
</header>
<init fun="pca9685_i2c_init()"/>
<periodic fun="pca9685_i2c_periodic()" freq="10"/>
<event fun="pca9685_i2c_event()"/>
<makefile>
<file name="pca9685_i2c.c"/>
</makefile>
</module>
conf/modules/uav_recovery.xml
+32
View File
@@ -0,0 +1,32 @@
<!DOCTYPE module SYSTEM "module.dtd">
<module name="uav_recovery" dir="uav_recovery">
<doc>
<description>Parachute</description>
<define name="USE_PARACHUTE" value="true OR false" description="whether to use parachute recovery or not"/>
<define name="PARACHUTE_SERVO_CHANNEL" value="The paraccute servo name" description="which servo to use, -1 if none" />
<define name="PARACHUTE_DESCENT_RATE" value="3.0" description="calculated descent rate in meters/second"/>
<define name="PARACHUTE_WIND_CORRECTION" value="1.0" description="to estimate the landing spot better"/>
<define name="PARACHUTE_LINE_LENGTH" value="3.0" description="the parachute to plane distance in meters."/>
<define name="FIXED_WIND_DIRECTION_FOR_TESTING" value="90." description="fixed wind direction for the simulator ONLY!"/>
<define name="FIXED_WIND_SPEED_FOR_TESTING" value="10." description="fixed wind speed for the simulator ONLY!"/>
<define name="AIRBORNE_WIND_CORRECTION" value="1.0" description="Airframe dependant wind calculation correction factor." />
</doc>
<settings>
<dl_settings>
<dl_settings NAME="Parachute">
<dl_setting MAX="1" MIN="0" STEP="1" VAR="deploy_parachute_var" shortname="deploy parachute">
<!-- <strip_button name="parachute" icon="on.png" value="1" group="parachute" /> -->
</dl_setting>
</dl_settings>
</dl_settings>
</settings>
<header>
<file name="uav_recovery.h"/>
</header>
<init fun="uav_recovery_init()"/>
<periodic fun="uav_recovery_periodic()" freq="10"/>
<makefile>
<file name="uav_recovery.c"/>
</makefile>
</module>
conf/radios/openTX_radio_8ch.xml
+9 -9
View File
@@ -38,14 +38,14 @@
-- Note: a command may be reversed by exchanging min and max values
-->
<!DOCTYPE radio SYSTEM "/radio.dtd">
<radio name="X8_OPEN9X" data_min="700" data_max="2200" sync_min="2500" sync_max="18000" pulse_type="NEGATIVE">
<channel ctl="A" function="ROLL" min="988" neutral="1500" max="2012" average="0"/>
<channel ctl="B" function="PITCH" min="988" neutral="1500" max="2012" average="0"/>
<channel ctl="C" function="THROTTLE" min="988" neutral="988" max="2012" average="0"/>
<channel ctl="D" function="YAW" min="988" neutral="1500" max="2012" average="0"/>
<channel ctl="E" function="MODE" min="988" neutral="1500" max="2012" average="10"/>
<channel ctl="E" function="PARACHUTE" min="988" neutral="988" max="2012" average="10"/>
<channel ctl="F" function="AUX1" min="988" neutral="1500" max="2012" average="0"/>
<channel ctl="G" function="AUX2" min="988" neutral="1500" max="2012" average="0"/>
<radio name="OPENTX" data_min="700" data_max="2200" sync_min="2500" sync_max="18000" pulse_type="NEGATIVE">
<channel ctl="A" function="ROLL" min="988" neutral="1500" max="2012" average="0"/>
<channel ctl="B" function="PITCH" min="988" neutral="1500" max="2012" average="0"/>
<channel ctl="C" function="THROTTLE" min="988" neutral="988" max="2012" average="0"/>
<channel ctl="D" function="YAW" min="988" neutral="1500" max="2012" average="0"/>
<channel ctl="E" function="MODE" min="988" neutral="1500" max="2012" average="10"/>
<channel ctl="E" function="AUX1" min="988" neutral="1500" max="2012" average="10"/>
<channel ctl="F" function="AUX2" min="988" neutral="1500" max="2012" average="0"/>
<channel ctl="G" function="AUX3" min="988" neutral="1500" max="2012" average="0"/>
</radio>
conf/telemetry/telemetry_openlrsng_19200.xml
+28 -24
View File
@@ -3,32 +3,35 @@
<telemetry>
<process name="Ap">
<mode name="default">
<message name="ALIVE" period="5.0"/>
<message name="ATTITUDE" period="0.55"/>
<message name="ESTIMATOR" period="0.60"/>
<message name="DL_VALUE" period="0.65"/>
<message name="WP_MOVED" period="0.75"/>
<message name="GPS" period="1.25"/>
<message name="ENERGY" period="1.35"/>
<message name="SEGMENT" period="1.45"/>
<message name="WEATHER" period="1.65"/>
<message name="BMP_STATUS" period="1.75"/> <!-- BARO BMP280 DATA -->
<message name="CIRCLE" period="2.05"/>
<message name="DESIRED" period="2.25"/>
<message name="NAVIGATION" period="3.05"/>
<message name="PPRZ_MODE" period="5.05"/>
<message name="GPS_SOL" period="5.25"/>
<message name="NAVIGATION_REF" period="9.35"/>
<message name="ALIVE" period="5.0"/>
<message name="ATTITUDE" period="0.45"/>
<message name="ESTIMATOR" period="0.55"/>
<message name="DL_VALUE" period="0.65"/>
<message name="IMU_MAG" period="0.75"/>
<message name="CAM_POINT" period="0.85"/>
<message name="CAM" period="0.95"/>
<message name="WP_MOVED" period="1.05"/>
<message name="GPS" period="1.15"/>
<message name="ENERGY" period="1.25"/>
<message name="SEGMENT" period="1.35"/>
<message name="BMP_STATUS" period="1.55"/> <!-- BARO BMP280 DATA -->
<message name="CIRCLE" period="2.05"/>
<message name="WEATHER" period="2.15"/>
<message name="DESIRED" period="2.25"/>
<message name="NAVIGATION" period="3.05"/>
<message name="PPRZ_MODE" period="5.05"/>
<message name="GPS_SOL" period="5.25"/>
<message name="NAVIGATION_REF" period="9.35"/>
</mode>
<mode name="minimal">
<message name="ALIVE" period="5.0"/>
<message name="ATTITUDE" period="0.65"/>
<message name="ENERGY" period="0.95"/>
<message name="ATTITUDE" period="0.55"/>
<message name="ESTIMATOR" period="0.65"/>
<message name="DL_VALUE" period="0.75"/>
<message name="CAM_POINT" period="1.05"/>
<message name="GPS" period="1.25"/>
<message name="ESTIMATOR" period="1.35"/>
<message name="GPS" period="1.15"/>
<message name="WP_MOVED" period="1.45"/>
<message name="DL_VALUE" period="1.55"/>
<message name="ENERGY" period="1.65"/>
<message name="SURVEY" period="2.05"/>
<message name="CIRCLE" period="2.15"/>
<message name="DESIRED" period="2.25"/>
@@ -40,13 +43,14 @@
</mode>
<mode name="extremal">
<message name="ALIVE" period="5"/>
<message name="ATTITUDE" period="0.5"/>
<message name="ATTITUDE" period="0.55"/>
<message name="DL_VALUE" period="0.75"/>
<message name="CAM_POINT" period="1.0"/>
<message name="GPS" period="5.1"/>
<message name="ESTIMATOR" period="5.2"/>
<message name="NAVIGATION" period="5.4"/>
<message name="PPRZ_MODE" period="5.5"/>
<message name="ENERGY" period="10.1"/>
<message name="PPRZ_MODE" period="5.6"/>
<message name="ENERGY" period="5.8"/>
<message name="DESIRED" period="10.2"/>
</mode>
<mode name="raw_sensors">
sw/airborne/boards/matek_f405_wing_v1.h
+2 -2
View File
@@ -251,8 +251,8 @@
#define ADC_CHANNEL_VSUPPLY ADC_1
#endif
#ifndef CURRENT_ADC_IN
#define CURRENT_ADC_IN ADC_2
#ifndef ADC_CHANNEL_CURRENT
#define ADC_CHANNEL_CURRENT ADC_2
#endif
/* no voltage divider on board, adjust VoltageOfAdc in airframe file */
sw/airborne/modules/display/max7456.c
+585 -231
View File
File diff suppressed because it is too large Load Diff
sw/airborne/modules/display/max7456.h
+3
View File
@@ -36,6 +36,9 @@ extern void max7456_periodic(void);
extern void max7456_event(void);
extern uint8_t osd_enable;
extern float mag_course_deg;
extern float mag_heading_rad;
extern float home_dir_deg;
#endif //MAX7456_H
sw/airborne/modules/pca9685/pca9685_i2c.c
+471
View File
@@ -0,0 +1,471 @@
/*
* Copyright (C) 2013-2020 Chris Efstathiou hendrixgr@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.
*/
/**
* @file modules/pca9685/pca9685.c
* PCA9685 LED DRIVER USED AS A 16 ADDITIONAL PWM SERVO DRIVER.
*
*/
//###################################################################################################
// I N C L U D E D H E A D E R F I L E S
//###################################################################################################
#include "modules/pca9685/pca9685_i2c.h"
#include "math/pprz_algebra_int.h"
#include "mcu_periph/i2c.h"
#include "mcu_periph/sys_time.h"
#include "math/pprz_isa.h"
#include "mcu_periph/sys_time.h"
#include "subsystems/abi.h"
#include "mcu_periph/uart.h"
#if DOWNLINK
#include "subsystems/datalink/telemetry.h"
#endif
//###################################################################################################
// P R E P R O C E S S O R D I R E C T I V E S A N D D E F I N I T I O N S
//###################################################################################################
// BIG ENDIAN DEFINITIONS
//#define BUF2INT(_buf,_idx) ((int16_t)((_buf[_idx]<<8) | _buf[_idx+1]))
//#define INT2BUF(_int,_buf,_idx) { _buf[_idx] = (_int>>8); _buf[_idx+1] = _int; }
// LITTLE ENDIAN DEFINITIONS USED FOR AvR CPU DUE TO MORE EFFICIENT DATA TRANSFERS
#define BUF2INT(_buf,_idx) ((int16_t)((_buf[_idx+1]<<8) | _buf[_idx]))
#define INT2BUF(_int,_buf,_idx) { _buf[_idx] = _int; _buf[_idx+1] = (_int>>8); }
#ifndef PCA9685_I2C_DEV
#define PCA9685_I2C_DEV i2c2
#endif
PRINT_CONFIG_VAR(PCA9685_I2C_DEV)
#ifndef PCA9865_SRV_RESOLUTION
#define PCA9865_SRV_RESOLUTION 1. // in microseconds
#endif
#ifndef PCA9865_SRV_DEFAULT_VAL_US
#define PCA9865_SRV_DEFAULT_VAL_US 1000. // in microseconds
#endif
#ifndef PCA9865_SRV_NUMBER
#define PCA9865_SRV_NUMBER 16 // in microseconds
#endif
// I2C Addreses
#ifndef PCA9685_I2C_SLAVE_ADDR
#define PCA9685_I2C_SLAVE_ADDR 0xE0
#endif
PRINT_CONFIG_VAR(PCA9685_I2C_SLAVE_ADDR)
#ifndef PCA9685_I2C_ALLCALL_ADDR
#define PCA9685_I2C_ALLCALL_ADDR 0xE0
#endif
#ifndef PCA9685_I2C_RESET_ADDR
#define PCA9685_I2C_RESET_ADDR 0x06
#endif
#ifndef PCA9685_I2C_GEN_CALL_ADDR
#define PCA9685_I2C_GEN_CALL_ADDR 0x00
#endif
// Register addresses
#define PCA9685_MODE1_REG_ADDR 0x00
#define PCA9685_MODE2_REG_ADDR 0x01
#define PCA9685_ALLCALL_ADDR 0x05
#define PCA9685_LED0_ON_L_REG_ADDR 0X06
#define PCA9685_LED0_ON_H_REG_ADDR 0X07
#define PCA9685_LED0_OFF_L_REG_ADDR 0X08
#define PCA9685_LED0_OFF_H_REG_ADDR 0X09
#define PCA9685_LED1_ON_L_REG_ADDR 0X0A
#define PCA9685_LED1_ON_H_REG_ADDR 0X0B
#define PCA9685_LED1_OFF_L_REG_ADDR 0X0C
#define PCA9685_LED1_OFF_H_REG_ADDR 0X0D
#define PCA9685_LED2_ON_L_REG_ADDR 0X0E
#define PCA9685_LED2_ON_H_REG_ADDR 0X0F
#define PCA9685_LED2_OFF_L_REG_ADDR 0X10
#define PCA9685_LED2_OFF_H_REG_ADDR 0X11
#define PCA9685_LED3_ON_L_REG_ADDR 0X12
#define PCA9685_LED3_ON_H_REG_ADDR 0X13
#define PCA9685_LED3_OFF_L_REG_ADDR 0X14
#define PCA9685_LED3_OFF_H_REG_ADDR 0X15
#define PCA9685_LED4_ON_L_REG_ADDR 0X16
#define PCA9685_LED4_ON_H_REG_ADDR 0X17
#define PCA9685_LED4_OFF_L_REG_ADDR 0X18
#define PCA9685_LED4_OFF_H_REG_ADDR 0X19
#define PCA9685_LED5_ON_L_REG_ADDR 0X1A
#define PCA9685_LED5_ON_H_REG_ADDR 0X1B
#define PCA9685_LED5_OFF_L_REG_ADDR 0X1C
#define PCA9685_LED5_OFF_H_REG_ADDR 0X1D
#define PCA9685_LED6_ON_L_REG_ADDR 0X1E
#define PCA9685_LED6_ON_H_REG_ADDR 0X1F
#define PCA9685_LED6_OFF_L_REG_ADDR 0X20
#define PCA9685_LED6_OFF_H_REG_ADDR 0X21
#define PCA9685_LED7_ON_L_REG_ADDR 0X22
#define PCA9685_LED7_ON_H_REG_ADDR 0X23
#define PCA9685_LED7_OFF_L_REG_ADDR 0X24
#define PCA9685_LED7_OFF_H_REG_ADDR 0X25
#define PCA9685_LED8_ON_L_REG_ADDR 0X26
#define PCA9685_LED8_ON_H_REG_ADDR 0X27
#define PCA9685_LED8_OFF_L_REG_ADDR 0X28
#define PCA9685_LED8_OFF_H_REG_ADDR 0X29
#define PCA9685_LED9_ON_L_REG_ADDR 0X2A
#define PCA9685_LED9_ON_H_REG_ADDR 0X2B
#define PCA9685_LED9_OFF_L_REG_ADDR 0X2C
#define PCA9685_LED9_OFF_H_REG_ADDR 0X2D
#define PCA9685_LED10_ON_L_REG_ADDR 0X2E
#define PCA9685_LED10_ON_H_REG_ADDR 0X2F
#define PCA9685_LED10_OFF_L_REG_ADDR 0X30
#define PCA9685_LED10_OFF_H_REG_ADDR 0X31
#define PCA9685_LED11_ON_L_REG_ADDR 0X32
#define PCA9685_LED11_ON_H_REG_ADDR 0X33
#define PCA9685_LED11_OFF_L_REG_ADDR 0X34
#define PCA9685_LED11_OFF_H_REG_ADDR 0X35
#define PCA9685_LED12_ON_L_REG_ADDR 0X36
#define PCA9685_LED12_ON_H_REG_ADDR 0X37
#define PCA9685_LED12_OFF_L_REG_ADDR 0X38
#define PCA9685_LED12_OFF_H_REG_ADDR 0X39
#define PCA9685_LED13_ON_L_REG_ADDR 0X3A
#define PCA9685_LED13_ON_H_REG_ADDR 0X3B
#define PCA9685_LED13_OFF_L_REG_ADDR 0X3C
#define PCA9685_LED13_OFF_H_REG_ADDR 0X3D
#define PCA9685_LED14_ON_L_REG_ADDR 0X3E
#define PCA9685_LED14_ON_H_REG_ADDR 0X3F
#define PCA9685_LED14_OFF_L_REG_ADDR 0X40
#define PCA9685_LED14_OFF_H_REG_ADDR 0X41
#define PCA9685_LED15_ON_L_REG_ADDR 0X42
#define PCA9685_LED15_ON_H_REG_ADDR 0X43
#define PCA9685_LED15_OFF_L_REG_ADDR 0X44
#define PCA9685_LED15_OFF_H_REG_ADDR 0X45
#define PCA9685_ALL_LED_ON_L_REG_ADDR 0XFA
#define PCA9685_ALL_LED_ON_H_REG_ADDR 0XFB
#define PCA9685_ALL_LED_OFF_L_REG_ADDR 0XFC
#define PCA9685_ALL_LED_OFF_H_REG_ADDR 0XFD
#define PCA9685_PRESCALER_REG_ADDR 0XFE
//Bit positions
//MODE 0 REGISTER
#define PCA9865_RESTART_BIT 7 // 1= RESTART ENABLE, DEFAULT = 0
#define PCA9865_AUTO_INCREMENT_BIT 5 // 1 = AUTO INCREMENT, DEFAULT = 0
#define PCA9865_SLEEP_BIT 4 // 1 = LOW POWER MODE (OSC OFF), 0=NORMAL MODE, DEFAULT=0
#define PCA9865_ALLCALL_BIT 0 // 1 = ALL LED ENABLE, DEFAULT = 1
//MODE 1 REGISTER
#define PCA9865_OUTDRV_BIT 2 // 1 = TOTEM POLE, 0 = OPEN DRAIN, DEFAULT = 1
#if !defined(SITL)
//###################################################################################################
// P R I V A T E F U N C T I O N P R O T O T Y P E S
//###################################################################################################
#if defined(PCA9685_SEND_SERVO_VALUES) && PCA9685_SEND_SERVO_VALUES == 1
static void pca9685_send_servo_values(struct transport_tx *trans, struct link_device *dev);
#endif
//###################################################################################################
// G L O B A L V A R I A B L E S
//###################################################################################################
enum {
PCA9685_I2C_STATUS_UNINIT,
PCA9685_I2C_STATUS_INITIALIZED,
PCA9865_I2C_STATUS_WRITE_LED_REG_BUSY,
PCA9865_I2C_STATUS_WRITE_LED_REG_READY,
PCA9865_I2C_STATUS_READ_LED_REG_BUSY,
PCA9865_I2C_STATUS_CHANGE_LED_REG_FINISHED
};
struct i2c_transaction pca9685_i2c_trans;
uint8_t pca9685_i2c_status = 0;
uint8_t srv_cnt = 0;
uint16_t pca9865_write_servo_vals[PCA9865_SRV_NUMBER];
#if defined(PCA9685_SEND_SERVO_VALUES) && PCA9685_SEND_SERVO_VALUES == 1
PRINT_CONFIG_MSG("PCA9685 servo values can be found in the ""BARO_WORDS"" message")
uint16_t pca9865_read_servo_vals[PCA9865_SRV_NUMBER];
#endif
uint8_t pca9865_reg_nb[16];
//###################################################################################################
// P R I V A T E F U N C T I O N D E F I N I T I O N S
//###################################################################################################
//111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111
#if defined(PCA9685_SEND_SERVO_VALUES) && PCA9685_SEND_SERVO_VALUES == 1
static void pca9685_send_servo_values(struct transport_tx *trans, struct link_device *dev)
{
// For debugging purposes we send anly the first 4 servo values using the BARO_WORDS message.
// Those values are read back from PCA9865 so they indicate if the module is working as intended.
pprz_msg_send_CSC_SERVO_CMD(
trans, dev, AC_ID,
&pca9865_read_servo_vals[0], &pca9865_read_servo_vals[1],
&pca9865_read_servo_vals[2], &pca9865_read_servo_vals[3]
);
return;
}
#endif
//@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
// P U B L I C F U N C T I O N D E F I N I T I O N S
//@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
//111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111
bool pca9865_set_servo(uint8_t srv_nb, uint16_t srv_val)
{
uint16_t servo_value = 0;
if (srv_nb < 16) {
servo_value = (uint16_t)((float)srv_val / (float)PCA9865_SRV_RESOLUTION);
pca9865_write_servo_vals[srv_nb] = servo_value;
pca9865_reg_nb[srv_nb] = (srv_nb * 4) + 8;
}
return (FALSE);
}
//222222222222222222222222222222222222222222222222222222222222222222222222222222222222222222222222222
void pca9685_i2c_init(void)
{
uint16_t reg_value = 0;
uint32_t timer0 = 0;
// clear all arrays to zero.
for (srv_cnt = 0; srv_cnt < PCA9865_SRV_NUMBER; srv_cnt++) {
pca9865_write_servo_vals[srv_cnt] = 0;
#if defined(PCA9685_SEND_SERVO_VALUES) && PCA9685_SEND_SERVO_VALUES == 1
pca9865_read_servo_vals[srv_cnt] = 0;
#endif
pca9865_reg_nb[srv_cnt] = 0;
}
srv_cnt = 0;
pca9685_i2c_trans.status = I2CTransDone;
pca9685_i2c_status = PCA9685_I2C_STATUS_UNINIT;
pca9685_i2c_trans.buf[0] = PCA9685_I2C_RESET_ADDR;
i2c_transmit(&PCA9685_I2C_DEV, &pca9685_i2c_trans, PCA9685_I2C_GEN_CALL_ADDR, 1);
timer0 = sys_time.nb_tick + (SYS_TIME_FREQUENCY / 2);
while (pca9685_i2c_trans.status == I2CTransPending) {
if (sys_time.nb_tick > timer0) { break; }
}
// Internal Oscillator Off, the oscillator is off by default on power up.
pca9685_i2c_trans.buf[0] = PCA9685_MODE1_REG_ADDR;
pca9685_i2c_trans.buf[1] = (1 << PCA9865_SLEEP_BIT) | (1 << PCA9865_ALLCALL_BIT); // OSC off
i2c_transmit(&PCA9685_I2C_DEV, &pca9685_i2c_trans, PCA9685_I2C_SLAVE_ADDR, 2);
timer0 = sys_time.nb_tick + (SYS_TIME_FREQUENCY / 2);
while (pca9685_i2c_trans.status == I2CTransPending) {
if (sys_time.nb_tick > timer0) { break; }
}
reg_value = (uint16_t)((float)PCA9865_SRV_RESOLUTION / 0.04); //0.04 = 1/25 Mhz
if (reg_value > 0xFF) { reg_value = 0xFF; } // Sanity check.
pca9685_i2c_trans.buf[0] = PCA9685_PRESCALER_REG_ADDR;
pca9685_i2c_trans.buf[1] = (uint8_t)reg_value;
i2c_transmit(&PCA9685_I2C_DEV, &pca9685_i2c_trans, PCA9685_I2C_SLAVE_ADDR, 2);
timer0 = sys_time.nb_tick + (SYS_TIME_FREQUENCY / 2);
while (pca9685_i2c_trans.status == I2CTransPending) {
if (sys_time.nb_tick > timer0) { break; }
}
// Internal Oscillator On, Auto Increment on, AllCall enabled, 500 microseconds needed.
pca9685_i2c_trans.buf[0] = PCA9685_MODE1_REG_ADDR;
pca9685_i2c_trans.buf[1] = (1 << PCA9865_AUTO_INCREMENT_BIT) | (1 << PCA9865_ALLCALL_BIT);
i2c_transmit(&PCA9685_I2C_DEV, &pca9685_i2c_trans, PCA9685_I2C_SLAVE_ADDR, 2);
timer0 = sys_time.nb_tick + (SYS_TIME_FREQUENCY / 2);
while (pca9685_i2c_trans.status == I2CTransPending) {
if (sys_time.nb_tick > timer0) { break; }
}
do {
pca9685_i2c_trans.buf[0] = PCA9685_MODE1_REG_ADDR;
i2c_transceive(&PCA9685_I2C_DEV, &pca9685_i2c_trans, PCA9685_I2C_SLAVE_ADDR, 1, 1);
timer0 = sys_time.nb_tick + (SYS_TIME_FREQUENCY / 2);
while (pca9685_i2c_trans.status == I2CTransPending) {
if (sys_time.nb_tick > timer0) { break; }
}
} while ((pca9685_i2c_trans.buf[0] & (1 << PCA9865_RESTART_BIT)) != 0);
pca9685_i2c_trans.buf[0] = PCA9685_ALL_LED_ON_L_REG_ADDR;
pca9685_i2c_trans.buf[1] = 0;
pca9685_i2c_trans.buf[2] = 0;
reg_value = (uint16_t)((float)PCA9865_SRV_DEFAULT_VAL_US / (float)PCA9865_SRV_RESOLUTION);
pca9685_i2c_trans.buf[3] = (uint8_t)reg_value;
pca9685_i2c_trans.buf[4] = (uint8_t)((reg_value >> 8) & 0x000F);
i2c_transmit(&PCA9685_I2C_DEV, &pca9685_i2c_trans, PCA9685_I2C_ALLCALL_ADDR, 5);
timer0 = sys_time.nb_tick + (SYS_TIME_FREQUENCY / 2);
while (pca9685_i2c_trans.status == I2CTransPending) {
if (sys_time.nb_tick > timer0) { break; }
}
pca9685_i2c_trans.status = I2CTransDone;
pca9685_i2c_status = PCA9685_I2C_STATUS_INITIALIZED;
#if DOWNLINK
#if defined(PCA9685_SEND_SERVO_VALUES) && PCA9685_SEND_SERVO_VALUES == 1
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_CSC_SERVO_CMD, pca9685_send_servo_values);
#endif
#endif
// Initialize all 16 servo with the default values
pca9865_set_servo(0, 1500);
pca9865_set_servo(1, 1500);
pca9865_set_servo(2, 1000);
pca9865_set_servo(3, 1500);
for (srv_cnt = 4; srv_cnt < PCA9865_SRV_NUMBER; srv_cnt++) {
pca9865_set_servo(srv_cnt, 1000);
}
srv_cnt = 0;
return;
}
//333333333333333333333333333333333333333333333333333333333333333333333333333333333333333333333333333
void pca9685_i2c_periodic(void)
{
// If a servo change was requested write the appropriate pca9685 registers.
// Since those servos are not intented for flight surface control they can be updated slower
// at about one servo each module cycle. This way i2c load is minimized.
//
// Start searching for a servo change request either after intialization or after all
// servo channels have been searched (looping).
// This loop will be executed always until a servo value change is requested.
// Then it will break with "srv_cnt" having the servo number to be changed.
// DELAY IN ORDER TO GIVE TIME TO THE INTERNAL PCA9865 OSCILLATOR TO STABILIZE.
//if (sys_time.nb_sec > 3){
if (srv_cnt >= 16 || pca9685_i2c_status == PCA9685_I2C_STATUS_INITIALIZED) {
for (srv_cnt = 0; srv_cnt < 16; srv_cnt++) {
if (pca9865_reg_nb[srv_cnt] > 0) {
pca9685_i2c_status = PCA9865_I2C_STATUS_CHANGE_LED_REG_FINISHED;
break;
}
}
}
// Change the above found register in order to update the servo pwm value.
// The search for another servo to change is completed in the "event" function.
if (srv_cnt < 16) {
if (pca9685_i2c_status == PCA9865_I2C_STATUS_CHANGE_LED_REG_FINISHED) {
pca9685_i2c_trans.buf[0] = pca9865_reg_nb[srv_cnt];
pca9685_i2c_trans.buf[1] = (uint8_t)pca9865_write_servo_vals[srv_cnt];
pca9685_i2c_trans.buf[2] = (uint8_t)(pca9865_write_servo_vals[srv_cnt] >> 8) & 0x0F;
i2c_transmit(&PCA9685_I2C_DEV, &pca9685_i2c_trans, PCA9685_I2C_SLAVE_ADDR, 3);
pca9685_i2c_status = PCA9865_I2C_STATUS_WRITE_LED_REG_BUSY;
}
}
//}
return;
}
//444444444444444444444444444444444444444444444444444444444444444444444444444444444444444444444444444
void pca9685_i2c_event(void)
{
//--------------------------------------------------------------------------------------------------
// IF SUCCESFULL
//--------------------------------------------------------------------------------------------------
if (pca9685_i2c_trans.status == I2CTransSuccess) {
if (pca9685_i2c_status == PCA9865_I2C_STATUS_WRITE_LED_REG_BUSY) {
// Read back the changed value just to be sure...
pca9685_i2c_trans.buf[0] = pca9865_reg_nb[srv_cnt];
i2c_transceive(&PCA9685_I2C_DEV, &pca9685_i2c_trans, PCA9685_I2C_SLAVE_ADDR, 1, 2);
pca9685_i2c_status = PCA9865_I2C_STATUS_READ_LED_REG_BUSY;
} else if (pca9685_i2c_status == PCA9865_I2C_STATUS_READ_LED_REG_BUSY) {
#if defined(PCA9685_SEND_SERVO_VALUES) && PCA9685_SEND_SERVO_VALUES == 1
pca9865_read_servo_vals[srv_cnt] = BUF2INT(pca9685_i2c_trans.buf, 0);
#endif
if (BUF2INT(pca9685_i2c_trans.buf, 0) != pca9865_write_servo_vals[srv_cnt]) {
//pca9685_i2c_init();
return;
}
pca9685_i2c_status = PCA9865_I2C_STATUS_CHANGE_LED_REG_FINISHED;
// Delete this request.
pca9865_reg_nb[srv_cnt] = 0;
// Search for the next changed servo value.
// If a value > 0 is detected x will contain the new servo number to be changed.
srv_cnt++;
for (; srv_cnt < 16; srv_cnt++) { if (pca9865_reg_nb[srv_cnt] > 0) { break; } }
}
//-------------------------------------------------------------------------------------------------
// ELSE IF NOT SUCCESFULL
//-------------------------------------------------------------------------------------------------
}//else{ pca9685_i2c_init(); }
return;
}
#else
// just to stop the compiler complaining i manipulte srv_nb and srv_val a little.
bool pca9865_set_servo(uint8_t srv_nb, uint16_t srv_val) { srv_nb = srv_val; srv_nb /= 2; return (FALSE); }
void pca9685_i2c_init(void) { return; }
void pca9685_i2c_periodic(void) { return; }
void pca9685_i2c_event(void) { return; }
#endif // #if !defined(SITL)
/************************************************************************************************/
// T H E E N D
/************************************************************************************************/
sw/airborne/modules/pca9685/pca9685_i2c.h
+57
View File
@@ -0,0 +1,57 @@
/*
* Copyright (C) 2013-2020 Chris Efstathiou hendrixgr@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.
*/
/**
* @file modules/pca9685/pca9685.h
* PCA9685 LED DRIVER USED AS A 16 ADDITIONAL PWM SERVO DRIVER.
*
*/
#ifndef PCA9685_I2C_H
#define PCA9685_I2C_H
#include "std.h"
#define PCA9865_SRV0 PCA9685_LED0_OFF_L_REG_ADDR
#define PCA9865_SRV1 PCA9685_LED1_OFF_L_REG_ADDR
#define PCA9865_SRV2 PCA9685_LED2_OFF_L_REG_ADDR
#define PCA9865_SRV3 PCA9685_LED3_OFF_L_REG_ADDR
#define PCA9865_SRV4 PCA9685_LED4_OFF_L_REG_ADDR
#define PCA9865_SRV5 PCA9685_LED5_OFF_L_REG_ADDR
#define PCA9865_SRV6 PCA9685_LED6_OFF_L_REG_ADDR
#define PCA9865_SRV7 PCA9685_LED7_OFF_L_REG_ADDR
#define PCA9865_SRV8 PCA9685_LED8_OFF_L_REG_ADDR
#define PCA9865_SRV9 PCA9685_LED9_OFF_L_REG_ADDR
#define PCA9865_SRV10 PCA9685_LED10_OFF_L_REG_ADDR
#define PCA9865_SRV11 PCA9685_LED11_OFF_L_REG_ADDR
#define PCA9865_SRV12 PCA9685_LED12_OFF_L_REG_ADDR
#define PCA9865_SRV13 PCA9685_LED13_OFF_L_REG_ADDR
#define PCA9865_SRV14 PCA9685_LED14_OFF_L_REG_ADDR
#define PCA9865_SRV15 PCA9685_LED15_OFF_L_REG_ADDR
extern void pca9685_i2c_init(void);
extern void pca9685_i2c_periodic(void);
extern void pca9685_i2c_event(void);
bool pca9865_set_servo(uint8_t srv_nb, uint16_t srv_val);
#endif
sw/airborne/modules/sensors/baro_bmp280_i2c.c
+33 -6
View File
@@ -19,7 +19,7 @@
*/
/**
* @file modules/sensors/baro_bmp280.c
* @file modules/sensors/baro_bmp280_i2c.c
* Bosch BMP280 I2C sensor interface.
*
* This reads the values for pressure and temperature from the Bosch BMP280 sensor through I2C.
@@ -34,19 +34,44 @@
#include "subsystems/datalink/downlink.h"
#include "math/pprz_isa.h"
#if DOWNLINK && !defined(BMP280_SYNC_SEND)
#include "subsystems/datalink/telemetry.h"
#endif
/** default slave address */
#ifndef BMP280_SLAVE_ADDR
#define BMP280_SLAVE_ADDR BMP280_I2C_ADDR
#endif
float baro_alt = 0;
float baro_temp = 0;
float baro_press = 0;
bool baro_alt_valid = 0;
struct Bmp280_I2c baro_bmp280;
#if DOWNLINK && !defined(BMP280_SYNC_SEND)
static void send_baro_bmp_data(struct transport_tx *trans, struct link_device *dev)
{
int32_t baro_altitude = (int32_t)(baro_alt * 100);
int32_t baro_pressure = (int32_t)(baro_press);
int32_t baro_temperature = (int32_t)(baro_temp * 100);
pprz_msg_send_BMP_STATUS(trans, dev, AC_ID, &baro_altitude, &baro_altitude , &baro_pressure, & baro_temperature);
return;
}
#endif
void baro_bmp280_init(void)
{
bmp280_i2c_init(&baro_bmp280, &BMP280_I2C_DEV, BMP280_SLAVE_ADDR);
#if DOWNLINK && !defined(BMP280_SYNC_SEND)
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_BMP_STATUS, send_baro_bmp_data);
#endif
}
void baro_bmp280_periodic(void)
@@ -64,12 +89,14 @@ void baro_bmp280_event(void)
AbiSendMsgBARO_ABS(BARO_BMP_SENDER_ID, now_ts, baro_bmp280.pressure);
AbiSendMsgTEMPERATURE(BARO_BMP_SENDER_ID, baro_bmp280.temperature);
baro_bmp280.data_available = false;
baro_alt = pprz_isa_altitude_of_pressure((float)baro_bmp280.pressure);
baro_alt_valid = TRUE;
baro_alt = pprz_isa_altitude_of_pressure(baro_bmp280.pressure);
baro_alt_valid = true;
baro_press = (float)baro_bmp280.pressure;
baro_temp = ((float)baro_bmp280.temperature) / 100;
#ifdef BMP280_SYNC_SEND
int32_t up = (int32_t)(baro_alt * 100.0);
int32_t ut = (int32_t) baro_bmp280.raw_temperature;
#if defined(BMP280_SYNC_SEND)
int32_t up = (int32_t)(baro_bmp280.raw_pressure);
int32_t ut = (int32_t)(baro_bmp280.raw_temperature);
int32_t p = (int32_t) baro_bmp280.pressure;
int32_t t = (int32_t)(baro_bmp280.temperature);
DOWNLINK_SEND_BMP_STATUS(DefaultChannel, DefaultDevice, &up, &ut, &p, &t);
sw/airborne/modules/sensors/baro_bmp280_i2c.h
+3 -3
View File
@@ -20,14 +20,14 @@
*/
/**
* @file modules/sensors/baro_bmp280.h
* @file modules/sensors/baro_bmp280_i2c.h
* Bosch BMP280 I2C sensor interface.
*
* This reads the values for pressure and temperature from the Bosch BMP280 sensor through I2C.
*/
#ifndef BARO_BMP280_H
#define BARO_BMP280_H
#ifndef BARO_BMP280_I2C_H
#define BARO_BMP280_I2C_H
#include "peripherals/bmp280_i2c.h"
sw/airborne/modules/uav_recovery/uav_recovery.c
+289
View File
@@ -0,0 +1,289 @@
/*
* Copyright (C) 2013-2020 Chris Efstathiou hendrixgr@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.
*/
/**
* @file modules/uav_recovery/uav_recovery.c
*
*/
#include "autopilot.h"
#include "state.h"
#include "firmwares/fixedwing/nav.h"
#include "subsystems/navigation/common_nav.h"
#include "generated/flight_plan.h"
#include "generated/airframe.h"
#include "inter_mcu.h"
#include "subsystems/datalink/datalink.h"
#include "modules/multi/traffic_info.h"
#include "uav_recovery.h"
#if defined(USE_PARACHUTE) && USE_PARACHUTE == 1
#ifndef GLUE_DEFINITIONS_STEP2
#define GLUE_DEFINITIONS_STEP2(a, b) (a##b)
#endif
#ifndef GLUE_DEFINITIONS
#define GLUE_DEFINITIONS(a, b) GLUE_DEFINITIONS_STEP2(a, b)
#endif
#if defined(PARACHUTE_SERVO_CHANNEL) && PARACHUTE_SERVO_CHANNEL != -1
#define PARACHUTE_OUTPUT_COMMAND GLUE_DEFINITIONS(COMMAND_, PARACHUTE_SERVO_CHANNEL)
#else
#warning YOU HAVE NOT DEFINED A PARACHUTE RELEASE AUTOPILOT SERVO
#endif
#endif
/** TERMINAL VELOCITY OF THE PARACHUTE PLUS PAYLOAD */
#ifndef PARACHUTE_TRIGGER_DELAY
#define PARACHUTE_TRIGGER_DELAY 2.
#endif
#ifndef PARACHUTE_DESCENT_RATE
#define PARACHUTE_DESCENT_RATE 3.0
#endif
#ifndef PARACHUTE_WIND_CORRECTION
#define PARACHUTE_WIND_CORRECTION 1.0
#endif
#ifndef PARACHUTE_LINE_LENGTH
#define PARACHUTE_LINE_LENGTH 3.0
#endif
float parachute_start_qdr;
float parachute_z;
float airborne_wind_dir = 0;
float airborne_wind_speed = 0;
float calculated_wind_dir = 0;
bool wind_measurements_valid = true;
bool wind_info_valid = false;
bool deploy_parachute_var = 0;
bool land_direction = 0;
#if PERIODIC_TELEMETRY
#include "subsystems/datalink/telemetry.h"
static void send_wind_info(struct transport_tx *trans, struct link_device *dev)
{
float foo1 = 0, foo2 = 0, foo3 = 0;
pprz_msg_send_WEATHER(trans, dev, AC_ID, &foo1, &foo2, &airborne_wind_speed, &airborne_wind_dir, &foo3);
return;
}
#endif
void uav_recovery_init(void)
{
deploy_parachute_var = 0;
airborne_wind_dir = 0;
airborne_wind_speed = 0;
wind_measurements_valid = true;
wind_info_valid = true;
#if defined(PARACHUTE_OUTPUT_COMMAND)
ap_state->commands[PARACHUTE_OUTPUT_COMMAND] = MIN_PPRZ;
#endif
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_WEATHER, send_wind_info);
#endif
return;
}
void uav_recovery_periodic(void)
{
#if defined(PARACHUTE_OUTPUT_COMMAND)
if (autopilot.mode != AP_MODE_MANUAL) {
if (deploy_parachute_var == 1) {
ap_state->commands[PARACHUTE_OUTPUT_COMMAND] = MAX_PPRZ;
} else { ap_state->commands[PARACHUTE_OUTPUT_COMMAND] = MIN_PPRZ; }
}
#else
#warning PARACHUTE COMMAND NOT FOUND
#endif
return;
}
uint8_t LockParachute(void)
{
deploy_parachute_var = 0;
return (0);
}
uint8_t DeployParachute(void)
{
deploy_parachute_var = 1;
return (0);
}
uint8_t calculate_wind_no_airspeed(uint8_t wp, float radius, float height)
{
//struct EnuCoor_f* pos = stateGetPositionEnu_f();
//struct UtmCoor_f * pos = stateGetPositionUtm_f();
static bool init = false;
static float circle_count = 0;
static bool wind_measurement_started = false;
//Legacy estimator values
float estimator_hspeed_dir = 0;
float estimator_hspeed_mod = 0;
estimator_hspeed_dir = stateGetHorizontalSpeedDir_f();
estimator_hspeed_mod = stateGetHorizontalSpeedNorm_f();
float speed_max_buf = 0;
float speed_min_buf = 1000.0;
float heading_angle_buf = 0;
if (init == false) {
airborne_wind_dir = 0;
airborne_wind_speed = 0;
wind_measurements_valid = false;
speed_max_buf = 0;
speed_min_buf = 1000.;
heading_angle_buf = 0;
wind_measurement_started = false;
init = true;
}
NavVerticalAutoThrottleMode(RadOfDeg(0.000000));
NavVerticalAltitudeMode(Height(height), 0.);
NavCircleWaypoint(wp, radius);
if (nav_in_circle == false || GetPosAlt() < Height(height - 10)) {
return (true);
}
NavVerticalThrottleMode(MAX_PPRZ * (float)V_CTL_AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE);
// Wait until we are in altitude and in circle.
if (wind_measurement_started == false) {
circle_count = NavCircleCount();
wind_measurement_started = true;
}
if (estimator_hspeed_mod > speed_max_buf) {
speed_max_buf = estimator_hspeed_mod;
}
// WHEN THE GROUND SPEED IS LOWEST THIS IS WHERE THE WIND IS BLOWING FROM.
if (estimator_hspeed_mod < speed_min_buf) {
speed_min_buf = estimator_hspeed_mod;
heading_angle_buf = estimator_hspeed_dir; //180 to -180 in radians, 0 is to the NORTH!
}
if ((NavCircleCount() - circle_count) >= 1.1) {
//Update the wind direction and speed in the WEATHER message.
airborne_wind_speed = ((speed_max_buf - speed_min_buf) / 2) * AIRBORNE_WIND_CORRECTION;
#if defined(FIXED_WIND_SPEED_FOR_TESTING) && defined(SITL)
#pragma message "Wind SPEED for UAV recovery is fixed for the simulation"
#pragma message "You can change the value by editing 'uav_recovery.xml' file."
airborne_wind_speed = FIXED_WIND_SPEED_FOR_TESTING; // FOR TESTING IN SIMULATION ONLY!
#endif
#if defined(FIXED_WIND_DIRECTION_FOR_TESTING) && defined(SITL)
#pragma message "Wind direction for UAV recovery is fixed for the simulation"
#pragma message "You can change the value by editing 'uav_recovery.xml' file."
if (FIXED_WIND_DIRECTION_FOR_TESTING > 180.) {
heading_angle_buf = RadOfDeg(FIXED_WIND_DIRECTION_FOR_TESTING - 360.);
} else {
heading_angle_buf = RadOfDeg(FIXED_WIND_DIRECTION_FOR_TESTING);
}
#endif
airborne_wind_dir = DegOfRad(heading_angle_buf); //For use with the WEATHER message
//Convert from 180, -180 to 0, 360 in Degrees. 0=NORTH, 90=EAST
if (airborne_wind_dir < 0) { airborne_wind_dir += 360; }
calculated_wind_dir = heading_angle_buf;
wind_measurements_valid = true;
wind_info_valid = true;
//NavVerticalAutoThrottleMode(RadOfDeg(0.000000));
// EXIT and continue the flight plan.
init = false;
return (false);
}
return (true);
}
// Compute an approximation for the RELEASE waypoint from expected descent and altitude
unit_t parachute_compute_approach(uint8_t baseleg, uint8_t release, uint8_t wp_target)
{
float x = 0, y = 0;
float approach_dir = 0;
float calculated_wind_east = 0;
float calculated_wind_north = 0;
struct {
float x;
float y;
float a;
} start_wp;
// Calculate a starting waypoint opposite to the wind.
//180 TO -180 in Radians. Convert from 0 = NORTH to 0 = EAST
approach_dir = RadOfDeg(90.) - calculated_wind_dir;
if (approach_dir > M_PI) { approach_dir -= 2 * M_PI; }
approach_dir -= M_PI; // reverse the direction as we must release the parachute against the wind!
start_wp.x = (cos(approach_dir) * nav_radius) + waypoints[wp_target].x;
start_wp.y = (sin(approach_dir) * nav_radius) + waypoints[wp_target].y;
start_wp.a = waypoints[release].a;
// We approximate vx and vy, taking into account that RELEASE is close to the TARGET
float x_0 = waypoints[wp_target].x - start_wp.x;
float y_0 = waypoints[wp_target].y - start_wp.y;
// Unit vector from the calculated starting waypoint to TARGET
float d = sqrt(x_0 * x_0 + y_0 * y_0);
float x1 = x_0 / d;
float y_1 = y_0 / d;
waypoints[baseleg].x = start_wp.x + y_1 * nav_radius;
waypoints[baseleg].y = start_wp.y - x1 * nav_radius;
waypoints[baseleg].a = start_wp.a;
parachute_start_qdr = M_PI - atan2(-y_1, -x1);
if (nav_radius < 0) {
parachute_start_qdr += M_PI;
}
parachute_z = waypoints[release].a - ground_alt - PARACHUTE_LINE_LENGTH;
calculated_wind_north = cosf(calculated_wind_dir) * airborne_wind_speed;
calculated_wind_east = sinf(calculated_wind_dir) * airborne_wind_speed;
x = (parachute_z / PARACHUTE_DESCENT_RATE) * calculated_wind_east * PARACHUTE_WIND_CORRECTION;
y = (parachute_z / PARACHUTE_DESCENT_RATE) * calculated_wind_north * PARACHUTE_WIND_CORRECTION;
waypoints[release].x = (waypoints[wp_target].x + x);
waypoints[release].y = waypoints[wp_target].y + y;
return 0;
}
sw/airborne/modules/uav_recovery/uav_recovery.h
+48
View File
@@ -0,0 +1,48 @@
/*
* Copyright (C) 2013-2020 Chris Efstathiou hendrixgr@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.
*/
/**
* @file modules/uav_recovery/uav_recovery.h
*
*/
#if !defined(UAV_RECOVERY_H)
#define UAV_RECOVERY_H
#define MY_PARACHUTE_RADIUS DEFAULT_CIRCLE_RADIUS
extern unit_t parachute_compute_approach(uint8_t baseleg, uint8_t release, uint8_t wp_target);
extern float parachute_start_qdr;
extern bool deploy_parachute_var;
extern float airborne_wind_dir;
extern float airborne_wind_speed;
extern bool wind_measurements_valid;
extern bool wind_info_valid;
extern void uav_recovery_init(void);
extern void uav_recovery_periodic(void);
extern uint8_t DeployParachute(void);
extern uint8_t LockParachute(void);
extern uint8_t calculate_wind_no_airspeed(uint8_t wp, float radius, float height);
#define ParachuteComputeApproach(_baseleg, _release, _target) parachute_compute_approach(_baseleg, _release, _target)
#endif