mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-06-04 22:17:01 +08:00
Merge remote-tracking branch 'paparazzi/master' into telemetry
Conflicts: sw/airborne/firmwares/fixedwing/fbw_downlink.h
This commit is contained in:
Gautier Hattenberger
@@ -23,19 +23,17 @@
|
||||
<define name="USE_GYRO_PITCH_RATE"/>
|
||||
|
||||
<target name="sim" board="pc"/>
|
||||
<target name="ap" board="apogee_0.99"/>
|
||||
<configure name="FLASH_MODE" value="SWD"/>
|
||||
<target name="ap" board="apogee_1.0"/>
|
||||
<!--configure name="FLASH_MODE" value="SWD"/-->
|
||||
|
||||
<subsystem name="radio_control" type="ppm"/>
|
||||
<subsystem name="radio_control" type="sbus"/>
|
||||
|
||||
<!-- Communication -->
|
||||
<subsystem name="telemetry" type="transparent"/>
|
||||
|
||||
<!-- Actuators are automatically chosen according to board-->
|
||||
<subsystem name="imu" type="apogee"/>
|
||||
<subsystem name="ahrs" type="float_dcm">
|
||||
<define name="USE_HIGH_ACCEL_FLAG"/>
|
||||
</subsystem>
|
||||
<subsystem name="ahrs" type="float_dcm"/>
|
||||
<subsystem name="ins" type="alt_float"/>
|
||||
<subsystem name="control"/>
|
||||
<subsystem name="navigation"/>
|
||||
|
||||
@@ -26,9 +26,7 @@
|
||||
<subsystem name="radio_control" type="ppm"/>
|
||||
<subsystem name="telemetry" type="transparent"/>
|
||||
<subsystem name="imu" type="umarim"/>
|
||||
<subsystem name="ahrs" type="float_dcm">
|
||||
<define name="USE_HIGH_ACCEL_FLAG"/>
|
||||
</subsystem>
|
||||
<subsystem name="ahrs" type="float_dcm"/>
|
||||
<subsystem name="ins" type="alt_float"/>
|
||||
<subsystem name="control"/>
|
||||
<subsystem name="gps" type="ublox"/>
|
||||
|
||||
@@ -32,9 +32,7 @@
|
||||
|
||||
<!-- Actuators are automatically chosen according to board-->
|
||||
<subsystem name="imu" type="umarim"/>
|
||||
<subsystem name="ahrs" type="float_dcm">
|
||||
<define name="USE_HIGH_ACCEL_FLAG"/>
|
||||
</subsystem>
|
||||
<subsystem name="ahrs" type="float_dcm"/>
|
||||
<subsystem name="ins" type="alt_float">
|
||||
<!--define name="USE_BAROMETER"/-->
|
||||
</subsystem>
|
||||
|
||||
@@ -39,9 +39,7 @@
|
||||
|
||||
<!-- Actuators are automatically chosen according to board-->
|
||||
<subsystem name="imu" type="umarim"/>
|
||||
<subsystem name="ahrs" type="float_dcm">
|
||||
<define name="USE_HIGH_ACCEL_FLAG"/>
|
||||
</subsystem>
|
||||
<subsystem name="ahrs" type="float_dcm"/>
|
||||
<subsystem name="ins" type="alt_float"/>
|
||||
<subsystem name="control" type="new">
|
||||
<define name="USE_GYRO_PITCH_RATE"/>
|
||||
|
||||
@@ -66,9 +66,7 @@
|
||||
|
||||
<!-- Actuators are automatically chosen according to board-->
|
||||
<subsystem name="imu" type="umarim"/>
|
||||
<subsystem name="ahrs" type="float_dcm">
|
||||
<define name="USE_HIGH_ACCEL_FLAG"/>
|
||||
</subsystem>
|
||||
<subsystem name="ahrs" type="float_dcm"/>
|
||||
<subsystem name="ins" type="alt_float"/>
|
||||
<subsystem name="control" type="new"/>
|
||||
<subsystem name="navigation"/>
|
||||
|
||||
@@ -20,26 +20,27 @@
|
||||
<define name="USE_I2C1"/>
|
||||
<!--define name="AGR_CLIMB"/-->
|
||||
<define name="ALT_KALMAN"/>
|
||||
<define name="USE_AIRSPEED"/>
|
||||
<!--define name="USE_AIRSPEED"/-->
|
||||
<!--define name="USE_BAROMETER"/-->
|
||||
<!--define name="LOITER_TRIM"/-->
|
||||
<!--define name="PITCH_TRIM"/-->
|
||||
|
||||
<target name="sim" board="pc"/>
|
||||
<target name="ap" board="umarim_1.0">
|
||||
<configure name="FLASH_MODE" value="IAP"/>
|
||||
</target>
|
||||
<target name="ap" board="umarim_1.0"/>
|
||||
|
||||
<subsystem name="radio_control" type="ppm"/>
|
||||
<subsystem name="radio_control" type="ppm">
|
||||
<define name="USE_PPM_RSSI_GPIO"/>
|
||||
<define name="PPM_RSSI_IOPIN" value="IO0PIN"/>
|
||||
<define name="PPM_RSSI_PIN" value="17"/>
|
||||
<define name="PPM_RSSI_VALID_LEVEL" value="0"/>
|
||||
</subsystem>
|
||||
|
||||
<!-- Communication -->
|
||||
<subsystem name="telemetry" type="xbee_api"/>
|
||||
|
||||
<!-- Actuators are automatically chosen according to board-->
|
||||
<subsystem name="imu" type="umarim"/>
|
||||
<subsystem name="ahrs" type="float_dcm">
|
||||
<define name="USE_HIGH_ACCEL_FLAG"/>
|
||||
</subsystem>
|
||||
<subsystem name="imu" type="umarim"/>
|
||||
<subsystem name="ahrs" type="float_dcm"/>
|
||||
<subsystem name="ins" type="alt_float"/>
|
||||
<subsystem name="control" type="new"/>
|
||||
<subsystem name="navigation"/>
|
||||
@@ -49,17 +50,11 @@
|
||||
<!--subsystem name="spi"/-->
|
||||
</firmware>
|
||||
|
||||
|
||||
<firmware name="setup">
|
||||
<target name="setup_actuators" board="umarim_1.0"/>
|
||||
<target name="tunnel" board="umarim_1.0"/>
|
||||
</firmware>
|
||||
|
||||
<!-- commands section -->
|
||||
<servos>
|
||||
<servo name="MOTOR" no="7" min="1040" neutral="1040" max="2000"/>
|
||||
<servo name="AILEVON_LEFT" no="6" min="1900" neutral="1543" max="1100"/>
|
||||
<servo name="AILEVON_RIGHT" no="3" min="1100" neutral="1561" max="1900"/>
|
||||
<servo name="AILEVON_LEFT" no="6" min="1750" neutral="1543" max="1250"/>
|
||||
<servo name="AILEVON_RIGHT" no="3" min="1250" neutral="1561" max="1750"/>
|
||||
</servos>
|
||||
|
||||
<commands>
|
||||
@@ -83,13 +78,13 @@
|
||||
<let var="aileron" value="@ROLL * AILEVON_AILERON_RATE"/>
|
||||
<let var="elevator" value="@PITCH * AILEVON_ELEVATOR_RATE"/>
|
||||
<set servo="MOTOR" value="@THROTTLE"/>
|
||||
<set servo="AILEVON_LEFT" value="$elevator + $aileron"/>
|
||||
<set servo="AILEVON_RIGHT" value="$elevator - $aileron"/>
|
||||
<set servo="AILEVON_LEFT" value="$elevator - $aileron"/>
|
||||
<set servo="AILEVON_RIGHT" value="$elevator + $aileron"/>
|
||||
</command_laws>
|
||||
|
||||
<section name="AUTO1" prefix="AUTO1_">
|
||||
<define name="MAX_ROLL" value="0.85"/>
|
||||
<define name="MAX_PITCH" value="0.6"/>
|
||||
<define name="MAX_ROLL" value="45" unit="deg"/>
|
||||
<define name="MAX_PITCH" value="30" unit="deg"/>
|
||||
</section>
|
||||
|
||||
<section name="IMU" prefix="IMU_">
|
||||
@@ -123,8 +118,8 @@
|
||||
</section>
|
||||
|
||||
<section name="INS" prefix="INS_">
|
||||
<define name="ROLL_NEUTRAL_DEFAULT" value="0.048" unit="rad"/>
|
||||
<define name="PITCH_NEUTRAL_DEFAULT" value="0." unit="rad"/>
|
||||
<define name="ROLL_NEUTRAL_DEFAULT" value="3." unit="deg"/>
|
||||
<define name="PITCH_NEUTRAL_DEFAULT" value="0." unit="deg"/>
|
||||
<define name="BARO_SENS" value="1.0"/> <!-- TODO calibration -->
|
||||
</section>
|
||||
|
||||
@@ -161,15 +156,15 @@
|
||||
|
||||
<!-- Climb loop (throttle) -->
|
||||
<define name="AUTO_THROTTLE_CLIMB_THROTTLE_INCREMENT" value="0.1" unit="%/(m/s)"/>
|
||||
<define name="AUTO_THROTTLE_PGAIN" value="0.004"/> <!-- -0.005 -->
|
||||
<define name="AUTO_THROTTLE_DGAIN" value="0.01"/> <!-- 0.005 -->
|
||||
<define name="AUTO_THROTTLE_IGAIN" value="0.004"/> <!-- 0.005 -->
|
||||
<define name="AUTO_THROTTLE_PGAIN" value="0.003"/> <!-- -0.005 -->
|
||||
<define name="AUTO_THROTTLE_DGAIN" value="0.007"/> <!-- 0.005 -->
|
||||
<define name="AUTO_THROTTLE_IGAIN" value="0.002"/> <!-- 0.005 -->
|
||||
|
||||
<!-- Climb loop (pitch) -->
|
||||
<define name="AUTO_THROTTLE_PITCH_OF_VZ_PGAIN" value="0.02"/>
|
||||
<define name="AUTO_PITCH_PGAIN" value="0.038"/> <!-- -0.03 -->
|
||||
<define name="AUTO_PITCH_DGAIN" value="0.036"/> <!-- -0.03 -->
|
||||
<define name="AUTO_PITCH_IGAIN" value="0.0"/>
|
||||
<define name="AUTO_PITCH_PGAIN" value="0.006"/> <!-- -0.03 -->
|
||||
<define name="AUTO_PITCH_DGAIN" value="0.0"/> <!-- -0.03 -->
|
||||
<define name="AUTO_PITCH_IGAIN" value="0.002"/>
|
||||
|
||||
<!-- airspeed control -->
|
||||
<define name="AUTO_AIRSPEED_SETPOINT" value="16."/>
|
||||
@@ -206,8 +201,8 @@
|
||||
<define name="ROLL_KFFA" value="0"/>
|
||||
<define name="ROLL_KFFD" value="0"/>
|
||||
|
||||
<define name="PITCH_PGAIN" value="10000."/>
|
||||
<define name="PITCH_DGAIN" value="0."/>
|
||||
<define name="PITCH_PGAIN" value="8000"/>
|
||||
<define name="PITCH_DGAIN" value="500"/>
|
||||
<define name="PITCH_IGAIN" value="250."/>
|
||||
<define name="PITCH_KFFA" value="0."/>
|
||||
<define name="PITCH_KFFD" value="0."/>
|
||||
@@ -234,7 +229,7 @@
|
||||
<define name="MAX_ROLL_DOT" value="20" unit="rad/s"/-->
|
||||
<define name="JSBSIM_MODEL" value=""Malolo1""/>
|
||||
<!--define name="JSBSIM_INIT" value=""Malolo1-IC""/-->
|
||||
<define name="JSBSIM_LAUNCHSPEED" value="15.0"/>
|
||||
<define name="JSBSIM_LAUNCHSPEED" value="15.0"/>
|
||||
<define name="JSBSIM_IR_ROLL_NEUTRAL" value="0." unit="deg"/>
|
||||
<define name="JSBSIM_IR_PITCH_NEUTRAL" value="0." unit="deg"/>
|
||||
</section>
|
||||
|
||||
@@ -213,7 +213,7 @@
|
||||
<define name="IGAIN" value="15"/>
|
||||
<define name="NGAIN" value="0"/>
|
||||
<!-- feedforward -->
|
||||
<define name="AGAIN" value="100"/>
|
||||
<define name="AGAIN" value="0"/>
|
||||
</section>
|
||||
|
||||
<section name="BAT">
|
||||
|
||||
@@ -3,11 +3,11 @@
|
||||
<airframe name="Hummingbird (Asctec) Enac-Navgo 1">
|
||||
|
||||
<modules main_freq="512">
|
||||
<load name="mavlink_decoder.xml">
|
||||
<!--load name="mavlink_decoder.xml">
|
||||
<configure name="MAVLINK_PORT" value="UART0"/>
|
||||
<configure name="MAVLINK_BAUD" value="B115200"/>
|
||||
</load>
|
||||
<load name="px4flow.xml"/>
|
||||
<load name="px4flow.xml"/-->
|
||||
<!--load name="adc_generic.xml">
|
||||
<configure name="ADC_CHANNEL_GENERIC1" value="ADC_0"/>
|
||||
</load-->
|
||||
@@ -23,23 +23,23 @@
|
||||
<define name="ACTUATORS_START_DELAY" value="3"/>
|
||||
</target>
|
||||
<target name="nps" board="pc">
|
||||
<subsystem name="fdm" type="jsbsim"/>
|
||||
<subsystem name="fdm" type="jsbsim"/>
|
||||
</target>
|
||||
|
||||
<subsystem name="radio_control" type="ppm"/>
|
||||
<subsystem name="telemetry" type="transparent"/>
|
||||
<subsystem name="telemetry" type="xbee_api"/>
|
||||
<!--subsystem name="actuators" type="skiron">
|
||||
<define name="SKIRON_I2C_SCL_TIME" value="25"/>
|
||||
</subsystem-->
|
||||
<subsystem name="actuators" type="asctec_v2"/>
|
||||
<subsystem name="actuators" type="asctec_v2"/>
|
||||
<subsystem name="motor_mixing"/>
|
||||
<subsystem name="imu" type="navgo"/>
|
||||
<!--subsystem name="gps" type="ublox">
|
||||
<subsystem name="imu" type="navgo"/>
|
||||
<subsystem name="gps" type="ublox">
|
||||
<configure name="GPS_BAUD" value="B57600"/>
|
||||
</subsystem-->
|
||||
<subsystem name="stabilization" type="euler"/>
|
||||
<subsystem name="ahrs" type="int_cmpl_quat"/>
|
||||
<subsystem name="ins" type="hff"/>
|
||||
</subsystem>
|
||||
<subsystem name="stabilization" type="int_euler"/>
|
||||
<subsystem name="ahrs" type="int_cmpl_quat"/>
|
||||
<subsystem name="ins"/>
|
||||
</firmware>
|
||||
|
||||
<section name="MIXING" prefix="MOTOR_MIXING_">
|
||||
@@ -48,9 +48,9 @@
|
||||
<define name="TRIM_YAW" value="0"/>
|
||||
<define name="NB_MOTOR" value="4"/>
|
||||
<define name="SCALE" value="256"/>
|
||||
<define name="ROLL_COEF" value="{ 0 , 0, 256, -256 }"/>
|
||||
<define name="PITCH_COEF" value="{ 256, -256, 0, 0 }"/>
|
||||
<define name="YAW_COEF" value="{ -256, -256, 256, 256 }"/>
|
||||
<define name="ROLL_COEF" value="{ 0 , 0, 256, -256 }"/>
|
||||
<define name="PITCH_COEF" value="{ 256, -256, 0, 0 }"/>
|
||||
<define name="YAW_COEF" value="{ -256, -256, 256, 256 }"/>
|
||||
<define name="THRUST_COEF" value="{ 256, 256, 256, 256 }"/>
|
||||
</section>
|
||||
|
||||
@@ -62,25 +62,25 @@
|
||||
</servos-->
|
||||
|
||||
<servos driver="Asctec_v2">
|
||||
<servo name="FRONT" no="0" min="0" neutral="1" max="200"/>
|
||||
<servo name="BACK" no="1" min="0" neutral="1" max="200"/>
|
||||
<servo name="LEFT" no="2" min="0" neutral="1" max="200"/>
|
||||
<servo name="RIGHT" no="3" min="0" neutral="1" max="200"/>
|
||||
<servo name="FRONT" no="0" min="0" neutral="1" max="200"/>
|
||||
<servo name="BACK" no="1" min="0" neutral="1" max="200"/>
|
||||
<servo name="LEFT" no="2" min="0" neutral="1" max="200"/>
|
||||
<servo name="RIGHT" no="3" min="0" neutral="1" max="200"/>
|
||||
</servos>
|
||||
|
||||
<commands>
|
||||
<axis name="PITCH" failsafe_value="0"/>
|
||||
<axis name="ROLL" failsafe_value="0"/>
|
||||
<axis name="YAW" failsafe_value="0"/>
|
||||
<axis name="PITCH" failsafe_value="0"/>
|
||||
<axis name="ROLL" failsafe_value="0"/>
|
||||
<axis name="YAW" failsafe_value="0"/>
|
||||
<axis name="THRUST" failsafe_value="0"/>
|
||||
</commands>
|
||||
|
||||
<command_laws>
|
||||
<call fun="motor_mixing_run(autopilot_motors_on,FALSE,values)"/>
|
||||
<set servo="FRONT" value="motor_mixing.commands[SERVO_FRONT]"/>
|
||||
<set servo="BACK" value="motor_mixing.commands[SERVO_BACK]"/>
|
||||
<set servo="RIGHT" value="motor_mixing.commands[SERVO_RIGHT]"/>
|
||||
<set servo="LEFT" value="motor_mixing.commands[SERVO_LEFT]"/>
|
||||
<set servo="FRONT" value="motor_mixing.commands[SERVO_FRONT]"/>
|
||||
<set servo="BACK" value="motor_mixing.commands[SERVO_BACK]"/>
|
||||
<set servo="RIGHT" value="motor_mixing.commands[SERVO_RIGHT]"/>
|
||||
<set servo="LEFT" value="motor_mixing.commands[SERVO_LEFT]"/>
|
||||
</command_laws>
|
||||
|
||||
<section name="IMU" prefix="IMU_">
|
||||
@@ -110,9 +110,9 @@
|
||||
<define name="MAG_Y_SENS" value="4.23614065134" integer="16"/>
|
||||
<define name="MAG_Z_SENS" value="4.937732599" integer="16"/>
|
||||
|
||||
<define name="BODY_TO_IMU_PHI" value="0." unit="deg"/>
|
||||
<define name="BODY_TO_IMU_PHI" value="0." unit="deg"/>
|
||||
<define name="BODY_TO_IMU_THETA" value="0." unit="deg"/>
|
||||
<define name="BODY_TO_IMU_PSI" value="0." unit="deg"/>
|
||||
<define name="BODY_TO_IMU_PSI" value="0." unit="deg"/>
|
||||
|
||||
</section>
|
||||
|
||||
@@ -146,76 +146,77 @@
|
||||
<section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
|
||||
|
||||
<!-- setpoints -->
|
||||
<define name="SP_MAX_PHI" value="45." unit="deg"/>
|
||||
<define name="SP_MAX_THETA" value="45." unit="deg"/>
|
||||
<define name="SP_MAX_R" value="90." unit="deg/s"/>
|
||||
<define name="DEADBAND_R" value="250"/>
|
||||
<define name="SP_MAX_PHI" value="45." unit="deg"/>
|
||||
<define name="SP_MAX_THETA" value="45." unit="deg"/>
|
||||
<define name="SP_MAX_R" value="90." unit="deg/s"/>
|
||||
<define name="DEADBAND_R" value="250"/>
|
||||
|
||||
<!-- reference -->
|
||||
<define name="REF_OMEGA_P" value="800" unit="deg/s"/>
|
||||
<define name="REF_ZETA_P" value="0.85"/>
|
||||
<define name="REF_MAX_P" value="300." unit="deg/s"/>
|
||||
<define name="REF_OMEGA_P" value="800" unit="deg/s"/>
|
||||
<define name="REF_ZETA_P" value="0.85"/>
|
||||
<define name="REF_MAX_P" value="300." unit="deg/s"/>
|
||||
<define name="REF_MAX_PDOT" value="RadOfDeg(7000.)"/>
|
||||
|
||||
<define name="REF_OMEGA_Q" value="800" unit="deg/s"/>
|
||||
<define name="REF_ZETA_Q" value="0.85"/>
|
||||
<define name="REF_MAX_Q" value="300." unit="deg/s"/>
|
||||
<define name="REF_OMEGA_Q" value="800" unit="deg/s"/>
|
||||
<define name="REF_ZETA_Q" value="0.85"/>
|
||||
<define name="REF_MAX_Q" value="300." unit="deg/s"/>
|
||||
<define name="REF_MAX_QDOT" value="RadOfDeg(7000.)"/>
|
||||
|
||||
<define name="REF_OMEGA_R" value="500" unit="deg/s"/>
|
||||
<define name="REF_ZETA_R" value="0.85"/>
|
||||
<define name="REF_MAX_R" value="90." unit="deg/s"/>
|
||||
<define name="REF_OMEGA_R" value="500" unit="deg/s"/>
|
||||
<define name="REF_ZETA_R" value="0.85"/>
|
||||
<define name="REF_MAX_R" value="90." unit="deg/s"/>
|
||||
<define name="REF_MAX_RDOT" value="RadOfDeg(900.)"/>
|
||||
|
||||
<!-- feedback -->
|
||||
<define name="PHI_PGAIN" value="1500"/>
|
||||
<define name="PHI_DGAIN" value="200"/>
|
||||
<define name="PHI_IGAIN" value="200"/>
|
||||
<define name="PHI_PGAIN" value="1272"/>
|
||||
<define name="PHI_DGAIN" value="132"/>
|
||||
<define name="PHI_IGAIN" value="146"/>
|
||||
|
||||
<define name="THETA_PGAIN" value="1500"/>
|
||||
<define name="THETA_DGAIN" value="200"/>
|
||||
<define name="THETA_IGAIN" value="200"/>
|
||||
<define name="THETA_PGAIN" value="1272"/>
|
||||
<define name="THETA_DGAIN" value="132"/>
|
||||
<define name="THETA_IGAIN" value="157"/>
|
||||
|
||||
<define name="PSI_PGAIN" value="1000"/>
|
||||
<define name="PSI_DGAIN" value="350"/>
|
||||
<define name="PSI_IGAIN" value="20"/>
|
||||
<define name="PSI_PGAIN" value="1000"/>
|
||||
<define name="PSI_DGAIN" value="244"/>
|
||||
<define name="PSI_IGAIN" value="20"/>
|
||||
|
||||
<!-- feedforward -->
|
||||
<define name="PHI_DDGAIN" value="170"/>
|
||||
<define name="THETA_DDGAIN" value="170"/>
|
||||
<define name="PSI_DDGAIN" value="170"/>
|
||||
<define name="PHI_DDGAIN" value="170"/>
|
||||
<define name="THETA_DDGAIN" value="150"/>
|
||||
<define name="PSI_DDGAIN" value="170"/>
|
||||
|
||||
<define name="PHI_AGAIN" value="0"/>
|
||||
<define name="PHI_AGAIN" value="0"/>
|
||||
<define name="THETA_AGAIN" value="0"/>
|
||||
<define name="PSI_AGAIN" value="0"/>
|
||||
<define name="PSI_AGAIN" value="0"/>
|
||||
</section>
|
||||
|
||||
<section name="GUIDANCE_V" prefix="GUIDANCE_V_">
|
||||
<section name="GUIDANCE_V" prefix="GUIDANCE_V_">
|
||||
|
||||
<define name="MIN_ERR_Z" value="POS_BFP_OF_REAL(-10.)"/>
|
||||
<define name="MAX_ERR_Z" value="POS_BFP_OF_REAL( 10.)"/>
|
||||
<define name="MIN_ERR_ZD" value="SPEED_BFP_OF_REAL(-10.)"/>
|
||||
<define name="MAX_ERR_ZD" value="SPEED_BFP_OF_REAL( 10.)"/>
|
||||
<define name="MAX_SUM_ERR" value="2000000"/>
|
||||
<define name="REF_MIN_ZDD" value="-0.5*9.81"/>
|
||||
<define name="REF_MAX_ZDD" value=" 0.5*9.81"/>
|
||||
<define name="REF_MIN_ZD" value="-1.5"/>
|
||||
<define name="REF_MAX_ZD" value=" 1.5"/>
|
||||
<define name="HOVER_KP" value="130"/>
|
||||
<define name="HOVER_KD" value="80"/>
|
||||
<define name="HOVER_KI" value="0"/>
|
||||
<define name="MIN_ERR_Z" value="POS_BFP_OF_REAL(-10.)"/>
|
||||
<define name="MAX_ERR_Z" value="POS_BFP_OF_REAL( 10.)"/>
|
||||
<define name="MIN_ERR_ZD" value="SPEED_BFP_OF_REAL(-10.)"/>
|
||||
<define name="MAX_ERR_ZD" value="SPEED_BFP_OF_REAL( 10.)"/>
|
||||
<define name="MAX_SUM_ERR" value="2000000"/>
|
||||
<define name="REF_MIN_ZDD" value="-0.5*9.81"/>
|
||||
<define name="REF_MAX_ZDD" value=" 0.5*9.81"/>
|
||||
<define name="REF_MIN_ZD" value="-1.5"/>
|
||||
<define name="REF_MAX_ZD" value=" 1.5"/>
|
||||
<define name="HOVER_KP" value="130"/>
|
||||
<define name="HOVER_KD" value="80"/>
|
||||
<define name="HOVER_KI" value="0"/>
|
||||
<!-- SPEED_BFP_OF_REAL(1.5) / (MAX_PPRZ/2) -->
|
||||
<define name="RC_CLIMB_COEF" value ="163"/>
|
||||
<define name="RC_CLIMB_COEF" value="163"/>
|
||||
<!-- SPEED_BFP_OF_REAL(1.5) * 20% -->
|
||||
<define name="RC_CLIMB_DEAD_BAND" value ="160000"/>
|
||||
<define name="RC_CLIMB_DEAD_BAND" value="160000"/>
|
||||
<define name="ADAPT_NOISE_FACTOR" value="1.5"/>
|
||||
</section>
|
||||
|
||||
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
|
||||
<define name="USE_REF" value="FALSE"/>
|
||||
<define name="MAX_BANK" value="30" unit="deg"/>
|
||||
<define name="PGAIN" value="50"/>
|
||||
<define name="DGAIN" value="80"/>
|
||||
<define name="IGAIN" value="20"/>
|
||||
<define name="PGAIN" value="40"/>
|
||||
<define name="DGAIN" value="70"/>
|
||||
<define name="IGAIN" value="15"/>
|
||||
<define name="NGAIN" value="0"/>
|
||||
<!-- feedforward -->
|
||||
<define name="AGAIN" value="0"/>
|
||||
@@ -227,9 +228,8 @@
|
||||
|
||||
<section name="AUTOPILOT">
|
||||
<define name="MODE_MANUAL" value="AP_MODE_ATTITUDE_DIRECT"/>
|
||||
<define name="MODE_AUTO1" value="AP_MODE_ATTITUDE_Z_HOLD"/>
|
||||
<!--define name="MODE_AUTO2" value="AP_MODE_ATTITUDE_Z_HOLD"/-->
|
||||
<define name="MODE_AUTO2" value="AP_MODE_NAV"/>
|
||||
<define name="MODE_AUTO1" value="AP_MODE_ATTITUDE_Z_HOLD"/>
|
||||
<define name="MODE_AUTO2" value="AP_MODE_NAV"/>
|
||||
</section>
|
||||
|
||||
<section name="FMS">
|
||||
|
||||
@@ -0,0 +1,58 @@
|
||||
# Hey Emacs, this is a -*- makefile -*-
|
||||
#
|
||||
# apogee_1.0.makefile
|
||||
#
|
||||
#
|
||||
|
||||
BOARD=apogee
|
||||
BOARD_VERSION=1.0
|
||||
BOARD_CFG=\"boards/$(BOARD)_$(BOARD_VERSION).h\"
|
||||
|
||||
ARCH=stm32
|
||||
ARCH_L=f4
|
||||
ARCH_DIR=stm32
|
||||
SRC_ARCH=arch/$(ARCH_DIR)
|
||||
$(TARGET).ARCHDIR = $(ARCH)
|
||||
$(TARGET).LDSCRIPT=$(SRC_ARCH)/apogee.ld
|
||||
|
||||
HARD_FLOAT=yes
|
||||
|
||||
# default flash mode is via usb dfu bootloader
|
||||
# possibilities: DFU, SWD
|
||||
FLASH_MODE ?= DFU
|
||||
STLINK ?= y
|
||||
DFU_UTIL ?= y
|
||||
|
||||
#
|
||||
# default LED configuration
|
||||
#
|
||||
RADIO_CONTROL_LED ?= 4
|
||||
BARO_LED ?= none
|
||||
AHRS_ALIGNER_LED ?= 2
|
||||
GPS_LED ?= 3
|
||||
SYS_TIME_LED ?= 1
|
||||
|
||||
#
|
||||
# default UART configuration (modem, gps, spektrum)
|
||||
#
|
||||
|
||||
MODEM_PORT ?= UART1
|
||||
MODEM_BAUD ?= B57600
|
||||
|
||||
GPS_PORT ?= UART4
|
||||
GPS_BAUD ?= B38400
|
||||
|
||||
RADIO_CONTROL_SPEKTRUM_PRIMARY_PORT ?= UART2
|
||||
|
||||
SBUS_PORT ?= UART2
|
||||
|
||||
#
|
||||
# default actuator configuration
|
||||
#
|
||||
# you can use different actuators by adding a configure option to your firmware section
|
||||
# e.g. <configure name="ACTUATORS" value="actuators_ppm/>
|
||||
# and by setting the correct "driver" attribute in servo section
|
||||
# e.g. <servo driver="Ppm">
|
||||
#
|
||||
ACTUATORS ?= actuators_pwm
|
||||
|
||||
@@ -155,4 +155,14 @@
|
||||
settings=" settings/fixedwing_basic.xml settings/control/ctl_energyadaptive.xml"
|
||||
gui_color="blue"
|
||||
/>
|
||||
<aircraft
|
||||
name="krooz_quad"
|
||||
ac_id="23"
|
||||
airframe="airframes/examples/krooz_sd/krooz_sd_quad_mkk.xml"
|
||||
radio="radios/cockpitSX.xml"
|
||||
telemetry="telemetry/default_rotorcraft.xml"
|
||||
flight_plan="flight_plans/rotorcraft_basic.xml"
|
||||
settings=" settings/rotorcraft_basic.xml settings/estimation/ahrs_int_cmpl_quat.xml settings/control/stabilization_att_int.xml settings/control/stabilization_rate.xml"
|
||||
gui_color="blue"
|
||||
/>
|
||||
</conf>
|
||||
|
||||
@@ -36,43 +36,7 @@
|
||||
#
|
||||
#
|
||||
|
||||
|
||||
# for fixedwing firmware and ap only
|
||||
ifeq ($(TARGET), ap)
|
||||
IMU_ASPIRIN_2_CFLAGS = -DUSE_IMU
|
||||
endif
|
||||
|
||||
IMU_ASPIRIN_2_CFLAGS += -DIMU_TYPE_H=\"imu/imu_aspirin_2_spi.h\"
|
||||
IMU_ASPIRIN_2_SRCS = $(SRC_SUBSYSTEMS)/imu.c
|
||||
IMU_ASPIRIN_2_SRCS += $(SRC_SUBSYSTEMS)/imu/imu_aspirin_2_spi.c
|
||||
IMU_ASPIRIN_2_SRCS += peripherals/mpu60x0.c
|
||||
IMU_ASPIRIN_2_SRCS += peripherals/mpu60x0_spi.c
|
||||
|
||||
# Magnetometer
|
||||
#IMU_ASPIRIN_2_SRCS += peripherals/hmc58xx.c
|
||||
|
||||
include $(CFG_SHARED)/spi_master.makefile
|
||||
|
||||
ifeq ($(ARCH), lpc21)
|
||||
IMU_ASPIRIN_2_CFLAGS += -DUSE_SPI_SLAVE0
|
||||
IMU_ASPIRIN_2_CFLAGS += -DASPIRIN_2_SPI_SLAVE_IDX=SPI_SLAVE0
|
||||
IMU_ASPIRIN_2_CFLAGS += -DASPIRIN_2_SPI_DEV=spi1
|
||||
IMU_ASPIRIN_2_CFLAGS += -DUSE_SPI1
|
||||
else ifeq ($(ARCH), stm32)
|
||||
IMU_ASPIRIN_2_CFLAGS += -DUSE_SPI2
|
||||
# Slave select configuration
|
||||
# SLAVE2 is on PB12 (NSS) (MPU600 CS)
|
||||
IMU_ASPIRIN_2_CFLAGS += -DUSE_SPI_SLAVE2
|
||||
endif
|
||||
|
||||
# Keep CFLAGS/Srcs for imu in separate expression so we can assign it to other targets
|
||||
# see: conf/autopilot/subsystems/lisa_passthrough/imu_b2_v1.1.makefile for example
|
||||
include $(CFG_SHARED)/imu_aspirin_v2_common.makefile
|
||||
|
||||
ap.CFLAGS += $(IMU_ASPIRIN_2_CFLAGS)
|
||||
ap.srcs += $(IMU_ASPIRIN_2_SRCS)
|
||||
|
||||
|
||||
#
|
||||
# NPS simulator
|
||||
#
|
||||
include $(CFG_SHARED)/imu_nps.makefile
|
||||
|
||||
@@ -2,13 +2,14 @@
|
||||
#
|
||||
# Aspirin IMU v2.2
|
||||
#
|
||||
# actually identical with v2.1 since baro is not read in IMU driver
|
||||
# nearly identical with v2.1, only has the MS5611 baro on SPI.
|
||||
# The Baro CS line is
|
||||
#
|
||||
#
|
||||
# required xml:
|
||||
# <section name="IMU" prefix="IMU_">
|
||||
#
|
||||
# <!-- these gyro and accel calib values are the defaults for aspirin2.1 -->
|
||||
# <!-- these gyro and accel calib values are the defaults for aspirin2.1/2.2 -->
|
||||
# <define name="GYRO_X_NEUTRAL" value="0"/>
|
||||
# <define name="GYRO_Y_NEUTRAL" value="0"/>
|
||||
# <define name="GYRO_Z_NEUTRAL" value="0"/>
|
||||
@@ -39,4 +40,18 @@
|
||||
#
|
||||
|
||||
|
||||
include $(CFG_SHARED)/imu_aspirin_v2.1.makefile
|
||||
include $(CFG_SHARED)/imu_aspirin_v2_common.makefile
|
||||
|
||||
#
|
||||
# Baro is connected via SPI, so additionally specify the slave select line for it,
|
||||
# so that it will be unselected at init (baro CS line high)
|
||||
#
|
||||
ifeq ($(ARCH), lpc21)
|
||||
IMU_ASPIRIN_2_CFLAGS += -DUSE_SPI_SLAVE1
|
||||
else ifeq ($(ARCH), stm32)
|
||||
# SLAVE3 is on PC13, which is the baro CS
|
||||
IMU_ASPIRIN_2_CFLAGS += -DUSE_SPI_SLAVE3
|
||||
endif
|
||||
|
||||
ap.CFLAGS += $(IMU_ASPIRIN_2_CFLAGS)
|
||||
ap.srcs += $(IMU_ASPIRIN_2_SRCS)
|
||||
|
||||
@@ -0,0 +1,72 @@
|
||||
# Hey Emacs, this is a -*- makefile -*-
|
||||
#
|
||||
# Common part for Aspirin IMU v2.1 and v2.2
|
||||
#
|
||||
#
|
||||
# required xml:
|
||||
# <section name="IMU" prefix="IMU_">
|
||||
#
|
||||
# <!-- these gyro and accel calib values are the defaults for aspirin2.1/2.2 -->
|
||||
# <define name="GYRO_X_NEUTRAL" value="0"/>
|
||||
# <define name="GYRO_Y_NEUTRAL" value="0"/>
|
||||
# <define name="GYRO_Z_NEUTRAL" value="0"/>
|
||||
#
|
||||
# <define name="GYRO_X_SENS" value="4.359" integer="16"/>
|
||||
# <define name="GYRO_Y_SENS" value="4.359" integer="16"/>
|
||||
# <define name="GYRO_Z_SENS" value="4.359" integer="16"/>
|
||||
#
|
||||
# <define name="ACCEL_X_NEUTRAL" value="0"/>
|
||||
# <define name="ACCEL_Y_NEUTRAL" value="0"/>
|
||||
# <define name="ACCEL_Z_NEUTRAL" value="0"/>
|
||||
#
|
||||
# <define name="ACCEL_X_SENS" value="4.905" integer="16"/>
|
||||
# <define name="ACCEL_Y_SENS" value="4.905" integer="16"/>
|
||||
# <define name="ACCEL_Z_SENS" value="4.905" integer="16"/>
|
||||
#
|
||||
# <!-- replace the mag calibration with your own-->
|
||||
# <define name="MAG_X_NEUTRAL" value="-45"/>
|
||||
# <define name="MAG_Y_NEUTRAL" value="334"/>
|
||||
# <define name="MAG_Z_NEUTRAL" value="7"/>
|
||||
#
|
||||
# <define name="MAG_X_SENS" value="3.4936416" integer="16"/>
|
||||
# <define name="MAG_Y_SENS" value="3.607713" integer="16"/>
|
||||
# <define name="MAG_Z_SENS" value="4.90788848" integer="16"/>
|
||||
#
|
||||
# </section>
|
||||
#
|
||||
#
|
||||
|
||||
|
||||
# for fixedwing firmware and ap only
|
||||
ifeq ($(TARGET), ap)
|
||||
IMU_ASPIRIN_2_CFLAGS = -DUSE_IMU
|
||||
endif
|
||||
|
||||
IMU_ASPIRIN_2_CFLAGS += -DIMU_TYPE_H=\"imu/imu_aspirin_2_spi.h\"
|
||||
IMU_ASPIRIN_2_SRCS = $(SRC_SUBSYSTEMS)/imu.c
|
||||
IMU_ASPIRIN_2_SRCS += $(SRC_SUBSYSTEMS)/imu/imu_aspirin_2_spi.c
|
||||
IMU_ASPIRIN_2_SRCS += peripherals/mpu60x0.c
|
||||
IMU_ASPIRIN_2_SRCS += peripherals/mpu60x0_spi.c
|
||||
|
||||
include $(CFG_SHARED)/spi_master.makefile
|
||||
|
||||
ifeq ($(ARCH), lpc21)
|
||||
IMU_ASPIRIN_2_CFLAGS += -DUSE_SPI_SLAVE0
|
||||
IMU_ASPIRIN_2_CFLAGS += -DASPIRIN_2_SPI_SLAVE_IDX=SPI_SLAVE0
|
||||
IMU_ASPIRIN_2_CFLAGS += -DASPIRIN_2_SPI_DEV=spi1
|
||||
IMU_ASPIRIN_2_CFLAGS += -DUSE_SPI1
|
||||
else ifeq ($(ARCH), stm32)
|
||||
IMU_ASPIRIN_2_CFLAGS += -DUSE_SPI2
|
||||
# Slave select configuration
|
||||
# SLAVE2 is on PB12 (NSS) (MPU600 CS)
|
||||
IMU_ASPIRIN_2_CFLAGS += -DUSE_SPI_SLAVE2
|
||||
endif
|
||||
|
||||
# Keep CFLAGS/Srcs for imu in separate expression so we can assign it to other targets
|
||||
# and re-use that in the imu_aspirin_v2.1 and imu_aspirin_v2.2 makefiles
|
||||
|
||||
|
||||
#
|
||||
# NPS simulator
|
||||
#
|
||||
include $(CFG_SHARED)/imu_nps.makefile
|
||||
@@ -0,0 +1,20 @@
|
||||
#
|
||||
# Makefile for shared radio_control SBUS subsystem
|
||||
#
|
||||
|
||||
$(TARGET).CFLAGS += -DRADIO_CONTROL
|
||||
ifneq ($(RADIO_CONTROL_LED),none)
|
||||
ap.CFLAGS += -DRADIO_CONTROL_LED=$(RADIO_CONTROL_LED)
|
||||
endif
|
||||
|
||||
# convert SBUS_PORT to upper and lower case strings:
|
||||
SBUS_PORT_UPPER=$(shell echo $(SBUS_PORT) | tr a-z A-Z)
|
||||
SBUS_PORT_LOWER=$(shell echo $(SBUS_PORT) | tr A-Z a-z)
|
||||
|
||||
$(TARGET).CFLAGS += -DUSE_$(SBUS_PORT_UPPER) -D$(SBUS_PORT_UPPER)_BAUD=B100000
|
||||
$(TARGET).CFLAGS += -DSBUS_UART_DEV=$(SBUS_PORT_LOWER)
|
||||
$(TARGET).CFLAGS += -DRADIO_CONTROL_TYPE_H=\"subsystems/radio_control/sbus.h\"
|
||||
$(TARGET).CFLAGS += -DRADIO_CONTROL_TYPE_SBUS
|
||||
$(TARGET).srcs += $(SRC_SUBSYSTEMS)/radio_control.c
|
||||
$(TARGET).srcs += $(SRC_SUBSYSTEMS)/radio_control/sbus.c
|
||||
|
||||
@@ -1,3 +1,3 @@
|
||||
<!DOCTYPE maps SYSTEM "maps.dtd">
|
||||
|
||||
<maps google_version="89"/>
|
||||
<maps google_version="129"/>
|
||||
|
||||
@@ -0,0 +1,55 @@
|
||||
<?xml version="1.0"?>
|
||||
<!-- $Id$
|
||||
--
|
||||
-- (c) 2013 Gautier Hattenberger
|
||||
--
|
||||
-- This file is part of paparazzi.
|
||||
--
|
||||
-- paparazzi is free software; you can redistribute it and/or modify
|
||||
-- it under the terms of the GNU General Public License as published by
|
||||
-- the Free Software Foundation; either version 2, or (at your option)
|
||||
-- any later version.
|
||||
--
|
||||
-- paparazzi is distributed in the hope that it will be useful,
|
||||
-- but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
-- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
-- GNU General Public License for more details.
|
||||
--
|
||||
-- You should have received a copy of the GNU General Public License
|
||||
-- along with paparazzi; see the file COPYING. If not, write to
|
||||
-- the Free Software Foundation, 59 Temple Place - Suite 330,
|
||||
-- Boston, MA 02111-1307, USA.
|
||||
-->
|
||||
|
||||
<!--
|
||||
-- Attributes of root (Radio) tag :
|
||||
-- name: name of RC
|
||||
-- data_min: min width of a pulse to be considered as a data pulse
|
||||
-- data_max: max width of a pulse to be considered as a data pulse
|
||||
-- sync_min: min width of a pulse to be considered as a synchro pulse
|
||||
-- sync_max: max width of a pulse to be considered as a synchro pulse
|
||||
-- pulse_type: POSITIVE ( Futaba and others) | NEGATIVE (JR)
|
||||
-- min, max and sync are expressed in micro-seconds
|
||||
-->
|
||||
|
||||
<!--
|
||||
-- Attributes of channel tag :
|
||||
-- ctl: name of the command on the transmitter - only for displaying
|
||||
-- function: logical command
|
||||
-- average: (boolean) channel filtered through several frames (for discrete commands)
|
||||
-- min: minimum pulse length (micro-seconds)
|
||||
-- max: maximum pulse length (micro-seconds)
|
||||
-- neutral: neutral pulse length (micro-seconds)
|
||||
-- Note: a command may be reversed by exchanging min and max values
|
||||
-->
|
||||
|
||||
<!DOCTYPE radio SYSTEM "radio.dtd">
|
||||
<radio name="Futaba T10CG with SBUS" data_min="900" data_max="2100" sync_min ="5000" sync_max ="15000" pulse_type="POSITIVE">
|
||||
<channel ctl="A" function="ROLL" min="363" neutral="1024" max="1683" average="0"/>
|
||||
<channel ctl="B" function="PITCH" min="363" neutral="1024" max="1683" average="0"/>
|
||||
<channel ctl="C" function="THROTTLE" min="1683" neutral="1683" max="363" average="0"/>
|
||||
<channel ctl="D" function="YAW" min="363" neutral="1024" max="1683" average="0"/>
|
||||
<channel ctl="E" function="MODE" max="137" neutral="1024" min="1908" average="1"/>
|
||||
<channel ctl="F" function="GAIN1" min="137" neutral="1024" max="1908" average="0"/>
|
||||
<channel ctl="G" function="GAIN2" min="137" neutral="1024" max="1908" average="0"/>
|
||||
</radio>
|
||||
+1
-1
@@ -28,7 +28,7 @@ $(DATADIR)/maps.google.com: $(DATADIR) FORCE
|
||||
exit 1)
|
||||
|
||||
$(PAPARAZZI_HOME)/conf/maps.xml: $(DATADIR)/maps.google.com
|
||||
$(eval GOOGLE_VERSION := $(shell grep -E "http://khm[0-9]+.google.com/kh/v=[0-9]+.x26" $(DATADIR)/maps.google.com | sed -E 's#.*http://khm[0-9]+.google.com/kh/v=##;s#.x26.*##'))
|
||||
$(eval GOOGLE_VERSION := $(shell tr -s '[[:space:]]' '\n' < $(DATADIR)/maps.google.com | grep -E "http[s]?://khm[s]?[0-9]+.google.com/kh/v=[0-9]+.x26" | sed -E 's#.*http[s]?://khm[s]?[0-9]+.google.com/kh/v=##;s#.x26.*##'))
|
||||
$(eval $@_TMP := $(shell $(MKTEMP)))
|
||||
@echo "Updated google maps version to $(GOOGLE_VERSION)"
|
||||
@echo "-----------------------------------------------"
|
||||
|
||||
Binary file not shown.
|
After Width: | Height: | Size: 6.5 KiB |
@@ -68,6 +68,10 @@ void uart_periph_set_baudrate(struct uart_periph* p, uint32_t baud) {
|
||||
uart_enable_interrupts(p);
|
||||
}
|
||||
|
||||
void uart_periph_set_bits_stop_parity(struct uart_periph* __attribute__((unused)) p, uint8_t __attribute__((unused)) bits, uint8_t __attribute__((unused)) stop, uint8_t __attribute__((unused)) parity) {
|
||||
// TBD
|
||||
}
|
||||
|
||||
void uart_transmit(struct uart_periph* p, uint8_t data ) {
|
||||
uint16_t temp;
|
||||
unsigned cpsr;
|
||||
|
||||
@@ -41,11 +41,8 @@
|
||||
|
||||
void uart_periph_set_baudrate(struct uart_periph* p, uint32_t baud) {
|
||||
|
||||
/* Configure USART */
|
||||
/* Configure USART baudrate */
|
||||
usart_set_baudrate((uint32_t)p->reg_addr, baud);
|
||||
usart_set_databits((uint32_t)p->reg_addr, 8);
|
||||
usart_set_stopbits((uint32_t)p->reg_addr, USART_STOPBITS_1);
|
||||
usart_set_parity((uint32_t)p->reg_addr, USART_PARITY_NONE);
|
||||
|
||||
/* Disable Idle Line interrupt */
|
||||
USART_CR1((uint32_t)p->reg_addr) &= ~USART_CR1_IDLEIE;
|
||||
@@ -61,6 +58,33 @@ void uart_periph_set_baudrate(struct uart_periph* p, uint32_t baud) {
|
||||
|
||||
}
|
||||
|
||||
void uart_periph_set_bits_stop_parity(struct uart_periph* p, uint8_t bits, uint8_t stop, uint8_t parity) {
|
||||
/* Configure USART parity and data bits */
|
||||
if (parity == UPARITY_EVEN) {
|
||||
usart_set_parity((uint32_t)p->reg_addr, USART_PARITY_EVEN);
|
||||
if (bits == UBITS_7)
|
||||
usart_set_databits((uint32_t)p->reg_addr, 8);
|
||||
else // 8 data bits by default
|
||||
usart_set_databits((uint32_t)p->reg_addr, 9);
|
||||
}
|
||||
else if (parity == USART_PARITY_ODD) {
|
||||
usart_set_parity((uint32_t)p->reg_addr, USART_PARITY_ODD);
|
||||
if (bits == UBITS_7)
|
||||
usart_set_databits((uint32_t)p->reg_addr, 8);
|
||||
else // 8 data bits by default
|
||||
usart_set_databits((uint32_t)p->reg_addr, 9);
|
||||
}
|
||||
else { // 8 data bist, NO_PARITY by default
|
||||
usart_set_parity((uint32_t)p->reg_addr, USART_PARITY_NONE);
|
||||
usart_set_databits((uint32_t)p->reg_addr, 8); // is 7bits without parity possible ?
|
||||
}
|
||||
/* Configure USART stop bits */
|
||||
if (stop == USTOP_2)
|
||||
usart_set_stopbits((uint32_t)p->reg_addr, USART_STOPBITS_2);
|
||||
else // 1 stop bit by default
|
||||
usart_set_stopbits((uint32_t)p->reg_addr, USART_STOPBITS_1);
|
||||
}
|
||||
|
||||
void uart_periph_set_mode(struct uart_periph* p, bool_t tx_enabled, bool_t rx_enabled, bool_t hw_flow_control) {
|
||||
uint32_t mode = 0;
|
||||
if (tx_enabled)
|
||||
@@ -173,6 +197,18 @@ static inline void usart_enable_irq(uint8_t IRQn) {
|
||||
#define UART1_HW_FLOW_CONTROL FALSE
|
||||
#endif
|
||||
|
||||
#ifndef UART1_BITS
|
||||
#define UART1_BITS UBITS_8
|
||||
#endif
|
||||
|
||||
#ifndef UART1_STOP
|
||||
#define UART1_STOP USTOP_1
|
||||
#endif
|
||||
|
||||
#ifndef UART1_PARITY
|
||||
#define UART1_PARITY UPARITY_NO
|
||||
#endif
|
||||
|
||||
void uart1_init( void ) {
|
||||
|
||||
uart_periph_init(&uart1);
|
||||
@@ -201,7 +237,8 @@ void uart1_init( void ) {
|
||||
/* Configure USART1, enable hardware flow control*/
|
||||
uart_periph_set_mode(&uart1, USE_UART1_TX, USE_UART1_RX, UART1_HW_FLOW_CONTROL);
|
||||
|
||||
/* Set USART1 baudrate and enable interrupt */
|
||||
/* Set USART1 parameters and enable interrupt */
|
||||
uart_periph_set_bits_stop_parity(&uart1, UART1_BITS, UART1_STOP, UART1_PARITY);
|
||||
uart_periph_set_baudrate(&uart1, UART1_BAUD);
|
||||
}
|
||||
|
||||
@@ -224,6 +261,18 @@ void usart1_isr(void) { usart_isr(&uart1); }
|
||||
#define UART2_HW_FLOW_CONTROL FALSE
|
||||
#endif
|
||||
|
||||
#ifndef UART2_BITS
|
||||
#define UART2_BITS UBITS_8
|
||||
#endif
|
||||
|
||||
#ifndef UART2_STOP
|
||||
#define UART2_STOP USTOP_1
|
||||
#endif
|
||||
|
||||
#ifndef UART2_PARITY
|
||||
#define UART2_PARITY UPARITY_NO
|
||||
#endif
|
||||
|
||||
void uart2_init( void ) {
|
||||
|
||||
uart_periph_init(&uart2);
|
||||
@@ -253,6 +302,7 @@ void uart2_init( void ) {
|
||||
uart_periph_set_mode(&uart2, USE_UART2_TX, USE_UART2_RX, UART2_HW_FLOW_CONTROL);
|
||||
|
||||
/* Configure USART */
|
||||
uart_periph_set_bits_stop_parity(&uart2, UART2_BITS, UART2_STOP, UART2_PARITY);
|
||||
uart_periph_set_baudrate(&uart2, UART2_BAUD);
|
||||
}
|
||||
|
||||
@@ -275,6 +325,18 @@ void usart2_isr(void) { usart_isr(&uart2); }
|
||||
#define UART3_HW_FLOW_CONTROL FALSE
|
||||
#endif
|
||||
|
||||
#ifndef UART3_BITS
|
||||
#define UART3_BITS UBITS_8
|
||||
#endif
|
||||
|
||||
#ifndef UART3_STOP
|
||||
#define UART3_STOP USTOP_1
|
||||
#endif
|
||||
|
||||
#ifndef UART3_PARITY
|
||||
#define UART3_PARITY UPARITY_NO
|
||||
#endif
|
||||
|
||||
void uart3_init( void ) {
|
||||
|
||||
uart_periph_init(&uart3);
|
||||
@@ -304,6 +366,7 @@ void uart3_init( void ) {
|
||||
uart_periph_set_mode(&uart3, USE_UART3_TX, USE_UART3_RX, UART3_HW_FLOW_CONTROL);
|
||||
|
||||
/* Configure USART */
|
||||
uart_periph_set_bits_stop_parity(&uart3, UART3_BITS, UART3_STOP, UART3_PARITY);
|
||||
uart_periph_set_baudrate(&uart3, UART3_BAUD);
|
||||
}
|
||||
|
||||
@@ -322,6 +385,18 @@ void usart3_isr(void) { usart_isr(&uart3); }
|
||||
#define USE_UART4_RX TRUE
|
||||
#endif
|
||||
|
||||
#ifndef UART4_BITS
|
||||
#define UART4_BITS UBITS_8
|
||||
#endif
|
||||
|
||||
#ifndef UART4_STOP
|
||||
#define UART4_STOP USTOP_1
|
||||
#endif
|
||||
|
||||
#ifndef UART4_PARITY
|
||||
#define UART4_PARITY UPARITY_NO
|
||||
#endif
|
||||
|
||||
void uart4_init( void ) {
|
||||
|
||||
uart_periph_init(&uart4);
|
||||
@@ -342,6 +417,7 @@ void uart4_init( void ) {
|
||||
|
||||
/* Configure USART */
|
||||
uart_periph_set_mode(&uart4, USE_UART4_TX, USE_UART4_RX, FALSE);
|
||||
uart_periph_set_bits_stop_parity(&uart4, UART4_BITS, UART4_STOP, UART4_PARITY);
|
||||
uart_periph_set_baudrate(&uart4, UART4_BAUD);
|
||||
}
|
||||
|
||||
@@ -360,6 +436,18 @@ void uart4_isr(void) { usart_isr(&uart4); }
|
||||
#define USE_UART5_RX TRUE
|
||||
#endif
|
||||
|
||||
#ifndef UART5_BITS
|
||||
#define UART5_BITS UBITS_8
|
||||
#endif
|
||||
|
||||
#ifndef UART5_STOP
|
||||
#define UART5_STOP USTOP_1
|
||||
#endif
|
||||
|
||||
#ifndef UART5_PARITY
|
||||
#define UART5_PARITY UPARITY_NO
|
||||
#endif
|
||||
|
||||
void uart5_init( void ) {
|
||||
|
||||
uart_periph_init(&uart5);
|
||||
@@ -380,6 +468,7 @@ void uart5_init( void ) {
|
||||
|
||||
/* Configure USART */
|
||||
uart_periph_set_mode(&uart5, USE_UART5_TX, USE_UART5_RX, FALSE);
|
||||
uart_periph_set_bits_stop_parity(&uart5, UART5_BITS, UART5_STOP, UART5_PARITY);
|
||||
uart_periph_set_baudrate(&uart5, UART5_BAUD);
|
||||
}
|
||||
|
||||
@@ -402,6 +491,18 @@ void uart5_isr(void) { usart_isr(&uart5); }
|
||||
#define UART6_HW_FLOW_CONTROL FALSE
|
||||
#endif
|
||||
|
||||
#ifndef UART6_BITS
|
||||
#define UART6_BITS UBITS_8
|
||||
#endif
|
||||
|
||||
#ifndef UART6_STOP
|
||||
#define UART6_STOP USTOP_1
|
||||
#endif
|
||||
|
||||
#ifndef UART6_PARITY
|
||||
#define UART6_PARITY UPARITY_NO
|
||||
#endif
|
||||
|
||||
void uart6_init( void ) {
|
||||
|
||||
uart_periph_init(&uart6);
|
||||
@@ -430,7 +531,7 @@ void uart6_init( void ) {
|
||||
|
||||
/* Configure USART Tx,Rx and hardware flow control*/
|
||||
uart_periph_set_mode(&uart6, USE_UART6_TX, USE_UART6_RX, UART6_HW_FLOW_CONTROL);
|
||||
|
||||
uart_periph_set_bits_stop_parity(&uart6, UART6_BITS, UART6_STOP, UART6_PARITY);
|
||||
uart_periph_set_baudrate(&uart6, UART6_BAUD);
|
||||
}
|
||||
|
||||
|
||||
@@ -0,0 +1,399 @@
|
||||
#ifndef CONFIG_APOGEE_0_99_H
|
||||
#define CONFIG_APOGEE_0_99_H
|
||||
|
||||
#define BOARD_APOGEE
|
||||
|
||||
/* Apogee has a 16MHz external clock and 168MHz internal. */
|
||||
#define EXT_CLK 16000000
|
||||
#define AHB_CLK 168000000
|
||||
|
||||
/*
|
||||
* Onboard LEDs
|
||||
*/
|
||||
|
||||
/* red, on PC0 */
|
||||
#ifndef USE_LED_1
|
||||
#define USE_LED_1 1
|
||||
#endif
|
||||
#define LED_1_GPIO GPIOC
|
||||
#define LED_1_GPIO_CLK RCC_AHB1ENR_IOPCEN
|
||||
#define LED_1_GPIO_PIN GPIO0
|
||||
#define LED_1_GPIO_ON gpio_clear
|
||||
#define LED_1_GPIO_OFF gpio_set
|
||||
#define LED_1_AFIO_REMAP ((void)0)
|
||||
|
||||
/* orange, on PC13 */
|
||||
#ifndef USE_LED_2
|
||||
#define USE_LED_2 1
|
||||
#endif
|
||||
#define LED_2_GPIO GPIOC
|
||||
#define LED_2_GPIO_CLK RCC_AHB1ENR_IOPCEN
|
||||
#define LED_2_GPIO_PIN GPIO13
|
||||
#define LED_2_GPIO_ON gpio_clear
|
||||
#define LED_2_GPIO_OFF gpio_set
|
||||
#define LED_2_AFIO_REMAP ((void)0)
|
||||
|
||||
/* green, on PC1 */
|
||||
#ifndef USE_LED_3
|
||||
#define USE_LED_3 1
|
||||
#endif
|
||||
#define LED_3_GPIO GPIOC
|
||||
#define LED_3_GPIO_CLK RCC_AHB1ENR_IOPCEN
|
||||
#define LED_3_GPIO_PIN GPIO1
|
||||
#define LED_3_GPIO_ON gpio_clear
|
||||
#define LED_3_GPIO_OFF gpio_set
|
||||
#define LED_3_AFIO_REMAP ((void)0)
|
||||
|
||||
/* yellow, on PC3 */
|
||||
#ifndef USE_LED_4
|
||||
#define USE_LED_4 1
|
||||
#endif
|
||||
#define LED_4_GPIO GPIOC
|
||||
#define LED_4_GPIO_CLK RCC_AHB1ENR_IOPCEN
|
||||
#define LED_4_GPIO_PIN GPIO3
|
||||
#define LED_4_GPIO_ON gpio_clear
|
||||
#define LED_4_GPIO_OFF gpio_set
|
||||
#define LED_4_AFIO_REMAP ((void)0)
|
||||
|
||||
/* AUX1, on PB1, 1 on LED_ON, 0 on LED_OFF */
|
||||
#ifndef USE_LED_5
|
||||
#define USE_LED_5 0
|
||||
#endif
|
||||
#define LED_5_GPIO GPIOB
|
||||
#define LED_5_GPIO_CLK RCC_AHB1ENR_IOPBEN
|
||||
#define LED_5_GPIO_PIN GPIO1
|
||||
#define LED_5_GPIO_ON gpio_set
|
||||
#define LED_5_GPIO_OFF gpio_clear
|
||||
#define LED_5_AFIO_REMAP ((void)0)
|
||||
|
||||
/* AUX2, on PC5, 1 on LED_ON, 0 on LED_OFF */
|
||||
#ifndef USE_LED_6
|
||||
#define USE_LED_6 0
|
||||
#endif
|
||||
#define LED_6_GPIO GPIOC
|
||||
#define LED_6_GPIO_CLK RCC_AHB1ENR_IOPCEN
|
||||
#define LED_6_GPIO_PIN GPIO5
|
||||
#define LED_6_GPIO_ON gpio_set
|
||||
#define LED_6_GPIO_OFF gpio_clear
|
||||
#define LED_6_AFIO_REMAP ((void)0)
|
||||
|
||||
/* AUX3, on PC4, 1 on LED_ON, 0 on LED_OFF */
|
||||
#ifndef USE_LED_7
|
||||
#define USE_LED_7 0
|
||||
#endif
|
||||
#define LED_7_GPIO GPIOC
|
||||
#define LED_7_GPIO_CLK RCC_AHB1ENR_IOPCEN
|
||||
#define LED_7_GPIO_PIN GPIO4
|
||||
#define LED_7_GPIO_ON gpio_set
|
||||
#define LED_7_GPIO_OFF gpio_clear
|
||||
#define LED_7_AFIO_REMAP ((void)0)
|
||||
|
||||
/* AUX4, on PB15, 1 on LED_ON, 0 on LED_OFF */
|
||||
#ifndef USE_LED_8
|
||||
#define USE_LED_8 0
|
||||
#endif
|
||||
#define LED_8_GPIO GPIOB
|
||||
#define LED_8_GPIO_CLK RCC_AHB1ENR_IOPBEN
|
||||
#define LED_8_GPIO_PIN GPIO15
|
||||
#define LED_8_GPIO_ON gpio_set
|
||||
#define LED_8_GPIO_OFF gpio_clear
|
||||
#define LED_8_AFIO_REMAP ((void)0)
|
||||
|
||||
/* Power Switch, on PB12, 1 on LED_ON, 0 on LED_OFF */
|
||||
#ifndef USE_LED_9
|
||||
#define USE_LED_9 1
|
||||
#endif
|
||||
#define LED_9_GPIO GPIOB
|
||||
#define LED_9_GPIO_CLK RCC_AHB1ENR_IOPBEN
|
||||
#define LED_9_GPIO_PIN GPIO12
|
||||
#define LED_9_GPIO_ON gpio_set
|
||||
#define LED_9_GPIO_OFF gpio_clear
|
||||
#define LED_9_AFIO_REMAP ((void)0)
|
||||
|
||||
#define POWER_SWITCH_LED 9
|
||||
|
||||
|
||||
/* Pint to set Uart2 RX polarity, on PB13, output high inverts, low doesn't */
|
||||
#define RC_POLARITY_GPIO_PORT GPIOB
|
||||
#define RC_POLARITY_GPIO_PIN GPIO13
|
||||
|
||||
|
||||
/* Default actuators driver */
|
||||
#define DEFAULT_ACTUATORS "subsystems/actuators/actuators_pwm.h"
|
||||
#define ActuatorDefaultSet(_x,_y) ActuatorPwmSet(_x,_y)
|
||||
#define ActuatorsDefaultInit() ActuatorsPwmInit()
|
||||
#define ActuatorsDefaultCommit() ActuatorsPwmCommit()
|
||||
|
||||
#define DefaultVoltageOfAdc(adc) (0.006185*adc)
|
||||
|
||||
/* UART */
|
||||
#define UART1_GPIO_AF GPIO_AF7
|
||||
#define UART1_GPIO_PORT_RX GPIOA
|
||||
#define UART1_GPIO_RX GPIO10
|
||||
#define UART1_GPIO_PORT_TX GPIOB
|
||||
#define UART1_GPIO_TX GPIO6
|
||||
|
||||
#define UART2_GPIO_AF GPIO_AF7
|
||||
#define UART2_GPIO_PORT_RX GPIOA
|
||||
#define UART2_GPIO_RX GPIO3
|
||||
#define USE_UART2_TX FALSE
|
||||
|
||||
#define UART4_GPIO_AF GPIO_AF8
|
||||
#define UART4_GPIO_PORT_RX GPIOA
|
||||
#define UART4_GPIO_RX GPIO1
|
||||
#define UART4_GPIO_PORT_TX GPIOA
|
||||
#define UART4_GPIO_TX GPIO0
|
||||
|
||||
#define UART6_GPIO_AF GPIO_AF8
|
||||
#define UART6_GPIO_PORT_RX GPIOC
|
||||
#define UART6_GPIO_RX GPIO7
|
||||
#define UART6_GPIO_PORT_TX GPIOC
|
||||
#define UART6_GPIO_TX GPIO6
|
||||
|
||||
/* SPI */
|
||||
#define SPI1_GPIO_AF GPIO_AF5
|
||||
#define SPI1_GPIO_PORT_MISO GPIOA
|
||||
#define SPI1_GPIO_MISO GPIO6
|
||||
#define SPI1_GPIO_PORT_MOSI GPIOA
|
||||
#define SPI1_GPIO_MOSI GPIO7
|
||||
#define SPI1_GPIO_PORT_SCK GPIOA
|
||||
#define SPI1_GPIO_SCK GPIO5
|
||||
|
||||
#define SPI_SELECT_SLAVE0_PORT GPIOB
|
||||
#define SPI_SELECT_SLAVE0_PIN GPIO9
|
||||
|
||||
|
||||
/* Onboard ADCs */
|
||||
#define USE_AD_TIM4 1
|
||||
|
||||
#define BOARD_ADC_CHANNEL_1 9
|
||||
#define BOARD_ADC_CHANNEL_2 15
|
||||
#define BOARD_ADC_CHANNEL_3 14
|
||||
#define BOARD_ADC_CHANNEL_4 4
|
||||
|
||||
#ifndef USE_AD1
|
||||
#define USE_AD1 1
|
||||
#endif
|
||||
/* provide defines that can be used to access the ADC_x in the code or airframe file
|
||||
* these directly map to the index number of the 4 adc channels defined above
|
||||
* 4th (index 3) is used for bat monitoring by default
|
||||
*/
|
||||
// AUX 1
|
||||
#define ADC_1 ADC1_C1
|
||||
#ifdef USE_ADC_1
|
||||
#ifndef ADC_1_GPIO_CLOCK_PORT
|
||||
#define ADC_1_GPIO_CLOCK_PORT RCC_AHB1ENR_IOPBEN
|
||||
#define ADC_1_INIT() gpio_mode_setup(GPIOB, GPIO_MODE_ANALOG, GPIO_PUPD_NONE, GPIO1)
|
||||
#endif
|
||||
#define USE_AD1_1 1
|
||||
#else
|
||||
#define ADC_1_GPIO_CLOCK_PORT 0
|
||||
#define ADC_1_INIT() {}
|
||||
#endif
|
||||
|
||||
// AUX 2
|
||||
#define ADC_2 ADC1_C2
|
||||
#ifdef USE_ADC_2
|
||||
#ifndef ADC_2_GPIO_CLOCK_PORT
|
||||
#define ADC_2_GPIO_CLOCK_PORT RCC_AHB1ENR_IOPCEN
|
||||
#define ADC_2_INIT() gpio_mode_setup(GPIOC, GPIO_MODE_ANALOG, GPIO_PUPD_NONE, GPIO5)
|
||||
#endif
|
||||
#define USE_AD1_2 1
|
||||
#else
|
||||
#define ADC_2_GPIO_CLOCK_PORT 0
|
||||
#define ADC_2_INIT() {}
|
||||
#endif
|
||||
|
||||
// AUX 3
|
||||
#define ADC_3 ADC1_C3
|
||||
#ifdef USE_ADC_3
|
||||
#ifndef ADC_3_GPIO_CLOCK_PORT
|
||||
#define ADC_3_GPIO_CLOCK_PORT RCC_AHB1ENR_IOPCEN
|
||||
#define ADC_3_INIT() gpio_mode_setup(GPIOC, GPIO_MODE_ANALOG, GPIO_PUPD_NONE, GPIO4)
|
||||
#endif
|
||||
#define USE_AD1_3 1
|
||||
#else
|
||||
#define ADC_3_GPIO_CLOCK_PORT 0
|
||||
#define ADC_3_INIT() {}
|
||||
#endif
|
||||
|
||||
// BAT
|
||||
#define ADC_4 ADC1_C4
|
||||
#ifndef ADC_4_GPIO_CLOCK_PORT
|
||||
#define ADC_4_GPIO_CLOCK_PORT RCC_AHB1ENR_IOPAEN
|
||||
#define ADC_4_INIT() gpio_mode_setup(GPIOA, GPIO_MODE_ANALOG, GPIO_PUPD_NONE, GPIO4)
|
||||
#endif
|
||||
#define USE_AD1_4 1
|
||||
|
||||
/* allow to define ADC_CHANNEL_VSUPPLY in the airframe file*/
|
||||
#ifndef ADC_CHANNEL_VSUPPLY
|
||||
#define ADC_CHANNEL_VSUPPLY ADC_4
|
||||
#endif
|
||||
|
||||
#define ADC_GPIO_CLOCK_PORT (ADC_1_GPIO_CLOCK_PORT | ADC_2_GPIO_CLOCK_PORT | ADC_3_GPIO_CLOCK_PORT | ADC_4_GPIO_CLOCK_PORT)
|
||||
|
||||
/* GPIO mapping for ADC1 pins, overwrites the default in arch/stm32/mcu_periph/adc_arch.c */
|
||||
#ifdef USE_AD1
|
||||
#define ADC1_GPIO_INIT(gpio) { \
|
||||
ADC_1_INIT(); \
|
||||
ADC_2_INIT(); \
|
||||
ADC_3_INIT(); \
|
||||
ADC_4_INIT(); \
|
||||
}
|
||||
#endif // USE_AD1
|
||||
|
||||
|
||||
/* I2C mapping */
|
||||
#define I2C1_GPIO_PORT GPIOB
|
||||
#define I2C1_GPIO_SCL GPIO8
|
||||
#define I2C1_GPIO_SDA GPIO7
|
||||
|
||||
#define I2C2_GPIO_PORT GPIOB
|
||||
#define I2C2_GPIO_SCL GPIO10
|
||||
#define I2C2_GPIO_SDA GPIO11
|
||||
|
||||
|
||||
/* Activate onboard baro */
|
||||
#define BOARD_HAS_BARO 1
|
||||
|
||||
/* PWM */
|
||||
#define PWM_USE_TIM2 1
|
||||
#define PWM_USE_TIM3 1
|
||||
|
||||
#define USE_PWM0 1
|
||||
#define USE_PWM1 1
|
||||
#define USE_PWM2 1
|
||||
#define USE_PWM3 1
|
||||
#define USE_PWM4 1
|
||||
#define USE_PWM5 1
|
||||
|
||||
// PWM_SERVO_x is the index of the servo in the actuators_pwm_values array
|
||||
#if USE_PWM0
|
||||
#define PWM_SERVO_0 0
|
||||
#define PWM_SERVO_0_TIMER TIM3
|
||||
#define PWM_SERVO_0_RCC_IOP RCC_AHB1ENR_IOPBEN
|
||||
#define PWM_SERVO_0_GPIO GPIOB
|
||||
#define PWM_SERVO_0_PIN GPIO0
|
||||
#define PWM_SERVO_0_AF GPIO_AF2
|
||||
#define PWM_SERVO_0_OC TIM_OC3
|
||||
#define PWM_SERVO_0_OC_BIT (1<<2)
|
||||
#else
|
||||
#define PWM_SERVO_0_OC_BIT 0
|
||||
#endif
|
||||
|
||||
#if USE_PWM1
|
||||
#define PWM_SERVO_1 1
|
||||
#define PWM_SERVO_1_TIMER TIM2
|
||||
#define PWM_SERVO_1_RCC_IOP RCC_AHB1ENR_IOPAEN
|
||||
#define PWM_SERVO_1_GPIO GPIOA
|
||||
#define PWM_SERVO_1_PIN GPIO2
|
||||
#define PWM_SERVO_1_AF GPIO_AF1
|
||||
#define PWM_SERVO_1_OC TIM_OC3
|
||||
#define PWM_SERVO_1_OC_BIT (1<<2)
|
||||
#else
|
||||
#define PWM_SERVO_1_OC_BIT 0
|
||||
#endif
|
||||
|
||||
#if USE_PWM2
|
||||
#define PWM_SERVO_2 2
|
||||
#define PWM_SERVO_2_TIMER TIM3
|
||||
#define PWM_SERVO_2_RCC_IOP RCC_AHB1ENR_IOPBEN
|
||||
#define PWM_SERVO_2_GPIO GPIOB
|
||||
#define PWM_SERVO_2_PIN GPIO5
|
||||
#define PWM_SERVO_2_AF GPIO_AF2
|
||||
#define PWM_SERVO_2_OC TIM_OC2
|
||||
#define PWM_SERVO_2_OC_BIT (1<<1)
|
||||
#else
|
||||
#define PWM_SERVO_2_OC_BIT 0
|
||||
#endif
|
||||
|
||||
#if USE_PWM3
|
||||
#define PWM_SERVO_3_IDX 3
|
||||
#define PWM_SERVO_3_TIMER TIM3
|
||||
#define PWM_SERVO_3_RCC_IOP RCC_AHB1ENR_IOPBEN
|
||||
#define PWM_SERVO_3_GPIO GPIOB
|
||||
#define PWM_SERVO_3_PIN GPIO4
|
||||
#define PWM_SERVO_3_AF GPIO_AF2
|
||||
#define PWM_SERVO_3_OC TIM_OC1
|
||||
#define PWM_SERVO_3_OC_BIT (1<<0)
|
||||
#else
|
||||
#define PWM_SERVO_3_OC_BIT 0
|
||||
#endif
|
||||
|
||||
#if USE_PWM4
|
||||
#define PWM_SERVO_4 4
|
||||
#define PWM_SERVO_4_TIMER TIM2
|
||||
#define PWM_SERVO_4_RCC_IOP RCC_AHB1ENR_IOPBEN
|
||||
#define PWM_SERVO_4_GPIO GPIOB
|
||||
#define PWM_SERVO_4_PIN GPIO3
|
||||
#define PWM_SERVO_4_AF GPIO_AF1
|
||||
#define PWM_SERVO_4_OC TIM_OC2
|
||||
#define PWM_SERVO_4_OC_BIT (1<<1)
|
||||
#else
|
||||
#define PWM_SERVO_4_OC_BIT 0
|
||||
#endif
|
||||
|
||||
#if USE_PWM5
|
||||
#define PWM_SERVO_5 5
|
||||
#define PWM_SERVO_5_TIMER TIM2
|
||||
#define PWM_SERVO_5_RCC_IOP RCC_AHB1ENR_IOPAEN
|
||||
#define PWM_SERVO_5_GPIO GPIOA
|
||||
#define PWM_SERVO_5_PIN GPIO15
|
||||
#define PWM_SERVO_5_AF GPIO_AF1
|
||||
#define PWM_SERVO_5_OC TIM_OC1
|
||||
#define PWM_SERVO_5_OC_BIT (1<<0)
|
||||
#else
|
||||
#define PWM_SERVO_5_OC_BIT 0
|
||||
#endif
|
||||
|
||||
// PWM AUX1 (conflict with ADC0)
|
||||
#if USE_PWM6
|
||||
#define PWM_SERVO_6 6
|
||||
#define PWM_SERVO_6_TIMER TIM3
|
||||
#define PWM_SERVO_6_RCC_IOP RCC_AHB1ENR_IOPBEN
|
||||
#define PWM_SERVO_6_GPIO GPIOB
|
||||
#define PWM_SERVO_6_PIN GPIO1
|
||||
#define PWM_SERVO_6_AF GPIO_AF2
|
||||
#define PWM_SERVO_6_OC TIM_OC4
|
||||
#define PWM_SERVO_6_OC_BIT (1<<3)
|
||||
#else
|
||||
#define PWM_SERVO_6_OC_BIT 0
|
||||
#endif
|
||||
|
||||
|
||||
#define PWM_TIM2_CHAN_MASK (PWM_SERVO_1_OC_BIT|PWM_SERVO_4_OC_BIT|PWM_SERVO_5_OC_BIT)
|
||||
#define PWM_TIM3_CHAN_MASK (PWM_SERVO_0_OC_BIT|PWM_SERVO_2_OC_BIT|PWM_SERVO_3_OC_BIT|PWM_SERVO_6_OC_BIT)
|
||||
|
||||
/*
|
||||
* PPM
|
||||
*/
|
||||
#define USE_PPM_TIM1 1
|
||||
|
||||
#define PPM_CHANNEL TIM_IC1
|
||||
#define PPM_TIMER_INPUT TIM_IC_IN_TI1
|
||||
#define PPM_IRQ NVIC_TIM1_CC_IRQ
|
||||
#define PPM_IRQ2 NVIC_TIM1_UP_TIM10_IRQ
|
||||
// Capture/Compare InteruptEnable and InterruptFlag
|
||||
#define PPM_CC_IE TIM_DIER_CC1IE
|
||||
#define PPM_CC_IF TIM_SR_CC1IF
|
||||
#define PPM_GPIO_PORT GPIOA
|
||||
#define PPM_GPIO_PIN GPIO8
|
||||
#define PPM_GPIO_AF GPIO_AF1
|
||||
|
||||
/*
|
||||
* Spektrum
|
||||
*/
|
||||
/* The line that is pulled low at power up to initiate the bind process */
|
||||
#define SPEKTRUM_BIND_PIN GPIO8
|
||||
#define SPEKTRUM_BIND_PIN_PORT GPIOA
|
||||
|
||||
#define SPEKTRUM_UART2_RCC_REG &RCC_APB1ENR
|
||||
#define SPEKTRUM_UART2_RCC_DEV RCC_APB1ENR_USART2EN
|
||||
#define SPEKTRUM_UART2_BANK GPIOA
|
||||
#define SPEKTRUM_UART2_PIN GPIO3
|
||||
#define SPEKTRUM_UART2_AF GPIO_AF7
|
||||
#define SPEKTRUM_UART2_IRQ NVIC_USART2_IRQ
|
||||
#define SPEKTRUM_UART2_ISR usart2_isr
|
||||
#define SPEKTRUM_UART2_DEV USART2
|
||||
|
||||
#endif /* CONFIG_APOGEE_0_99_H */
|
||||
@@ -32,7 +32,7 @@
|
||||
#include <math.h>
|
||||
#include "imu_umarim.h"
|
||||
#include "mcu_periph/i2c.h"
|
||||
#include "led.h"
|
||||
#include "generated/airframe.h"
|
||||
|
||||
// Downlink
|
||||
#include "mcu_periph/uart.h"
|
||||
@@ -40,6 +40,11 @@
|
||||
#include "subsystems/datalink/downlink.h"
|
||||
|
||||
|
||||
#ifndef UMARIM_ACCEL_RANGE
|
||||
#define UMARIM_ACCEL_RANGE ADXL345_RANGE_16G
|
||||
#endif
|
||||
PRINT_CONFIG_VAR(UMARIM_ACCEL_RANGE)
|
||||
|
||||
#ifndef UMARIM_ACCEL_RATE
|
||||
#define UMARIM_ACCEL_RATE ADXL345_RATE_50HZ
|
||||
#endif
|
||||
@@ -71,6 +76,7 @@ void imu_impl_init(void)
|
||||
adxl345_i2c_init(&imu_umarim.adxl, &(IMU_UMARIM_I2C_DEV), ADXL345_ADDR_ALT);
|
||||
// change the default data rate
|
||||
imu_umarim.adxl.config.rate = UMARIM_ACCEL_RATE;
|
||||
imu_umarim.adxl.config.range = UMARIM_ACCEL_RANGE;
|
||||
|
||||
imu_umarim.gyr_valid = FALSE;
|
||||
imu_umarim.acc_valid = FALSE;
|
||||
|
||||
@@ -70,6 +70,9 @@ uint8_t joystick_block;
|
||||
}
|
||||
#endif
|
||||
|
||||
#if defined RADIO_CONTROL && defined RADIO_CONTROL_TYPE_DATALINK
|
||||
#include "subsystems/radio_control/rc_datalink.h"
|
||||
#endif
|
||||
|
||||
#define MOfCm(_x) (((float)(_x))/100.)
|
||||
|
||||
|
||||
@@ -157,6 +157,10 @@ void init_ap( void ) {
|
||||
mcu_init();
|
||||
#endif /* SINGLE_MCU */
|
||||
|
||||
/****** initialize and reset state interface ********/
|
||||
|
||||
stateInit();
|
||||
|
||||
/************* Sensors initialization ***************/
|
||||
#if USE_GPS
|
||||
gps_init();
|
||||
@@ -184,8 +188,6 @@ void init_ap( void ) {
|
||||
|
||||
ins_init();
|
||||
|
||||
stateInit();
|
||||
|
||||
/************* Links initialization ***************/
|
||||
#if defined MCU_SPI_LINK || defined MCU_UART_LINK
|
||||
link_mcu_init();
|
||||
|
||||
@@ -74,6 +74,16 @@ static inline void led_init ( void ) {
|
||||
LED_OFF(8);
|
||||
#endif /* LED_8 */
|
||||
|
||||
#if USE_LED_9
|
||||
LED_INIT(9);
|
||||
LED_OFF(9);
|
||||
#endif /* LED_9 */
|
||||
|
||||
#if USE_LED_10
|
||||
LED_INIT(10);
|
||||
LED_OFF(10);
|
||||
#endif /* LED_10 */
|
||||
|
||||
#ifdef USE_LED_BODY
|
||||
LED_INIT(BODY);
|
||||
LED_OFF(BODY);
|
||||
|
||||
@@ -46,10 +46,21 @@
|
||||
#define B19200 19200
|
||||
#define B38400 38400
|
||||
#define B57600 57600
|
||||
#define B100000 100000
|
||||
#define B115200 115200
|
||||
#define B230400 230400
|
||||
#define B921600 921600
|
||||
|
||||
#define UBITS_7 7
|
||||
#define UBITS_8 8
|
||||
|
||||
#define USTOP_1 1
|
||||
#define USTOP_2 2
|
||||
|
||||
#define UPARITY_NO 0
|
||||
#define UPARITY_ODD 1
|
||||
#define UPARITY_EVEN 2
|
||||
|
||||
/**
|
||||
* UART peripheral
|
||||
*/
|
||||
@@ -75,6 +86,7 @@ struct uart_periph {
|
||||
|
||||
extern void uart_periph_init(struct uart_periph* p);
|
||||
extern void uart_periph_set_baudrate(struct uart_periph* p, uint32_t baud);
|
||||
extern void uart_periph_set_bits_stop_parity(struct uart_periph* p, uint8_t bits, uint8_t stop, uint8_t parity);
|
||||
extern void uart_periph_set_mode(struct uart_periph* p, bool_t tx_enabled, bool_t rx_enabled, bool_t hw_flow_control);
|
||||
extern void uart_transmit(struct uart_periph* p, uint8_t data);
|
||||
extern bool_t uart_check_free_space(struct uart_periph* p, uint8_t len);
|
||||
@@ -97,6 +109,7 @@ extern void uart0_init(void);
|
||||
#define UART0Getch() uart_getch(&uart0)
|
||||
#define UART0TxRunning uart0.tx_running
|
||||
#define UART0SetBaudrate(_b) uart_periph_set_baudrate(&uart0, _b)
|
||||
#define UART0SetBitsStopParity(_b, _s, _p) uart_periph_set_bits_stop_parity(&uart0, _b, _s, _p)
|
||||
|
||||
#endif // USE_UART0
|
||||
|
||||
@@ -112,6 +125,7 @@ extern void uart1_init(void);
|
||||
#define UART1Getch() uart_getch(&uart1)
|
||||
#define UART1TxRunning uart1.tx_running
|
||||
#define UART1SetBaudrate(_b) uart_periph_set_baudrate(&uart1, _b)
|
||||
#define UART1SetBitsStopParity(_b, _s, _p) uart_periph_set_bits_stop_parity(&uart1, _b, _s, _p)
|
||||
|
||||
#endif // USE_UART1
|
||||
|
||||
@@ -127,6 +141,7 @@ extern void uart2_init(void);
|
||||
#define UART2Getch() uart_getch(&uart2)
|
||||
#define UART2TxRunning uart2.tx_running
|
||||
#define UART2SetBaudrate(_b) uart_periph_set_baudrate(&uart2, _b)
|
||||
#define UART2SetBitsStopParity(_b, _s, _p) uart_periph_set_bits_stop_parity(&uart2, _b, _s, _p)
|
||||
|
||||
#endif // USE_UART2
|
||||
|
||||
@@ -142,6 +157,7 @@ extern void uart3_init(void);
|
||||
#define UART3Getch() uart_getch(&uart3)
|
||||
#define UART3TxRunning uart3.tx_running
|
||||
#define UART3SetBaudrate(_b) uart_periph_set_baudrate(&uart3, _b)
|
||||
#define UART3SetBitsStopParity(_b, _s, _p) uart_periph_set_bits_stop_parity(&uart3, _b, _s, _p)
|
||||
|
||||
#endif // USE_UART3
|
||||
|
||||
@@ -157,6 +173,7 @@ extern void uart4_init(void);
|
||||
#define UART4Getch() uart_getch(&uart4)
|
||||
#define UART4TxRunning uart4.tx_running
|
||||
#define UART4SetBaudrate(_b) uart_periph_set_baudrate(&uart4, _b)
|
||||
#define UART4SetBitsStopParity(_b, _s, _p) uart_periph_set_bits_stop_parity(&uart4, _b, _s, _p)
|
||||
|
||||
#endif // USE_UART4
|
||||
|
||||
@@ -172,6 +189,7 @@ extern void uart5_init(void);
|
||||
#define UART5Getch() uart_getch(&uart5)
|
||||
#define UART5TxRunning uart5.tx_running
|
||||
#define UART5SetBaudrate(_b) uart_periph_set_baudrate(&uart5, _b)
|
||||
#define UART5SetBitsStopParity(_b, _s, _p) uart_periph_set_bits_stop_parity(&uart5, _b, _s, _p)
|
||||
|
||||
#endif // USE_UART5
|
||||
|
||||
@@ -187,6 +205,7 @@ extern void uart6_init(void);
|
||||
#define UART6Getch() uart_getch(&uart6)
|
||||
#define UART6TxRunning uart6.tx_running
|
||||
#define UART6SetBaudrate(_b) uart_periph_set_baudrate(&uart6, _b)
|
||||
#define UART6SetBitsStopParity(_b, _s, _p) uart_periph_set_bits_stop_parity(&uart6, _b, _s, _p)
|
||||
|
||||
#endif // USE_UART6
|
||||
|
||||
|
||||
@@ -1,91 +0,0 @@
|
||||
#include "std.h"
|
||||
#include "mcu.h"
|
||||
#include "mcu_periph/sys_time.h"
|
||||
#include "led.h"
|
||||
#include "mcu_periph/uart.h"
|
||||
|
||||
#include "messages.h"
|
||||
#include "subsystems/datalink/downlink.h"
|
||||
|
||||
#include "subsystems/datalink/datalink.h"
|
||||
#include "generated/settings.h"
|
||||
#include "dl_protocol.h"
|
||||
|
||||
#include "mcu_periph/spi.h"
|
||||
#include "sd_card.h"
|
||||
|
||||
static inline void main_init( void );
|
||||
static inline void main_periodic_task( void );
|
||||
static inline void main_event_task( void );
|
||||
|
||||
|
||||
uint8_t dl_buffer[MSG_SIZE] __attribute__ ((aligned));
|
||||
bool_t dl_msg_available;
|
||||
uint16_t datalink_time;
|
||||
|
||||
int main( void ) {
|
||||
main_init();
|
||||
while(1) {
|
||||
if (sys_time_check_and_ack_timer(0))
|
||||
main_periodic_task();
|
||||
main_event_task( );
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
static inline void main_init( void ) {
|
||||
mcu_init();
|
||||
sys_time_register_timer((1./PERIODIC_FREQUENCY), NULL);
|
||||
led_init();
|
||||
uart_init(&uart0);
|
||||
|
||||
spi_init();
|
||||
sd_card_init();
|
||||
|
||||
mcu_int_enable();
|
||||
}
|
||||
|
||||
static inline void main_periodic_task( void ) {
|
||||
LED_TOGGLE(1);
|
||||
//DOWNLINK_SEND_DEBUG(3,buf_input);
|
||||
}
|
||||
|
||||
static inline void main_event_task( void ) {
|
||||
DatalinkEvent();
|
||||
|
||||
// spi event
|
||||
if (spi_message_received) {
|
||||
/* Got a message on SPI. */
|
||||
spi_message_received = FALSE;
|
||||
sd_card_event();
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
#define IdOfMsg(x) (x[1])
|
||||
|
||||
void dl_parse_msg(void) {
|
||||
|
||||
LED_TOGGLE(1);
|
||||
|
||||
uint8_t msg_id = IdOfMsg(dl_buffer);
|
||||
switch (msg_id) {
|
||||
|
||||
case DL_PING: {
|
||||
DOWNLINK_SEND_PONG();
|
||||
break;
|
||||
}
|
||||
|
||||
case DL_SETTING : {
|
||||
uint8_t i = DL_SETTING_index(dl_buffer);
|
||||
float var = DL_SETTING_value(dl_buffer);
|
||||
DlSetting(i, var);
|
||||
DOWNLINK_SEND_DL_VALUE(&i, &var);
|
||||
break;
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -1,105 +0,0 @@
|
||||
/*
|
||||
* Copyright (C) 2007 ENAC
|
||||
*
|
||||
* 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
|
||||
* \brief
|
||||
*
|
||||
*/
|
||||
|
||||
#include "sd_card.h"
|
||||
#include "spi.h"
|
||||
|
||||
bool_t sd_card_available;
|
||||
|
||||
static bool_t status_read_data;
|
||||
|
||||
#define CMD_INIT_1 0x24 // set chanel AIN1/AIN2 and next operation on filter high
|
||||
#define CMD_INIT_2 0xCF // set unipolar mode, 24 bits, no boost, filter high
|
||||
#define CMD_INIT_3 0x34 // set chanel AIN1/AIN2 and next operation on filter low
|
||||
#define CMD_INIT_4 0x00 // set low filter
|
||||
#define CMD_INIT_5 0x14 // set chanel AIN1/AIN2 and next operation on mode register
|
||||
#define CMD_INIT_6 0x20 // set gain to 1, burnout current off, no filter sync, self calibration
|
||||
#define CMD_MEASUREMENT 0x54 // set chanel AIN1/AIN2 and next operation on data register
|
||||
|
||||
|
||||
uint8_t buf_input[3];
|
||||
uint8_t buf_output[3];
|
||||
|
||||
|
||||
static void send1_on_spi(uint8_t d) {
|
||||
buf_output[0] = d;
|
||||
spi_buffer_length = 1;
|
||||
|
||||
spi_buffer_input = (uint8_t*)&buf_input;
|
||||
spi_buffer_output = (uint8_t*)&buf_output;
|
||||
SpiStart();
|
||||
}
|
||||
|
||||
|
||||
void sd_card_init( void ) {
|
||||
|
||||
send1_on_spi(CMD_INIT_1);
|
||||
send1_on_spi(CMD_INIT_2);
|
||||
send1_on_spi(CMD_INIT_3);
|
||||
send1_on_spi(CMD_INIT_4);
|
||||
send1_on_spi(CMD_INIT_5);
|
||||
send1_on_spi(CMD_INIT_6);
|
||||
|
||||
status_read_data = FALSE;
|
||||
sd_card_available = FALSE;
|
||||
|
||||
}
|
||||
|
||||
void sd_card_periodic(void) {
|
||||
if (!SpiCheckAvailable()) {
|
||||
SpiOverRun();
|
||||
return;
|
||||
}
|
||||
|
||||
if (status_read_data) {
|
||||
buf_output[0] = buf_output[1] = buf_output[2] = 0;
|
||||
spi_buffer_length = 3;
|
||||
}
|
||||
else {
|
||||
buf_output[0] = CMD_MEASUREMENT;
|
||||
spi_buffer_length = 1;
|
||||
}
|
||||
|
||||
spi_buffer_input = (uint8_t*)&buf_input;
|
||||
spi_buffer_output = (uint8_t*)&buf_output;
|
||||
//if (status_read_data)
|
||||
// SpiSetCPHA();
|
||||
//else
|
||||
// SpiClrCPHA();
|
||||
SpiStart();
|
||||
}
|
||||
|
||||
/* Handle the SPI message, i.e. store the received values in variables */
|
||||
void sd_card_event( void ) {
|
||||
|
||||
if (status_read_data) {
|
||||
} /* else nothing to read */
|
||||
|
||||
status_read_data = !status_read_data;
|
||||
|
||||
}
|
||||
|
||||
@@ -1,38 +0,0 @@
|
||||
/*
|
||||
* Copyright (C) 2007 ENAC
|
||||
*
|
||||
* 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
|
||||
* \brief
|
||||
*
|
||||
*/
|
||||
|
||||
#include "std.h"
|
||||
|
||||
extern uint8_t buf_input[3];
|
||||
extern uint8_t buf_output[3];
|
||||
|
||||
extern bool_t sd_card_available;
|
||||
|
||||
extern void sd_card_init(void);
|
||||
extern void sd_card_periodic(void);
|
||||
extern void sd_card_event(void);
|
||||
|
||||
@@ -20,7 +20,6 @@
|
||||
* Theory: http://code.google.com/p/gentlenav/downloads/list file DCMDraft2.pdf
|
||||
*
|
||||
* Options:
|
||||
* - USE_HIGH_ACCEL_FLAG: no compensation when high accelerations present
|
||||
* - USE_MAGNETOMETER_ONGROUND: use magnetic compensation before takeoff only while GPS course not good
|
||||
* - USE_AHRS_GPS_ACCELERATIONS: forward acceleration compensation from GPS speed
|
||||
*
|
||||
@@ -111,18 +110,6 @@ int renorm_blowup_count = 0;
|
||||
float imu_health = 0.;
|
||||
#endif
|
||||
|
||||
#if USE_HIGH_ACCEL_FLAG
|
||||
// High Accel Flag
|
||||
#define HIGH_ACCEL_LOW_SPEED 15.0
|
||||
#define HIGH_ACCEL_LOW_SPEED_RESUME 4.0 // Hysteresis
|
||||
#define HIGH_ACCEL_HIGH_THRUST (0.8*MAX_PPRZ)
|
||||
#define HIGH_ACCEL_HIGH_THRUST_RESUME (0.1*MAX_PPRZ) // Hysteresis
|
||||
bool_t high_accel_done;
|
||||
bool_t high_accel_flag;
|
||||
// Command vector for thrust (fixed_wing)
|
||||
#include "inter_mcu.h"
|
||||
#endif
|
||||
|
||||
|
||||
static inline void set_dcm_matrix_from_rmat(struct FloatRMat *rmat)
|
||||
{
|
||||
@@ -152,11 +139,6 @@ void ahrs_init(void) {
|
||||
/* set inital filter dcm */
|
||||
set_dcm_matrix_from_rmat(&ahrs_impl.body_to_imu_rmat);
|
||||
|
||||
#if USE_HIGH_ACCEL_FLAG
|
||||
high_accel_done = FALSE;
|
||||
high_accel_flag = FALSE;
|
||||
#endif
|
||||
|
||||
ahrs_impl.gps_speed = 0;
|
||||
ahrs_impl.gps_acceleration = 0;
|
||||
ahrs_impl.gps_course = 0;
|
||||
@@ -434,25 +416,6 @@ void Drift_correction(void)
|
||||
// Weight for accelerometer info (<0.5G = 0.0, 1G = 1.0 , >1.5G = 0.0)
|
||||
Accel_weight = Chop(1 - 2*fabs(1 - Accel_magnitude),0,1); //
|
||||
|
||||
#if USE_HIGH_ACCEL_FLAG
|
||||
// Test for high acceleration:
|
||||
// - low speed
|
||||
// - high thrust
|
||||
float speed = *stateGetHorizontalSpeedNorm_f();
|
||||
if (speed < HIGH_ACCEL_LOW_SPEED && ap_state->commands[COMMAND_THROTTLE] > HIGH_ACCEL_HIGH_THRUST && !high_accel_done) {
|
||||
high_accel_flag = TRUE;
|
||||
} else {
|
||||
high_accel_flag = FALSE;
|
||||
if (speed > HIGH_ACCEL_LOW_SPEED && !high_accel_done) {
|
||||
high_accel_done = TRUE; // After takeoff, don't use high accel before landing (GS small, Throttle small)
|
||||
}
|
||||
if (speed < HIGH_ACCEL_HIGH_THRUST_RESUME && ap_state->commands[COMMAND_THROTTLE] < HIGH_ACCEL_HIGH_THRUST_RESUME) {
|
||||
high_accel_done = FALSE; // Activate high accel after landing
|
||||
}
|
||||
}
|
||||
if (high_accel_flag) { Accel_weight = 0.; }
|
||||
#endif
|
||||
|
||||
|
||||
#if PERFORMANCE_REPORTING == 1
|
||||
{
|
||||
|
||||
@@ -0,0 +1,145 @@
|
||||
/*
|
||||
* Copyright (C) 2013 Alexandre Bustico, Gautier Hattenberger
|
||||
*
|
||||
* This file is part of paparazzi.
|
||||
*
|
||||
* paparazzi is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 2, or (at your option)
|
||||
* any later version.
|
||||
*
|
||||
* paparazzi is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with paparazzi; see the file COPYING. If not, write to
|
||||
* the Free Software Foundation, 59 Temple Place - Suite 330,
|
||||
* Boston, MA 02111-1307, USA.
|
||||
*/
|
||||
|
||||
/** @file subsystems/radio_control/sbus.c
|
||||
*
|
||||
* Futaba SBUS decoder
|
||||
*/
|
||||
|
||||
#include "subsystems/radio_control.h"
|
||||
#include "subsystems/radio_control/sbus.h"
|
||||
#include BOARD_CONFIG
|
||||
#include "mcu_periph/uart.h"
|
||||
#include "mcu_periph/gpio.h"
|
||||
#include <string.h>
|
||||
|
||||
/*
|
||||
* SBUS protocol and state machine status
|
||||
*/
|
||||
#define SBUS_START_BYTE 0x0f
|
||||
#define SBUS_END_BYTE 0x00
|
||||
#define SBUS_BIT_PER_CHANNEL 11
|
||||
#define SBUS_BIT_PER_BYTE 8
|
||||
#define SBUS_FLAGS_BYTE 22
|
||||
#define SBUS_FRAME_LOST_BIT 2
|
||||
|
||||
#define SBUS_STATUS_UNINIT 0
|
||||
#define SBUS_STATUS_GOT_START 1
|
||||
|
||||
/** Set polarity using RC_POLARITY_GPIO.
|
||||
* SBUS signal has a reversed polarity compared to normal UART
|
||||
* this allows to using hardware UART peripheral by changing
|
||||
* the input signal polarity.
|
||||
* Setting this gpio ouput high inverts the signal,
|
||||
* output low sets it to normal polarity.
|
||||
*/
|
||||
#ifndef RC_SET_POLARITY
|
||||
#define RC_SET_POLARITY gpio_output_high
|
||||
#endif
|
||||
|
||||
|
||||
/** SBUS struct */
|
||||
struct _sbus sbus;
|
||||
|
||||
// Init function
|
||||
void radio_control_impl_init(void) {
|
||||
sbus.frame_available = FALSE;
|
||||
sbus.status = SBUS_STATUS_UNINIT;
|
||||
|
||||
// Set UART parameters (100K, 8 bits, 2 stops, even parity)
|
||||
uart_periph_set_bits_stop_parity(&SBUS_UART_DEV, UBITS_8, USTOP_2, UPARITY_EVEN);
|
||||
uart_periph_set_baudrate(&SBUS_UART_DEV, B100000);
|
||||
|
||||
// Set polarity
|
||||
#ifdef RC_POLARITY_GPIO_PORT
|
||||
gpio_setup_output(RC_POLARITY_GPIO_PORT, RC_POLARITY_GPIO_PIN);
|
||||
RC_SET_POLARITY(RC_POLARITY_GPIO_PORT, RC_POLARITY_GPIO_PIN);
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
/** Decode the raw buffer */
|
||||
static void decode_sbus_buffer (const uint8_t *src, uint16_t *dst, bool_t *available)
|
||||
{
|
||||
// reset counters
|
||||
uint8_t byteInRawBuf = 0;
|
||||
uint8_t bitInRawBuf = 0;
|
||||
uint8_t channel = 0;
|
||||
uint8_t bitInChannel = 0;
|
||||
|
||||
// clear bits
|
||||
memset (dst, 0, SBUS_NB_CHANNEL*sizeof(uint16_t));
|
||||
|
||||
// decode sbus data
|
||||
for (uint8_t i=0; i< (SBUS_NB_CHANNEL*SBUS_BIT_PER_CHANNEL); i++) {
|
||||
if (src[byteInRawBuf] & (1<<bitInRawBuf))
|
||||
dst[channel] |= (1<<bitInChannel);
|
||||
|
||||
bitInRawBuf++;
|
||||
bitInChannel++;
|
||||
|
||||
if (bitInRawBuf == SBUS_BIT_PER_BYTE) {
|
||||
bitInRawBuf = 0;
|
||||
byteInRawBuf++;
|
||||
}
|
||||
if (bitInChannel == SBUS_BIT_PER_CHANNEL) {
|
||||
bitInChannel = 0;
|
||||
channel++;
|
||||
}
|
||||
}
|
||||
// test frame lost flag
|
||||
*available = !bit_is_set(src[SBUS_FLAGS_BYTE], SBUS_FRAME_LOST_BIT);
|
||||
}
|
||||
|
||||
// Decoding event function
|
||||
// Reading from UART
|
||||
void sbus_decode_event(void) {
|
||||
uint8_t rbyte;
|
||||
if (uart_char_available(&SBUS_UART_DEV)) {
|
||||
do {
|
||||
rbyte = uart_getch(&SBUS_UART_DEV);
|
||||
switch (sbus.status) {
|
||||
case SBUS_STATUS_UNINIT:
|
||||
// Wait for the start byte
|
||||
if (rbyte == SBUS_START_BYTE) {
|
||||
sbus.status++;
|
||||
sbus.idx = 0;
|
||||
}
|
||||
break;
|
||||
case SBUS_STATUS_GOT_START:
|
||||
// Store buffer
|
||||
sbus.buffer[sbus.idx] = rbyte;
|
||||
sbus.idx++;
|
||||
if (sbus.idx == SBUS_BUF_LENGTH) {
|
||||
// Decode if last byte is the correct end byte
|
||||
if (rbyte == SBUS_END_BYTE) {
|
||||
decode_sbus_buffer(sbus.buffer, sbus.pulses, &sbus.frame_available);
|
||||
}
|
||||
sbus.status = SBUS_STATUS_UNINIT;
|
||||
}
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
} while (uart_char_available(&SBUS_UART_DEV));
|
||||
}
|
||||
}
|
||||
|
||||
@@ -0,0 +1,95 @@
|
||||
/*
|
||||
* Copyright (C) 2013 Alexandre Bustico, Gautier Hattenberger
|
||||
*
|
||||
* This file is part of paparazzi.
|
||||
*
|
||||
* paparazzi is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 2, or (at your option)
|
||||
* any later version.
|
||||
*
|
||||
* paparazzi is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with paparazzi; see the file COPYING. If not, write to
|
||||
* the Free Software Foundation, 59 Temple Place - Suite 330,
|
||||
* Boston, MA 02111-1307, USA.
|
||||
*/
|
||||
|
||||
#ifndef RC_SBUS_H
|
||||
#define RC_SBUS_H
|
||||
|
||||
/** @file subsystems/radio_control/sbus.h
|
||||
*
|
||||
* Futaba SBUS decoder
|
||||
*/
|
||||
|
||||
#include "std.h"
|
||||
|
||||
/**
|
||||
* Dummy macro to use radio.h file
|
||||
*/
|
||||
#define RC_PPM_TICKS_OF_USEC(_v) (_v)
|
||||
#define RC_PPM_SIGNED_TICKS_OF_USEC(_v) (_v)
|
||||
#define USEC_OF_RC_PPM_TICKS(_v) (_v)
|
||||
|
||||
/**
|
||||
* Generated code holding the description of a given
|
||||
* transmitter
|
||||
*/
|
||||
#include "generated/radio.h"
|
||||
|
||||
/**
|
||||
* Define number of channels.
|
||||
*
|
||||
* SBUS frame always have 16 channels
|
||||
* but only the X first one will be available
|
||||
* depending of the RC transmitter.
|
||||
* The radio XML file is used to assign the
|
||||
* input values to RC channels.
|
||||
*/
|
||||
#define SBUS_BUF_LENGTH 24
|
||||
#define SBUS_NB_CHANNEL 16
|
||||
#define RADIO_CONTROL_NB_CHANNEL SBUS_NB_CHANNEL
|
||||
|
||||
/**
|
||||
* SBUS structure
|
||||
*/
|
||||
struct _sbus {
|
||||
uint16_t pulses[SBUS_NB_CHANNEL]; ///< decoded values
|
||||
bool_t frame_available; ///< new frame available
|
||||
uint8_t buffer[SBUS_BUF_LENGTH]; ///< input buffer
|
||||
uint8_t idx; ///< input index
|
||||
uint8_t status; ///< decoder state machine status
|
||||
};
|
||||
|
||||
extern struct _sbus sbus;
|
||||
|
||||
/**
|
||||
* Decoding event function
|
||||
*/
|
||||
extern void sbus_decode_event(void);
|
||||
|
||||
/**
|
||||
* Event macro with handler callback
|
||||
*/
|
||||
#define RadioControlEvent(_received_frame_handler) { \
|
||||
sbus_decode_event(); \
|
||||
if (sbus.frame_available) { \
|
||||
radio_control.frame_cpt++; \
|
||||
radio_control.time_since_last_frame = 0; \
|
||||
if (radio_control.radio_ok_cpt > 0) { \
|
||||
radio_control.radio_ok_cpt--; \
|
||||
} else { \
|
||||
radio_control.status = RC_OK; \
|
||||
NormalizePpmIIR(sbus.pulses,radio_control); \
|
||||
_received_frame_handler(); \
|
||||
} \
|
||||
sbus.frame_available = FALSE; \
|
||||
} \
|
||||
}
|
||||
|
||||
#endif /* RC_SBUS_H */
|
||||
@@ -0,0 +1,67 @@
|
||||
|
||||
This is a script which loads GPS info into photo files (EXIF) based on the DC_SHOT messages
|
||||
in the telemetry data. The .data file contains DC_SHOT messages (possibly not all emitted by telemetry,
|
||||
but I'll get back to that later). This file together with the files in jpeg format are
|
||||
copied to a processing directory, where this script extracts the GPS position from each DC_SHOT message
|
||||
and edits the exif photo data in place, so effectively loads the GPS position into the EXIF data for
|
||||
each photo.
|
||||
|
||||
This script allows for gaps in DC_SHOT photo numbers and has some rudimentary error checking.
|
||||
|
||||
|
||||
Instructions for use (on Ubuntu):
|
||||
|
||||
1. Make sure python is installed.
|
||||
2. Install the necessary python packages:
|
||||
|
||||
# sudo apt-get install python-gdal
|
||||
# sudo apt-get install gir1.2-gexiv2-0.4
|
||||
|
||||
3. Create a directory for processing. The photo's EXIF data will be edited in place.
|
||||
4. Copy the .data file from the <paparazzi-dir>/var/log directory into this processing directory.
|
||||
5. Copy all photos taken during the session to the directory.
|
||||
|
||||
Verification:
|
||||
- Sort photos by name. Since most cameras output the photo name with a number at the end, this should
|
||||
show a list of photo names with consecutive numbers.
|
||||
- Verify that the first photo corresponds to the first DC_SHOT message (photonr == 1).
|
||||
- Add some dummy photos at the start to make up for any missing photos that may exist (such that they get
|
||||
sorted before the real actual one).
|
||||
|
||||
6. Run the script:
|
||||
|
||||
# python sw/tools/process_exif/process_exif.py /<processing-dir>
|
||||
|
||||
Verification of processing:
|
||||
- Look at the logs! These include the GPS positions calculated from UTM paparazzi positions.
|
||||
It has been tested on the southern+western hemispheres, but not yet on eastern and northern hemispheres.
|
||||
- Verify the tags in the photo:
|
||||
exiftool -v2 <processing-dir>/<some-photo-name>.jpg
|
||||
|
||||
----
|
||||
|
||||
Considerations:
|
||||
|
||||
The Ivy telemetry bus can get a little clogged up and this will result in discarded messages. This means that
|
||||
not all DC_SHOT messages actually emitted by telemetry need to be there on the ground station, so some photos
|
||||
won't have GPS coordinates loaded. Not all processing software for orthomosaics however require GPS positions
|
||||
for all photos, for example if they rely on recognizing corresponding features in overlapping photos.
|
||||
|
||||
Having more GPS coordinates in photos helps to improve accuracy, as the error in measurements approximates zero
|
||||
over time if the error follows a normal distribution.
|
||||
|
||||
Having many GPS coordinates is not enough however to achieve correct precision down to centimeters. Atmospheric conditions
|
||||
need to be eliminated by applying 3-5 ground control points in strategic locations. These conditions can easily
|
||||
cause the entire orthomosaic to be shifted over a meter.
|
||||
|
||||
|
||||
If your processing software does require the GPS coordinates per photo, you need to look into the use of a
|
||||
telemetry logger onboard. This logger can be hooked up separately on the tx+gnd lines of telemetry and "listen in" on
|
||||
the connection.
|
||||
|
||||
|
||||
The timing between actually taking a picture and the pulse event being sent is also an important consideration.
|
||||
Obviously the GPS frequency is also an issue. If this is used on a fixedwing, obviously if the plane is underway
|
||||
at 12m/s, 250ms will introduce a 3m bias on the position. Another reason to rely on GPS positions only as hints
|
||||
and apply ground control points to "set" the orthomosaic to the right location.
|
||||
|
||||
@@ -0,0 +1,166 @@
|
||||
#!/usr/bin/python
|
||||
#
|
||||
# <message NAME="DC_SHOT" ID="110">
|
||||
# <field TYPE="int16" NAME="photo_nr"/>
|
||||
# <field UNIT="cm" TYPE="int32" NAME="utm_east"/>
|
||||
# <field UNIT="cm" TYPE="int32" NAME="utm_north"/>
|
||||
# <field UNIT="m" TYPE="float" NAME="z"/>
|
||||
# <field TYPE="uint8" NAME="utm_zone"/>
|
||||
# <field UNIT="decideg" TYPE="int16" NAME="phi"/>
|
||||
# <field UNIT="decideg" TYPE="int16" NAME="theta"/>
|
||||
# <field UNIT="decideg" TYPE="int16" NAME="course"/>
|
||||
# <field UNIT="cm/s" TYPE="uint16" NAME="speed"/>
|
||||
# <field UNIT="ms" TYPE="uint32" NAME="itow"/>
|
||||
# </message>
|
||||
#
|
||||
|
||||
# sudo apt-get install python-gdal
|
||||
# sudo apt-get install gir1.2-gexiv2-0.4
|
||||
|
||||
from gi.repository import GExiv2
|
||||
import glob
|
||||
import os
|
||||
import re
|
||||
import fnmatch
|
||||
import sys
|
||||
import math
|
||||
|
||||
M_PI=3.14159265358979323846
|
||||
M_PI_2=(M_PI/2)
|
||||
M_PI_4=(M_PI/4)
|
||||
|
||||
def RadOfDeg( deg ):
|
||||
return (deg / M_PI) * 180.
|
||||
|
||||
# converts UTM coords to lat/long. Equations from USGS Bulletin 1532
|
||||
# East Longitudes are positive, West longitudes are negative.
|
||||
# North latitudes are positive, South latitudes are negative
|
||||
# Lat and Long are in decimal degrees.
|
||||
# Written by Chuck Gantz- chuck.gantz@globalstar.com
|
||||
|
||||
# ( I had some code here to use GDAL and which looked much simpler, but couldn't get that to work )
|
||||
|
||||
def UTMtoLL( northing, easting, utm_zone ):
|
||||
|
||||
k0 = 0.9996;
|
||||
a = 6378137; # WGS-84
|
||||
eccSquared = 0.00669438; # WGS-84
|
||||
e1 = (1-math.sqrt(1-eccSquared))/(1+math.sqrt(1-eccSquared));
|
||||
|
||||
x = easting - 500000.0; # remove 500,000 meter offset for longitude
|
||||
y = northing;
|
||||
|
||||
is_northern = northing < 0
|
||||
if ( not is_northern ):
|
||||
y -= 10000000.0 # remove 10,000,000 meter offset used for southern hemisphere
|
||||
|
||||
LongOrigin = (utm_zone - 1)*6 - 180 + 3; # +3 puts origin in middle of zone
|
||||
|
||||
eccPrimeSquared = (eccSquared)/(1-eccSquared);
|
||||
|
||||
M = y / k0;
|
||||
mu = M/(a*(1-eccSquared/4-3*eccSquared*eccSquared/64-5*eccSquared*eccSquared*eccSquared/256));
|
||||
|
||||
phi1Rad = mu + (3*e1/2-27*e1*e1*e1/32)*math.sin(2*mu) + (21*e1*e1/16-55*e1*e1*e1*e1/32)*math.sin(4*mu) +(151*e1*e1*e1/96)*math.sin(6*mu);
|
||||
phi1 = RadOfDeg(phi1Rad);
|
||||
|
||||
N1 = a/math.sqrt(1-eccSquared*math.sin(phi1Rad)*math.sin(phi1Rad));
|
||||
T1 = math.tan(phi1Rad)*math.tan(phi1Rad);
|
||||
C1 = eccPrimeSquared*math.cos(phi1Rad)*math.cos(phi1Rad);
|
||||
R1 = a*(1-eccSquared)/math.pow(1-eccSquared*math.sin(phi1Rad)*math.sin(phi1Rad), 1.5);
|
||||
D = x/(N1*k0);
|
||||
|
||||
Lat = phi1Rad - (N1*math.tan(phi1Rad)/R1)*(D*D/2-(5+3*T1+10*C1-4*C1*C1-9*eccPrimeSquared)*D*D*D*D/24+(61+90*T1+298*C1+45*T1*T1-252*eccPrimeSquared-3*C1*C1)*D*D*D*D*D*D/720);
|
||||
Lat = RadOfDeg(Lat)
|
||||
|
||||
Long = (D-(1+2*T1+C1)*D*D*D/6+(5-2*C1+28*T1-3*C1*C1+8*eccPrimeSquared+24*T1*T1)*D*D*D*D*D/120)/math.cos(phi1Rad)
|
||||
Long = LongOrigin + RadOfDeg(Long)
|
||||
|
||||
return Lat, Long
|
||||
|
||||
|
||||
# At least the directory must be given
|
||||
if len(sys.argv) < 2:
|
||||
print "This script requires one argument: A directory containing photos and the paparazzi .data file"
|
||||
sys.exit()
|
||||
|
||||
path = str(sys.argv[ 1] )
|
||||
|
||||
if os.path.isdir(path) == False:
|
||||
print "The indicated path '%s' is not a directory"%(path)
|
||||
sys.exit()
|
||||
|
||||
# Searching for all files with .data extension in indicated directory.
|
||||
# It should only have one.
|
||||
list_path = [i for i in os.listdir(path) if os.path.isfile(os.path.join(path, i))]
|
||||
files = [os.path.join(path, j) for j in list_path if re.match(fnmatch.translate('*.data'), j, re.IGNORECASE)]
|
||||
|
||||
if len(files) > 1:
|
||||
print "Too many data files found. Only one is allowed."
|
||||
sys.exit()
|
||||
|
||||
if len(files) == 0:
|
||||
print "No data files in 'data'. Copy data file there."
|
||||
sys.exit()
|
||||
|
||||
# Now searching for all photos (extension .jpg) in directory
|
||||
list_path = [i for i in os.listdir(path) if os.path.isfile(os.path.join(path, i))]
|
||||
photos = [os.path.join(path, j) for j in list_path if re.match(fnmatch.translate('*.jpg'), j, re.IGNORECASE)]
|
||||
|
||||
# Photos must be sorted by number
|
||||
photos.sort()
|
||||
|
||||
# Opening the data file, iterating all lines and searching for DC_SHOT messages
|
||||
f = open( files[0], 'r' )
|
||||
for line in f:
|
||||
line = line.rstrip()
|
||||
line = re.sub(' +',' ',line)
|
||||
if 'DC_SHOT' in line:
|
||||
# 618.710 1 DC_SHOT 212 29133350 -89510400 8.5 25 -9 29 0 0 385051650
|
||||
splitted = line.split( ' ' )
|
||||
|
||||
if len(splitted) < 12:
|
||||
continue
|
||||
try:
|
||||
photonr = int(splitted[ 3 ])
|
||||
utm_east = ( float(int(splitted[ 4 ])) / 100. )
|
||||
utm_north = ( float(int(splitted[ 5 ])) / 100. )
|
||||
alt = float(splitted[ 6 ])
|
||||
utm_zone = int(splitted[ 7 ])
|
||||
phi = int(splitted[ 8 ])
|
||||
theta = int(splitted[ 9 ])
|
||||
course = int(splitted[ 10 ])
|
||||
speed = int(splitted[ 11 ])
|
||||
|
||||
lon, lat = UTMtoLL( utm_north, utm_east, utm_zone )
|
||||
|
||||
# Check that there as many photos and pick the indicated one.
|
||||
# (this assumes the photos were taken correctly without a hiccup)
|
||||
# It would never be able to check this anyway, since the camera could stall or
|
||||
# not interpret the pulse? Leading to an incorrect GPS coordinate.
|
||||
if len( photos ) < photonr:
|
||||
print "Photo data %d found, but ran out of photos in directory"%(photonr)
|
||||
continue
|
||||
|
||||
# I've seen log files with -1 as DC_SHOT number due to an int8 I think. This should be
|
||||
# fixed now, but just in case someone runs this on old data.
|
||||
if (photonr < 0):
|
||||
print "Negative photonr found."
|
||||
continue
|
||||
|
||||
# Pick out photo, open it through exiv2,
|
||||
photoname = photos[ photonr - 1 ]
|
||||
photo = GExiv2.Metadata( photoname )
|
||||
|
||||
photo.set_gps_info(lat, lon, alt)
|
||||
photo.save_file()
|
||||
|
||||
print "Photo %s and photonr %d merged. Lat/Lon/Alt: %f, %f, %f"%(photoname, photonr, lat, lon, alt)
|
||||
|
||||
except ValueError as e:
|
||||
print "Cannot read line: %s"%(line)
|
||||
print "Value error(%s)"%(e)
|
||||
continue
|
||||
|
||||
print "Finished! exiting."
|
||||
|
||||
Reference in New Issue
Block a user