Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-29 02:38:07 +08:00
Code Issues Actions 26 Packages Projects Releases Wiki Activity

Merge branch 'master' of github.com:paparazzi/paparazzi

Browse Source
This commit is contained in:
Felix Ruess
2011-06-18 12:54:10 +02:00
parent 7e0a1a96c5 d1c02eb87f
commit 0a93537c72
15 changed files with 543 additions and 173 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/TU_Delft/MicrojetBRimu.xml
+307
View File
@@ -0,0 +1,307 @@
<!DOCTYPE airframe SYSTEM "../airframe.dtd">
<airframe name="Microjet BR Tiny 1.1">
<!-- commands section -->
<servos>
<servo name="THROTTLE" no="0" min="1100" neutral="1100" max="1700"/>
<servo name="AILEVON_LEFT" no="1" min="2000" neutral="1500" max="1000"/>
<servo name="AILEVON_RIGHT" no="3" min="950" neutral="1450" max="1950"/>
<servo name="CAM_PAN" no="5" min="1950" neutral="1450" max="950"/>
<servo name="CAM_TILT" no="2" min="1000" neutral="1550" max="2000"/>
</servos>
<commands>
<axis name="THROTTLE" failsafe_value="0"/>
<axis name="ROLL" failsafe_value="0"/>
<axis name="PITCH" failsafe_value="0"/>
<axis name="CAM_TILT" failsafe_value="0"/>
<axis name="CAM_PAN" failsafe_value="0"/>
</commands>
<rc_commands>
<set command="THROTTLE" value="@THROTTLE"/>
<set command="ROLL" value="@ROLL"/>
<set command="PITCH" value="@PITCH"/>
<set command="CAM_PAN" value="@YAW"/>
<set command="CAM_TILT" value="@YAW"/>
</rc_commands>
<section name="MIXER">
<define name="AILEVON_AILERON_RATE" value="0.4"/>
<define name="AILEVON_ELEVATOR_RATE" value="0.7"/>
</section>
<command_laws>
<let var="aileron" value="@ROLL * AILEVON_AILERON_RATE"/>
<let var="elevator" value="@PITCH * AILEVON_ELEVATOR_RATE"/>
<set servo="THROTTLE" value="@THROTTLE"/>
<set servo="AILEVON_LEFT" value="$elevator + $aileron"/>
<set servo="AILEVON_RIGHT" value="$elevator - $aileron"/>
<set servo="CAM_PAN" value="@CAM_PAN"/>
<set servo="CAM_TILT" value="@CAM_TILT"/>
</command_laws>
<!-- Local magnetic field -->
<section name="AHRS" prefix="AHRS_">
<define name="H_X" value="0.51562740288882"/>
<define name="H_Y" value="-0.05707735220832"/>
<define name="H_Z" value="0.85490967783446"/>
</section>
<section name="IMU" prefix="IMU_">
<!-- 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."/>
<define name="GYRO_Q_R" value="0."/>
<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_NEUTRAL" value="-14"/>
<define name="ACCEL_Y_NEUTRAL" value="0"/>
<define name="ACCEL_Z_NEUTRAL" value="0"/>
<!-- SENS = 256 LSB/g @ 2.5V [X&Y: 265 LSB/g @ 3.3V] / 9.81 ms2/g = 26.095 LSB/ms2 / 10bit FRAC: 1024 / 26.095 for z and 1024 / 27.01 for X&Y -->
<define name="ACCEL_X_SENS" value="37.9" integer="16"/>
<define name="ACCEL_Y_SENS" value="37.9" integer="16"/>
<define name="ACCEL_Z_SENS" value="39.24" integer="16"/>
<define name="ACCEL_X_SIGN" value="-1"/>
<define name="ACCEL_Y_SIGN" value="-1"/>
<define name="ACCEL_Z_SIGN" value="1"/>
<define name="MAG_X_NEUTRAL" value="0"/>
<define name="MAG_Y_NEUTRAL" value="0"/>
<define name="MAG_Z_NEUTRAL" value="0"/>
<define name="MAG_X_SENS" value="1" integer="16"/>
<define name="MAG_Y_SENS" value="1" integer="16"/>
<define name="MAG_Z_SENS" value="1" integer="16"/>
<define name="MAG_X_SIGN" value="1"/>
<define name="MAG_Y_SIGN" value="1"/>
<define name="MAG_Z_SIGN" value="1"/>
<define name="BODY_TO_IMU_PHI" value="0"/>
<define name="BODY_TO_IMU_THETA" value="0"/>
<define name="BODY_TO_IMU_PSI" value="0"/>
</section>
<section name="INS" prefix="INS_">
<define name="ROLL_NEUTRAL_DEFAULT" value="0." unit="deg"/>
<define name="PITCH_NEUTRAL_DEFAULT" value="0." unit="deg"/>
</section>
<section name="AUTO1" prefix="AUTO1_">
<define name="MAX_ROLL" value="1.1"/>
<define name="MAX_PITCH" value="0.9"/>
</section>
<section name="BAT">
<define name="MILLIAMP_AT_FULL_THROTTLE" value="25000"/>
<define name="CATASTROPHIC_BAT_LEVEL" value="9.3" unit="V"/>
</section>
<section name="MISC">
<define name="NOMINAL_AIRSPEED" value="17." unit="m/s"/>
<define name="CARROT" value="5." unit="s"/>
<define name="KILL_MODE_DISTANCE" value="(1.5*MAX_DIST_FROM_HOME)"/>
<define name="CONTROL_RATE" value="60" unit="Hz"/>
<!-- <define name="XBEE_INIT" value="&quot;ATPL2\rATRN5\rATTT80\r&quot;"/> -->
<!-- <define name="NO_XBEE_API_INIT" value="TRUE"/> -->
<define name="ALT_KALMAN_ENABLED" value="TRUE"/>
<define name="DEFAULT_CIRCLE_RADIUS" value="80."/>
<define name="GLIDE_AIRSPEED" value="10"/>
<define name="GLIDE_VSPEED" value="3."/>
<define name="GLIDE_PITCH" value="45" unit="deg"/>
<define name="LIGHT_LED_1" value="3"/>
</section>
<section name="external leds">
<!-- ADC 4 -->
<define name="LED_4_BANK" value="0"/>
<define name="LED_4_PIN" value="22"/>
</section>
<section name="VERTICAL CONTROL" prefix="V_CTL_">
<define name="POWER_CTL_BAT_NOMINAL" value="11.1" unit="volt"/>
<!-- outer loop proportional gain -->
<define name="ALTITUDE_PGAIN" value="-0.101000003517"/>
<!-- outer loop saturation max m/s the alt tap can be-->
<define name="ALTITUDE_MAX_CLIMB" value="4"/>
<!-- auto throttle inner loop -->
<define name="AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE" value="0.333000004292"/>
<define name="AUTO_THROTTLE_MIN_CRUISE_THROTTLE" value="0.15"/>
<define name="AUTO_THROTTLE_MAX_CRUISE_THROTTLE" value="0.45"/>
<define name="AUTO_THROTTLE_LOITER_TRIM" value="219.511993408"/>
<define name="AUTO_THROTTLE_DASH_TRIM" value="-1200"/>
<define name="AUTO_THROTTLE_CLIMB_THROTTLE_INCREMENT" value="0.104999996722" unit="%/(m/s)"/>
<define name="AUTO_THROTTLE_PGAIN" value="-0.00700000021607"/>
<define name="AUTO_THROTTLE_IGAIN" value="0."/>
<define name="AUTO_THROTTLE_PITCH_OF_VZ_PGAIN" value="0.12800000608"/>
<!-- auto pitch inner loop -->
<define name="AUTO_PITCH_PGAIN" value="-0.0399999991059"/>
<define name="AUTO_PITCH_IGAIN" value="0.019999999553"/>
<define name="AUTO_PITCH_MAX_PITCH" value="0.35"/>
<define name="AUTO_PITCH_MIN_PITCH" value="-0.35"/>
<define name="THROTTLE_SLEW_LIMITER" value="2" unit="s"/>
</section>
<section name="HORIZONTAL CONTROL" prefix="H_CTL_">
<define name="COURSE_PGAIN" value="-1.54100000858"/>
<define name="ROLL_MAX_SETPOINT" value="0.916999995708" unit="radians"/>
<define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
<define name="PITCH_MIN_SETPOINT" value="-0.4" unit="radians"/>
<define name="ROLL_PGAIN" value="5000."/>
<define name="AILERON_OF_THROTTLE" value="0.0"/>
<define name="PITCH_PGAIN" value="-7330.82714844"/>
<define name="PITCH_DGAIN" value="1.7254999876"/>
<define name="ELEVATOR_OF_ROLL" value="1250."/>
<define name="ROLL_RATE_GAIN" value="-112.781997681"/>
<define name="ROLL_ATTITUDE_GAIN" value="-4957.62695312"/>
<!-- roll rate loop -->
<define name="ROLL_RATE_MODE_DEFAULT" value="1"/>
<define name="ROLL_RATE_SETPOINT_PGAIN" value="-10." unit="rad/s/rad"/>
<define name="ROLL_RATE_MAX_SETPOINT" value="10"/>
<define name="LO_THROTTLE_ROLL_RATE_PGAIN" value="3000."/>
<define name="HI_THROTTLE_ROLL_RATE_PGAIN" value="3000."/>
<define name="ROLL_RATE_IGAIN" value="0."/>
<define name="ROLL_RATE_DGAIN" value="0."/>
<define name="ROLL_RATE_SUM_NB_SAMPLES" value="64"/>
<define name="ROLL_SLEW" value="0.1"/>
<define name="SIMPLE_ROLL_PGAIN" value="-15000"/>
<define name="SIMPLE_ROLL_DGAIN" value="-3000"/>
</section>
<section name="NAV">
<define name="NAV_PITCH" value="0."/>
<define name="NAV_GLIDE_PITCH_TRIM" value="0."/>
<!-- <define name="NAV_GROUND_SPEED_PGAIN" value="-0.015"/> NOG niet in de nav.c file
<define name="NAV_FOLLOW_PGAIN" value="-0.05"/> -->
</section>
<section name="AGGRESSIVE" prefix="AGR_">
<define name="BLEND_START" value="50"/><!-- Altitude Error to Initiate Aggressive Climb CANNOT BE ZERO!!-->
<define name="BLEND_END" value="25"/><!-- Altitude Error to Blend Aggressive to Regular Climb Modes CANNOT BE ZERO!!-->
<define name="CLIMB_THROTTLE" value="0.55"/><!-- Gaz for Aggressive Climb -->
<define name="CLIMB_PITCH" value="0.5"/><!-- Pitch for Aggressive Climb -->
<define name="DESCENT_THROTTLE" value="0.0"/><!-- Gaz for Aggressive Decent -->
<define name="DESCENT_PITCH" value="-0.3"/><!-- Pitch for Aggressive Decent -->
<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="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="80" unit="m"/>
</section>
<section name="CAM" prefix="CAM_">
<define name="TILT_MAX" value="30" unit="deg"/>
<define name="TILT_NEUTRAL" value="0" unit="deg"/>
<define name="TILT_MIN" value="-30" unit="deg"/>
<define name="TILT0" value="0" unit="deg"/>
<define name="PAN_MAX" value="45" unit="deg"/>
<define name="PAN_NEUTRAL" value="0" unit="deg"/>
<define name="PAN_MIN" value="-45" unit="deg"/>
<define name="PAN0" value="0" unit="deg"/>
</section>
<!--moet nog in tuning-->
<section name="Takeoff" prefix="Takeoff_">
<define name="Height" value="30" unit="m"/>
<define name="Speed" value="15" unit="m/s"/>
<define name="MinSpeed" value="5" unit="m/s"/>
<define name="Distance" value="10" unit="m"/>
</section>
<section name="Landing" prefix="Landing_">
<define name="FinalHeight" value="30" unit="m"/>
<define name="AFHeight" value="10" unit="m"/>
<define name="FinalStageTime" value="10" unit="s"/>
</section>
<section name="SIMU">
<define name="YAW_RESPONSE_FACTOR" value="0.5"/> <!-- ?????????????????????????? -->
</section>
<modules>
<load name="light.xml">
<define name="LIGHT_LED_STROBE" value="3"/>
<define name="STROBE_LIGHT_MODE_DEFAULT" value="6"/>
</load>
</modules>
<firmware name="fixedwing">
<target name="ap" board="tiny_1.1">
<configure name="FLASH_MODE" value="IAP"/>
<configure name="PERIODIC_FREQUENCY" value="120"/>
<configure name="AHRS_PROPAGATE_FREQUENCY" value="100"/>
<configure name="AHRS_CORRECT_FREQUENCY" value="100"/>
<define name="AHRS_TRIGGERED_ATTITUDE_LOOP"/>
<configure name="AHRS_ALIGNER_LED" value="2"/>
<define name="STRONG_WIND" />
<define name="LOITER_TRIM" />
<define name="ALT_KALMAN" />
<define name="AGR_CLIMB" />
<define name="TUNE_AGRESSIVE_CLIMB" />
</target>
<target name="sim" board="pc" >
<define name="STRONG_WIND" />
<define name="TUNE_AGRESSIVE_CLIMB" />
<define name="LOITER_TRIM" />
<define name="AGR_CLIMB" />
</target>
<subsystem name="radio_control" type="ppm"/>
<!-- Communication -->
<subsystem name="telemetry" type="transparent">
<configure name="MODEM_BAUD" value="B57600"/>
</subsystem>
<!-- Sensors -->
<subsystem name="imu" type="ppzuav"/>
<subsystem name="attitude" type="dcm"/>
<subsystem name="i2c"/>
<subsystem name="gps" type="ublox_lea4p"/>
<subsystem name="control" />
<subsystem name="navigation" />
</firmware>
</airframe>
conf/messages.xml
+6 -1
View File
@@ -1696,7 +1696,11 @@
<field name="cpu_time" type="uint16" unit="s"></field>
</message>
<!--232 is free -->
<message name="STATE_FILTER_STATUS" id="232">
<field name="state_filter_mode" type="uint8" values="UNKNOWN|INIT|ALIGN|OK|GPS_LOST|IMU_LOST|COV_ERR|IR_CONTRAST|ERROR"/>
<field name="value" type="uint16" />
</message>
<!--233 is free -->
<message name="OPTICFLOW" id="234">
@@ -2140,6 +2144,7 @@
<field name="gps_mode" type="string" values="NOFIX|DRO|2D|3D|GPSDRO"/>
<field name="kill_mode" type="string" values="OFF|ON"/>
<field name="flight_time" type="uint32" />
<field name="state_filter_mode" type="string" values="UNKNOWN|INIT|ALIGN|OK|GPS_LOST|IMU_LOST|COV_ERR|IR_CONTRAST|ERROR"/>
</message>
<message name="NAV_STATUS" id="13">
conf/telemetry/default.xml
+61 -58
View File
@@ -3,74 +3,77 @@
<telemetry>
<process name="Ap">
<mode name="default">
<message name="AIRSPEED" period="1"/>
<message name="ALIVE" period="5"/>
<message name="GPS" period="0.25"/>
<message name="NAVIGATION" period="1."/>
<message name="ATTITUDE" period="0.5"/>
<message name="ESTIMATOR" period="0.5"/>
<message name="ENERGY" period="2.5"/>
<message name="WP_MOVED" period="0.5"/>
<message name="CIRCLE" period="1.05"/>
<message name="DESIRED" period="1.05"/>
<message name="BAT" period="1.1"/>
<message name="BARO_MS5534A" period="1.0"/>
<message name="SCP_STATUS" period="1.0"/>
<message name="SEGMENT" period="1.2"/>
<message name="CALIBRATION" period="2.1"/>
<message name="NAVIGATION_REF" period="9."/>
<message name="PPRZ_MODE" period="5."/>
<message name="DOWNLINK" period="5.1"/>
<message name="DL_VALUE" period="1.5"/>
<message name="IR_SENSORS" period="1.2"/>
<message name="GYRO_RATES" period="1.1"/>
<message name="SURVEY" period="2.1"/>
<message name="GPS_SOL" period="2.0"/>
<message name="AIRSPEED" period="1"/>
<message name="ALIVE" period="5"/>
<message name="GPS" period="0.25"/>
<message name="NAVIGATION" period="1."/>
<message name="ATTITUDE" period="0.5"/>
<message name="ESTIMATOR" period="0.5"/>
<message name="ENERGY" period="2.5"/>
<message name="WP_MOVED" period="0.5"/>
<message name="CIRCLE" period="1.05"/>
<message name="DESIRED" period="1.05"/>
<message name="BAT" period="1.1"/>
<message name="BARO_MS5534A" period="1.0"/>
<message name="SCP_STATUS" period="1.0"/>
<message name="SEGMENT" period="1.2"/>
<message name="CALIBRATION" period="2.1"/>
<message name="NAVIGATION_REF" period="9."/>
<message name="PPRZ_MODE" period="5."/>
<message name="STATE_FILTER_STATUS" period="5."/>
<message name="DOWNLINK" period="5.1"/>
<message name="DL_VALUE" period="1.5"/>
<message name="IR_SENSORS" period="1.2"/>
<message name="GYRO_RATES" period="1.1"/>
<message name="SURVEY" period="2.1"/>
<message name="GPS_SOL" period="2.0"/>
</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="CIRCLE" period="3.05"/>
<message name="DESIRED" period="4.05"/>
<message name="BAT" period="1.1"/>
<message name="SEGMENT" period="3.2"/>
<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="DOWNLINK" period="5.1"/>
<message name="DL_VALUE" period="1.5"/>
<message name="IR_SENSORS" period="5.2"/>
<message name="GYRO_RATES" period="10.1"/>
<message name="SURVEY" period="2.1"/>
<message name="GPS_SOL" period="5.0"/>
<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="CIRCLE" period="3.05"/>
<message name="DESIRED" period="4.05"/>
<message name="BAT" period="1.1"/>
<message name="SEGMENT" period="3.2"/>
<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="DOWNLINK" period="5.1"/>
<message name="DL_VALUE" period="1.5"/>
<message name="IR_SENSORS" period="5.2"/>
<message name="GYRO_RATES" period="10.1"/>
<message name="SURVEY" period="2.1"/>
<message name="GPS_SOL" period="5.0"/>
</mode>
<mode name="extremal">
<message name="ALIVE" period="5"/>
<message name="GPS" period="5.1"/>
<message name="ESTIMATOR" period="5.3"/>
<message name="BAT" period="10.1"/>
<message name="DESIRED" period="10.2"/>
<message name="NAVIGATION" period="5.4"/>
<message name="PPRZ_MODE" period="5.5"/>
<message name="DOWNLINK" period="5.7"/>
<message name="ALIVE" period="5"/>
<message name="GPS" period="5.1"/>
<message name="ESTIMATOR" period="5.3"/>
<message name="BAT" 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="DOWNLINK" period="5.7"/>
</mode>
</process>
<process name="Fbw">
<mode name="default">
<message name="COMMANDS" period="5"/>
<message name="FBW_STATUS" period="2"/>
<message name="ACTUATORS" period="5"/> <!-- For trimming -->
<message name="COMMANDS" period="5"/>
<message name="FBW_STATUS" period="2"/>
<message name="ACTUATORS" 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" period="5"/> <!-- For trimming -->
<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" period="5"/> <!-- For trimming -->
</mode>
</process>
</telemetry>
conf/telemetry/default_fixedwing_imu.xml
+83 -79
View File
@@ -3,97 +3,101 @@
<telemetry>
<process name="Ap">
<mode name="default">
<message name="AIRSPEED" period="1"/>
<message name="ALIVE" period="5"/>
<message name="GPS" period="0.25"/>
<message name="NAVIGATION" period="1."/>
<message name="ATTITUDE" period="0.1"/>
<message name="ESTIMATOR" period="0.5"/>
<message name="ENERGY" period="2.5"/>
<message name="WP_MOVED" period="0.5"/>
<message name="CIRCLE" period="1.05"/>
<message name="DESIRED" period="1.05"/>
<message name="BAT" period="1.1"/>
<message name="BARO_MS5534A" period="1.0"/>
<message name="SCP_STATUS" period="1.0"/>
<message name="SEGMENT" period="1.2"/>
<message name="CALIBRATION" period="2.1"/>
<message name="NAVIGATION_REF" period="9."/>
<message name="PPRZ_MODE" period="5."/>
<message name="DOWNLINK" period="5.1"/>
<message name="DL_VALUE" period="1.5"/>
<message name="IR_SENSORS" period="1.2"/>
<message name="GYRO_RATES" period="1.1"/>
<message name="SURVEY" period="2.1"/>
<message name="GPS_SOL" period="2.0"/>
<message name="IMU_ACCEL" period=".8"/>
<message name="IMU_GYRO" period=".6"/>
<message name="IMU_MAG" period="1.3"/>
<message name="IMU_ACCEL_RAW" period="5.2"/>
<message name="IMU_GYRO_RAW" period="5.3"/>
<message name="IMU_MAG_RAW" period="5.4"/>
<message name="AIRSPEED" period="1"/>
<message name="ALIVE" period="5"/>
<message name="GPS" period="0.25"/>
<message name="NAVIGATION" period="1."/>
<message name="ATTITUDE" period="0.1"/>
<message name="ESTIMATOR" period="0.5"/>
<message name="ENERGY" period="2.5"/>
<message name="WP_MOVED" period="0.5"/>
<message name="CIRCLE" period="1.05"/>
<message name="DESIRED" period="1.05"/>
<message name="BAT" period="1.1"/>
<message name="BARO_MS5534A" period="1.0"/>
<message name="SCP_STATUS" period="1.0"/>
<message name="SEGMENT" period="1.2"/>
<message name="CALIBRATION" period="2.1"/>
<message name="NAVIGATION_REF" period="9."/>
<message name="PPRZ_MODE" period="5."/>
<message name="STATE_FILTER_STATUS" period="2.2"/>
<message name="DOWNLINK" period="5.1"/>
<message name="DL_VALUE" period="1.5"/>
<message name="IR_SENSORS" period="1.2"/>
<message name="GYRO_RATES" period="1.1"/>
<message name="SURVEY" period="2.1"/>
<message name="GPS_SOL" period="2.0"/>
<message name="IMU_ACCEL" period=".8"/>
<message name="IMU_GYRO" period=".6"/>
<message name="IMU_MAG" period="1.3"/>
<message name="IMU_ACCEL_RAW" period="5.2"/>
<message name="IMU_GYRO_RAW" period="5.3"/>
<message name="IMU_MAG_RAW" period="5.4"/>
</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="CIRCLE" period="3.05"/>
<message name="DESIRED" period="4.05"/>
<message name="BAT" period="1.1"/>
<message name="SEGMENT" period="3.2"/>
<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="DOWNLINK" period="5.1"/>
<message name="DL_VALUE" period="1.5"/>
<message name="IR_SENSORS" period="5.2"/>
<message name="GYRO_RATES" period="10.1"/>
<message name="SURVEY" period="2.1"/>
<message name="GPS_SOL" period="5.0"/>
<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="CIRCLE" period="3.05"/>
<message name="DESIRED" period="4.05"/>
<message name="BAT" period="1.1"/>
<message name="SEGMENT" period="3.2"/>
<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="DOWNLINK" period="5.1"/>
<message name="DL_VALUE" period="1.5"/>
<message name="IR_SENSORS" period="5.2"/>
<message name="GYRO_RATES" period="10.1"/>
<message name="SURVEY" period="2.1"/>
<message name="GPS_SOL" period="5.0"/>
</mode>
<mode name="debug_imu">
<message name="ATTITUDE" period="0.1"/>
<message name="ALIVE" period="5"/>
<message name="GPS" period="5.1"/>
<message name="ESTIMATOR" period="5.3"/>
<message name="BAT" period="10.1"/>
<message name="DESIRED" period="10.2"/>
<message name="NAVIGATION" period="5.4"/>
<message name="PPRZ_MODE" period="5.5"/>
<message name="DOWNLINK" period="5.7"/>
<message name="IMU_ACCEL" period=".5"/>
<message name="IMU_GYRO" period=".5"/>
<message name="IMU_MAG" period=".5"/>
<message name="IMU_ACCEL_RAW" period=".5"/>
<message name="IMU_GYRO_RAW" period=".5"/>
<message name="IMU_MAG_RAW" period=".5"/>
<message name="ATTITUDE" period="0.1"/>
<message name="ALIVE" period="5"/>
<message name="GPS" period="5.1"/>
<message name="ESTIMATOR" period="5.3"/>
<message name="BAT" 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="5."/>
<message name="DOWNLINK" period="5.7"/>
<message name="IMU_ACCEL" period=".5"/>
<message name="IMU_GYRO" period=".5"/>
<message name="IMU_MAG" period=".5"/>
<message name="IMU_ACCEL_RAW" period=".5"/>
<message name="IMU_GYRO_RAW" period=".5"/>
<message name="IMU_MAG_RAW" period=".5"/>
</mode>
<mode name="extremal">
<message name="ALIVE" period="5"/>
<message name="GPS" period="5.1"/>
<message name="ESTIMATOR" period="5.3"/>
<message name="BAT" period="10.1"/>
<message name="DESIRED" period="10.2"/>
<message name="NAVIGATION" period="5.4"/>
<message name="PPRZ_MODE" period="5.5"/>
<message name="DOWNLINK" period="5.7"/>
<message name="ALIVE" period="5"/>
<message name="GPS" period="5.1"/>
<message name="ESTIMATOR" period="5.3"/>
<message name="BAT" period="10.1"/>
<message name="DESIRED" period="10.2"/>
<message name="NAVIGATION" period="5.4"/>
<message name="PPRZ_MODE" period="7.5"/>
<message name="STATE_FILTER_STATUS" period="8."/>
<message name="DOWNLINK" period="5.7"/>
</mode>
</process>
<process name="Fbw">
<mode name="default">
<message name="COMMANDS" period="5"/>
<message name="FBW_STATUS" period="2"/>
<message name="ACTUATORS" period="5"/> <!-- For trimming -->
<message name="COMMANDS" period="5"/>
<message name="FBW_STATUS" period="2"/>
<message name="ACTUATORS" 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" period="5"/> <!-- For trimming -->
<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" period="5"/> <!-- For trimming -->
</mode>
</process>
</telemetry>
sw/airborne/ap_downlink.h
+8
View File
@@ -93,6 +93,14 @@
#define PERIODIC_SEND_PPRZ_MODE(_chan) DOWNLINK_SEND_PPRZ_MODE(_chan, &pprz_mode, &v_ctl_mode, &lateral_mode, &horizontal_mode, &rc_settings_mode, &mcu1_status);
#define PERIODIC_SEND_DESIRED(_chan) DOWNLINK_SEND_DESIRED(_chan, &h_ctl_roll_setpoint, &h_ctl_pitch_loop_setpoint, &h_ctl_course_setpoint, &desired_x, &desired_y, &v_ctl_altitude_setpoint, &v_ctl_climb_setpoint);
#ifdef USE_AHRS
#define PERIODIC_SEND_STATE_FILTER_STATUS(_chan) { uint8_t mde = 3; if (ahrs.status == AHRS_UNINIT) mde = 2; if (ahrs_timeout_counter > 10) mde = 5; uint16_t val = 0; DOWNLINK_SEND_STATE_FILTER_STATUS(_chan, &mde, &val); }
#elif defined(USE_INFRARED)
#define PERIODIC_SEND_STATE_FILTER_STATUS(_chan) { uint16_t contrast = abs(infrared.roll) + abs(infrared.pitch) + abs(infrared.top); uint8_t mde = 3; if (contrast < 50) mde = 7; DOWNLINK_SEND_STATE_FILTER_STATUS(_chan, &mde, &contrast); }
#else
#define PERIODIC_SEND_STATE_FILTER_STATUS(_chan) {}
#endif
#define PERIODIC_SEND_NAVIGATION_REF(_chan) DOWNLINK_SEND_NAVIGATION_REF(_chan, &nav_utm_east0, &nav_utm_north0, &nav_utm_zone0);
sw/airborne/firmwares/fixedwing/main_ap.c
+5
View File
@@ -78,6 +78,7 @@
static inline void on_gyro_event( void );
static inline void on_accel_event( void );
static inline void on_mag_event( void );
volatile uint8_t ahrs_timeout_counter = 0;
#endif
#ifdef USE_GPS
static inline void on_gps_solution( void );
@@ -427,6 +428,8 @@ void periodic_task_ap( void ) {
#ifdef USE_IMU
// Run at PERIODIC_FREQUENCY (60Hz if not defined)
imu_periodic();
if (ahrs_timeout_counter < 255)
ahrs_timeout_counter ++;
#endif // USE_IMU
@@ -664,6 +667,8 @@ static inline void on_accel_event( void ) {
static inline void on_gyro_event( void ) {
ahrs_timeout_counter = 0;
#ifdef AHRS_CPU_LED
LED_ON(AHRS_CPU_LED);
#endif
sw/airborne/firmwares/fixedwing/main_fbw.c
+6 -2
View File
@@ -57,6 +57,10 @@ uint8_t fbw_mode;
#include "inter_mcu.h"
volatile uint8_t fbw_new_actuators = 0;
/********** INIT *************************************************************/
void init_fbw( void ) {
@@ -68,6 +72,7 @@ void init_fbw( void ) {
actuators_init();
/* Load the failsafe defaults */
SetCommands(commands_failsafe);
fbw_new_actuators = 1;
#endif
#ifdef RADIO_CONTROL
radio_control_init();
@@ -90,11 +95,10 @@ void init_fbw( void ) {
static inline void set_failsafe_mode( void ) {
fbw_mode = FBW_MODE_FAILSAFE;
SetCommands(commands_failsafe);
fbw_new_actuators = 1;
}
volatile uint8_t fbw_new_actuators = 0;
#ifdef RADIO_CONTROL
static inline void handle_rc_frame( void ) {
fbw_mode = FBW_MODE_OF_PPRZ(radio_control.values[RADIO_MODE]);
sw/airborne/inter_mcu.h
+11 -6
View File
@@ -120,22 +120,27 @@ static inline void inter_mcu_fill_fbw_state (void) {
fbw_state->vsupply = electrical.vsupply;
fbw_state->current = electrical.current;
}
/** Prepares date for next comm with AP. Set ::ap_ok to TRUE */
static inline void inter_mcu_event_task( void) {
time_since_last_ap = 0;
ap_ok = TRUE;
#if defined SINGLE_MCU
/**Directly set the flag indicating to AP that shared buffer is available*/
inter_mcu_received_fbw = TRUE;
#endif
}
/** Prepares date for next comm with AP. Set ::ap_ok to TRUE */
static inline void inter_mcu_event_task( void) {
time_since_last_ap = 0;
ap_ok = TRUE;
}
/** Monitors AP. Set ::ap_ok to false if AP is down for a long time. */
static inline void inter_mcu_periodic_task(void) {
if (time_since_last_ap >= AP_STALLED_TIME) {
ap_ok = FALSE;
#ifdef SINGLE_MCU
// Keep filling the buffer even if no AP commands are received
inter_mcu_fill_fbw_state();
#endif
} else
time_since_last_ap++;
}
sw/ground_segment/cockpit/live.ml
+6 -2
View File
@@ -778,7 +778,7 @@ let get_fbw_msg = fun alarm _sender vs ->
ac.strip#set_rc rate status;
let mode = Pprz.string_assoc "rc_mode" vs in
if mode = "FAILSAFE" then begin
log_and_say alarm ac.ac_name (sprintf "%s, mayday, AP Failure" ac.ac_name)
log_and_say alarm ac.ac_name (sprintf "%s, mayday, AP Failure. Switch to manual." ac.ac_name)
end
@@ -1170,7 +1170,11 @@ let listen_flight_params = fun geomap auto_center_new_ac alert alt_graph ->
| _ -> alert_color in
ac.strip#set_color "AP" color;
end;
let gps_mode = Pprz.string_assoc "gps_mode" vs in
let status_filter_mode = Pprz.string_assoc "state_filter_mode" vs in
let gps_mode =
if (status_filter_mode <> "UNKNOWN") && (status_filter_mode <> "OK") && (status_filter_mode <> "GPS_LOST")
then status_filter_mode
else Pprz.string_assoc "gps_mode" vs in
ac.strip#set_label "GPS" gps_mode;
ac.strip#set_color "GPS" (if gps_mode<>"3D" then alert_color else ok_color);
let ft =
sw/ground_segment/tmtc/aircraft.ml
+5 -2
View File
@@ -36,6 +36,7 @@ type fbw = {
mutable rc_status : rc_status;
mutable rc_mode : rc_mode;
mutable rc_rate : int;
mutable pprz_mode_msgs_since_last_fbw_status_msg : int;
}
let gps_nb_channels = 16
@@ -137,6 +138,7 @@ type aircraft = {
cam : ac_cam;
mutable gps_mode : int;
mutable gps_Pacc : int;
mutable state_filter_mode : int;
fbw : fbw;
svinfo : svinfo array;
waypoints : (int, waypoint) Hashtbl.t;
@@ -169,9 +171,10 @@ let new_aircraft = fun id name fp airframe ->
throttle = 0.; throttle_accu = 0.; rpm = 0.; temp = 0.; bat = 42.; amp = 0.; energy = 0; ap_mode= -1;
kill_mode = false;
gaz_mode= -1; lateral_mode= -1;
gps_mode =0; gps_Pacc = 0; periodic_callbacks = [];
gps_mode = 0; gps_Pacc = 0; periodic_callbacks = [];
state_filter_mode = 0;
cam = { phi = 0.; theta = 0. ; target=(0.,0.)};
fbw = { rc_status = "???"; rc_mode = "???"; rc_rate=0 };
fbw = { rc_status = "???"; rc_mode = "???"; rc_rate=0; pprz_mode_msgs_since_last_fbw_status_msg=0 };
svinfo = svsinfo_init;
dl_setting_values = Array.create max_nb_dl_setting_values 42.;
nb_dl_setting_values = 0;
sw/ground_segment/tmtc/aircraft.mli
+2 -1
View File
@@ -32,7 +32,7 @@ type ac_cam = {
type rc_status = string
type rc_mode = string
type fbw = { mutable rc_status : rc_status; mutable rc_mode : rc_mode; mutable rc_rate : int }
type fbw = { mutable rc_status : rc_status; mutable rc_mode : rc_mode; mutable rc_rate : int; mutable pprz_mode_msgs_since_last_fbw_status_msg : int; }
val gps_nb_channels : int
type svinfo = {
svid : int;
@@ -104,6 +104,7 @@ type aircraft = {
cam : ac_cam;
mutable gps_mode : int;
mutable gps_Pacc : int;
mutable state_filter_mode : int;
fbw : fbw;
svinfo : svinfo array;
waypoints : (int, waypoint) Hashtbl.t;
sw/ground_segment/tmtc/fw_server.ml
+35 -19
View File
@@ -185,34 +185,50 @@ let log_and_parse = fun ac_name (a:Aircraft.aircraft) msg values ->
a.energy <- ivalue "energy"
| "FBW_STATUS" ->
a.bat <- fvalue "vsupply" /. 10.;
a.fbw.pprz_mode_msgs_since_last_fbw_status_msg <- 0;
a.fbw.rc_rate <- ivalue "frame_rate";
let fbw_rc_mode = ivalue "rc_status" in
a.fbw.rc_status <- (
match fbw_rc_mode with
2 -> "NONE"
| 1 -> "LOST"
| _ -> "OK" );
let fbw_mode = ivalue "mode" in
a.fbw.rc_mode <-
if fbw_mode = 2
then "FAILSAFE"
else if fbw_mode = 1
then "AUTO"
else "MANUAL";
a.fbw.rc_mode <- (
match fbw_mode with
2 -> "FAILSAFE"
| 1 -> "AUTO"
| _ -> "MANUAL" )
| "STATE_FILTER_STATUS" ->
a.state_filter_mode <- check_index (ivalue "state_filter_mode") state_filter_modes "STATE_FILTER_MODES"
| "PPRZ_MODE" ->
a.vehicle_type <- FixedWing;
a.ap_mode <- check_index (ivalue "ap_mode") fixedwing_ap_modes "AP_MODE";
a.gaz_mode <- check_index (ivalue "ap_gaz") gaz_modes "AP_GAZ";
a.lateral_mode <- check_index (ivalue "ap_lateral") lat_modes "AP_LAT";
a.horizontal_mode <- check_index (ivalue "ap_horizontal") horiz_modes "AP_HORIZ";
let mcu1_status = ivalue "mcu1_status" in
(** c.f. link_autopilot.h *)
a.fbw.rc_status <-
if mcu1_status land 0b1 > 0
then "OK"
else if mcu1_status land 0b10 > 0
then "NONE"
else "LOST";
a.fbw.rc_mode <-
if mcu1_status land 0b1000 > 0
then "FAILSAFE"
else if mcu1_status land 0b100 > 0
then "AUTO"
else "MANUAL";
if a.fbw.pprz_mode_msgs_since_last_fbw_status_msg < 10 then
a.fbw.pprz_mode_msgs_since_last_fbw_status_msg <- a.fbw.pprz_mode_msgs_since_last_fbw_status_msg + 1;
(* If we have recent direct information from the FBW, and it says FAILSAFE: then do not thrust the AP message PPRZ_MODE *)
if ((a.fbw.pprz_mode_msgs_since_last_fbw_status_msg >= 10) && (a.fbw.rc_status <> "FAILSAFE")) then begin
a.fbw.rc_status <-
if mcu1_status land 0b1 > 0
then "OK"
else if mcu1_status land 0b10 > 0
then "NONE"
else "LOST";
a.fbw.rc_mode <-
if mcu1_status land 0b1000 > 0
then "FAILSAFE"
else if mcu1_status land 0b100 > 0
then "AUTO"
else "MANUAL";
end ;
if a.fbw.rc_mode = "FAILSAFE" then
a.ap_mode <- 5 (* Override and set FAIL(Safe) Mode *)
else
a.ap_mode <- check_index (ivalue "ap_mode") fixedwing_ap_modes "AP_MODE"
| "CAM" ->
a.cam.phi <- (Deg>>Rad) (fvalue "phi");
a.cam.theta <- (Deg>>Rad) (fvalue "theta");
sw/ground_segment/tmtc/rotorcraft_server.ml
+3 -1
View File
@@ -186,7 +186,9 @@ let log_and_parse = fun ac_name (a:Aircraft.aircraft) msg values ->
a.gps_mode <- check_index (ivalue "gps_status") gps_modes "GPS_MODE";
a.ap_mode <- check_index (ivalue "ap_mode") rotorcraft_ap_modes "ROTORCRAFT_AP_MODE";
a.kill_mode <- ivalue "ap_motors_on" == 0;
a.bat <- fvalue "vsupply" /. 10.;
a.bat <- fvalue "vsupply" /. 10.
| "STATE_FILTER_STATUS" ->
a.state_filter_mode <- check_index (ivalue "state_filter_mode") state_filter_modes "STATE_FILTER_MODES"
| "INS_REF" ->
let x = foi32value "ecef_x0" /. 100.
and y = foi32value "ecef_y0" /. 100.
sw/ground_segment/tmtc/server.ml
+3 -1
View File
@@ -365,7 +365,8 @@ let send_aircraft_msg = fun ac ->
let gaz_mode = get_indexed_value gaz_modes a.gaz_mode in
let lat_mode = get_indexed_value lat_modes a.lateral_mode in
let horiz_mode = get_indexed_value horiz_modes a.horizontal_mode in
let gps_mode = get_indexed_value gps_modes a.gps_mode
let gps_mode = get_indexed_value gps_modes a.gps_mode in
let state_filter_mode = get_indexed_value state_filter_modes a.state_filter_mode
and kill_mode = if a.kill_mode then "ON" else "OFF" in
let values = ["ac_id", Pprz.String ac;
"flight_time", Pprz.Int a.flight_time;
@@ -374,6 +375,7 @@ let send_aircraft_msg = fun ac ->
"lat_mode", Pprz.String lat_mode;
"horiz_mode", Pprz.String horiz_mode;
"gps_mode", Pprz.String gps_mode;
"state_filter_mode", Pprz.String state_filter_mode;
"kill_mode", Pprz.String kill_mode
] in
Ground_Pprz.message_send my_id "AP_STATUS" values;
sw/ground_segment/tmtc/server_globals.ml
+2 -1
View File
@@ -3,12 +3,13 @@ exception Telemetry_error of string * string
let hostname = ref "localhost"
(** FIXME: Should be read from messages.xml *)
let fixedwing_ap_modes = [|"MANUAL";"AUTO1";"AUTO2";"HOME";"NOGPS"|]
let fixedwing_ap_modes = [|"MANUAL";"AUTO1";"AUTO2";"HOME";"NOGPS";"FAIL"|]
let rotorcraft_ap_modes = [|"SAFE";"KILL";"RATE";"ATT";"R_RCC";"A_RCC";"ATT_C";"R_ZH";"A_ZH";"HOVER";"HOV_C";"H_ZH";"NAV"|]
let _AUTO2 = 2
let gaz_modes = [|"MANUAL";"GAZ";"CLIMB";"ALT"|]
let lat_modes = [|"MANUAL";"ROLL_RATE";"ROLL";"COURSE"|]
let gps_modes = [|"NOFIX";"DRO";"2D";"3D";"GPSDRO"|]
let state_filter_modes = [|"UNKNOWN";"INIT";"ALIGN";"OK";"GPS_LOST";"IMU_LOST";"COV_ERR";"IR_CONTRAST";"ERROR"|]
let _3D = 3
let gps_hybrid_modes = [|"OFF";"ON"|]
let horiz_modes = [|"WAYPOINT";"ROUTE";"CIRCLE";"ATTITUDE"|]