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

Merge branch 'master' into telemetry

Browse Source 
to resolve conflicts and test again before merging back to master

* master:
  [extras] Add CHDK script to work with digital_cam module
  [modules] digital_cam: add DC_POWER_OFF_LED
  [imu] remove old aspirin2 driver
  [modules] ets baro/airspeed: cleanup and default delay of 0.2s
  [conf] add krooz_sd fixedwing example, thx Sergei
  [conf] update krooz_sd examples, thx Sergei
  [boards] update krooz_sd, thx Sergei
  [stm32][pwm] add PWM_SERVO_9
  [modules] baro_amsys: usable for ins_alt_float with USE_BARO_AMSYS
  [rotorcraft] motor_mixing: only lower throttle when hitting both min/max saturations
  [ardrone2] Parrot GPS show sattelite information.
  Added check if autopilot_motors_on do determine if you want to switch to AP_MODE_FAILSAFE
  [spi] update apogee and krooz board files for spi
  [spi] spi for stm32f4
  [rotorcraft] fix MODE_STARTUP
  [krooz] add imu driver for KroozSD board from Sergei
  [imu][aspirin_2_spi] clear naming for mag axes assignment
  [modules] airspeed ets start delay
  [modules] baro ets start delay

Conflicts:
	sw/airborne/firmwares/rotorcraft/autopilot.c
	sw/airborne/modules/sensors/baro_ets.c
This commit is contained in:
Felix Ruess
2013-07-17 13:15:15 +02:00
parent 2b3a1da82e 8b9c207351
commit 871588eacc
47 changed files with 1412 additions and 848 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/ENAC/fixed-wing/apogee.xml
+5
View File
@@ -8,6 +8,9 @@
<modules>
<!--load name="sys_mon.xml"/-->
<load name="mcp355x.xml">
<define name="USE_SPI1"/>
</load>
</modules>
<firmware name="fixedwing">
@@ -21,6 +24,7 @@
<target name="sim" board="pc"/>
<target name="ap" board="apogee_0.99"/>
<configure name="FLASH_MODE" value="SWD"/>
<subsystem name="radio_control" type="ppm"/>
@@ -38,6 +42,7 @@
<!-- Sensors -->
<subsystem name="gps" type="ublox"/>
<subsystem name="spi_master"/>
</firmware>
<!-- commands section -->
conf/airframes/examples/MentorEnergy.xml
+3 -3
View File
@@ -9,7 +9,6 @@
<firmware name="fixedwing">
<target name="ap" board="lisa_m_1.0">
<define name="LISA_M_LONGITUDINAL_X"/>
<define name="SENSOR_SYNC_SEND"/>
<configure name="PERIODIC_FREQUENCY" value="120"/>
<configure name="AHRS_PROPAGATE_FREQUENCY" value="100"/>
@@ -27,7 +26,6 @@
<define name="STRONG_WIND"/>
<define name="WIND_INFO"/>
<define name="WIND_INFO_RET"/>
<define name="SENSOR_SYNC_SEND"/>
<!-- Sensors -->
<!--
<subsystem name="ahrs" type="int_cmpl_quat">
@@ -60,7 +58,9 @@
<modules>
<load name="gps_ubx_ucenter.xml"/>
<load name="airspeed_ets.xml"/>
<load name="airspeed_ets.xml">
<define name="AIRSPEED_ETS_SYNC_SEND"/>
</load>
<load name="adc_generic.xml">
<configure name="ADC_CHANNEL_GENERIC1" value="0"/>
<configure name="ADC_CHANNEL_GENERIC2" value="1"/>
conf/airframes/examples/Twinstar_energyadaptive.xml
+2 -2
View File
@@ -44,14 +44,14 @@ twog_1.0 + aspirin + ETS baro + ETS speed
<modules>
<load name="airspeed_ets.xml">
<define name="SENSOR_SYNC_SEND"/>
<define name="AIRSPEED_ETS_SYNC_SEND"/>
<define name="AIRSPEED_ETS_SCALE" value="1.44"/> <!-- default 1.8-->
<!-- <define name="AIRSPEED_ETS_OFFSET" value="50"/> --> <!-- default 0 -->
<!-- <define name="AIRSPEED_ETS_I2C_DEV" value="i2c1"/> -->
</load>
<load name="baro_ets.xml">
<define name="BARO_ETS_SCALE" value="0.3"/>
<define name="BARO_ETS_TELEMETRY"/>
<define name="BARO_ETS_SYNC_SEND"/>
</load>
<load name="baro_board.xml">
<define name="BARO_ABS_EVENT" value="BaroEtsUpdate"/>
conf/airframes/examples/krooz_sd/fixedwing/krooz_sd_fw.xml
+190
View File
@@ -0,0 +1,190 @@
<!DOCTYPE airframe SYSTEM "../../../airframe.dtd">
<airframe name="KroozSD fixedwing example">
<modules>
</modules>
<firmware name="fixedwing">
<target name="ap" board="krooz_sd"/>
<target name="sim" board="pc"/>
<define name="LOITER_TRIM"/>
<define name="ALT_KALMAN"/>
<define name="AGR_CLIMB"/>
<subsystem name="radio_control" type="ppm"/>
<subsystem name="telemetry" type="transparent"/>
<subsystem name="imu" type="krooz_sd"/>
<subsystem name="ahrs" type="float_dcm">
<define name="USE_HIGH_ACCEL_FLAG"/>
<define name="KROOZ_GYRO_RANGE" value="MPU60X0_GYRO_RANGE_500"/>
<configure name="AHRS_PROPAGATE_FREQUENCY" value="60"/>
</subsystem>
<subsystem name="ins" type="alt_float"/>
<subsystem name="control"/>
<subsystem name="gps" type="ublox"/>
<subsystem name="navigation"/>
<subsystem name="actuators" type="pwm"/>
</firmware>
<!-- commands section -->
<servos driver="Pwm">
<servo name="MOTOR" no="4" min="1100" neutral="1100" max="1900"/>
<servo name="ELEVATOR" no="3" min="1200" neutral="1430" max="1800"/>
<servo name="RUDDER" no="1" min="1200" neutral="1619" max="2000"/>
<servo name="AILERON_RIGHT" no="0" max="1800" neutral="1521" min="1200"/>
<servo name="AILERON_LEFT" no="2" max="1800" neutral="1480" min="1200"/>
</servos>
<commands>
<axis name="THROTTLE" failsafe_value="0"/>
<axis name="ROLL" failsafe_value="0"/>
<axis name="PITCH" failsafe_value="0"/>
<axis name="YAW" failsafe_value="0"/>
</commands>
<rc_commands>
<set command="THROTTLE" value="@THROTTLE"/>
<set command="ROLL" value="@ROLL"/>
<set command="PITCH" value="@PITCH"/>
<set command="YAW" value="@YAW"/>
</rc_commands>
<section name="MIXER">
<define name="AILERON_DIFF" value="0.3"/>
</section>
<command_laws>
<set servo="MOTOR" value="@THROTTLE"/>
<set servo="ELEVATOR" value="@PITCH"/>
<let var="roll" value="@ROLL"/>
<set servo="AILERON_LEFT" value="($roll < 0 ? 1 : AILERON_DIFF) * $roll"/>
<set servo="AILERON_RIGHT" value="($roll < 0 ? AILERON_DIFF : 1) * $roll"/>
<set servo="RUDDER" value="@YAW"/>
</command_laws>
<section name="AUTO1" prefix="AUTO1_">
<define name="MAX_ROLL" value="0.85"/>
<define name="MAX_PITCH" value="0.6"/>
</section>
<section name="IMU" prefix="IMU_">
<!-- replace this with your own calibration -->
<define name="GYRO_P_NEUTRAL" value="0"/>
<define name="GYRO_Q_NEUTRAL" value="0"/>
<define name="GYRO_R_NEUTRAL" value="0"/>
<define name="GYRO_P_SENS" value="1.0908" integer="16"/>
<define name="GYRO_Q_SENS" value="1.0908" integer="16"/>
<define name="GYRO_R_SENS" value="1.0908" integer="16"/>
<define name="ACCEL_X_NEUTRAL" value="-48"/>
<define name="ACCEL_Y_NEUTRAL" value="86"/>
<define name="ACCEL_Z_NEUTRAL" value="-479"/>
<define name="ACCEL_X_SENS" value="0.6131" integer="16"/>
<define name="ACCEL_Y_SENS" value="0.6131" integer="16"/>
<define name="ACCEL_Z_SENS" value="0.6131" integer="16"/>
<define name="MAG_X_NEUTRAL" value="-282"/>
<define name="MAG_Y_NEUTRAL" value="-78"/>
<define name="MAG_Z_NEUTRAL" value="109"/>
<define name="MAG_X_SENS" value="3.33430456557" integer="16"/>
<define name="MAG_Y_SENS" value="3.53180739126" integer="16"/>
<define name="MAG_Z_SENS" value="3.68297143457" integer="16"/>
<define name="BODY_TO_IMU_PHI" value="0." unit="deg"/>
<define name="BODY_TO_IMU_THETA" value="0." unit="deg"/>
<define name="BODY_TO_IMU_PSI" value="0." unit="deg"/>
</section>
<section name="INS" prefix="INS_">
<define name="ROLL_NEUTRAL_DEFAULT" value="0." unit="rad"/>
<define name="PITCH_NEUTRAL_DEFAULT" value="0." unit="rad"/>
</section>
<section name="BAT">
<define name="CATASTROPHIC_BAT_LEVEL" value="9.3" unit="V"/>
<define name="MilliAmpereOfAdc(_adc)" value="(_adc-156)*18.2"/>
</section>
<section name="MISC">
<define name="MINIMUM_AIRSPEED" value="10." unit="m/s"/>
<define name="NOMINAL_AIRSPEED" value="12." unit="m/s"/>
<define name="MAXIMUM_AIRSPEED" value="19." unit="m/s"/>
<define name="CARROT" value="5." unit="s"/>
<define name="KILL_MODE_DISTANCE" value="(1.5*MAX_DIST_FROM_HOME)"/>
<define name="ALT_KALMAN_ENABLED" value="TRUE"/>
<define name="DEFAULT_CIRCLE_RADIUS" value="80."/>
<define name="UNLOCKED_HOME_MODE" value="TRUE"/>
</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.06"/>
<!-- outer loop saturation -->
<define name="ALTITUDE_MAX_CLIMB" value="3."/>
<!-- auto throttle inner loop -->
<define name="AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE" value="0.5"/>
<define name="AUTO_THROTTLE_MIN_CRUISE_THROTTLE" value="0.25"/>
<define name="AUTO_THROTTLE_MAX_CRUISE_THROTTLE" value="0.85"/>
<define name="AUTO_THROTTLE_LOITER_TRIM" value="1000"/>
<define name="AUTO_THROTTLE_DASH_TRIM" value="-1200"/>
<define name="AUTO_THROTTLE_CLIMB_THROTTLE_INCREMENT" value="0.116999998689" unit="%/(m/s)"/>
<define name="AUTO_THROTTLE_PGAIN" value="0.0109999999404"/>
<define name="AUTO_THROTTLE_IGAIN" value="0.119000002742"/>
<define name="AUTO_THROTTLE_PITCH_OF_VZ_PGAIN" value="0.082999996841"/>
<!-- auto pitch inner loop -->
<!--define name="AUTO_PITCH_PGAIN" value="-0.06"/>
<define name="AUTO_PITCH_IGAIN" value="0.0"/>
<define name="AUTO_PITCH_MAX_PITCH" value="0.35"/>
<define name="AUTO_PITCH_MIN_PITCH" value="-0.35"/-->
<define name="THROTTLE_SLEW" value="0.1"/>
</section>
<section name="HORIZONTAL CONTROL" prefix="H_CTL_">
<define name="COURSE_PGAIN" value="0.878000020981"/>
<define name="ROLL_MAX_SETPOINT" value="0.60" unit="radians"/>
<define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
<define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
<define name="ROLL_ATTITUDE_GAIN" value="11359.2226562"/>
<define name="ROLL_RATE_GAIN" value="2000."/>
<define name="PITCH_PGAIN" value="9587.37890625"/>
<define name="PITCH_DGAIN" value="0.4"/>
<define name="AILERON_OF_THROTTLE" value="0.0"/>
<define name="ELEVATOR_OF_ROLL" value="1500"/>
</section>
<section name="NAV">
<define name="NAV_GLIDE_PITCH_TRIM" value="0"/>
</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="15"/><!-- Altitude Error to Blend Aggressive to Regular Climb Modes CANNOT BE ZERO!!-->
<define name="CLIMB_THROTTLE" value="0.9"/><!-- Gaz for Aggressive Climb -->
<define name="CLIMB_PITCH" value="0.35"/><!-- Pitch for Aggressive Climb -->
<define name="DESCENT_THROTTLE" value="0.05"/><!-- Gaz for Aggressive Decent -->
<define name="DESCENT_PITCH" value="-0.35"/><!-- 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.17" unit="rad"/>
<define name="DEFAULT_PITCH" value="0.08" unit="rad"/>
<define name="HOME_RADIUS" value="100" unit="m"/>
</section>
</airframe>
conf/airframes/examples/krooz_sd/krooz_sd_hexa_mkk.xml
Executable → Regular
+17 -17
View File
@@ -17,7 +17,7 @@
<define name="I2C_TRANSACTION_QUEUE_LEN" value="10"/>
<define name="I2C1_CLOCK_SPEED" value="42000"/>
</subsystem>
<subsystem name="stabilization" type="euler"/>
<subsystem name="stabilization" type="int_euler"/>
<subsystem name="gps" type="ublox"/>
<subsystem name="imu" type="krooz_sd"/>
<subsystem name="ahrs" type="int_cmpl_quat"/>
@@ -91,23 +91,23 @@
<define name="GYRO_P_NEUTRAL" value="0"/>
<define name="GYRO_Q_NEUTRAL" value="0"/>
<define name="GYRO_R_NEUTRAL" value="0"/>
<define name="GYRO_P_SENS" value=" 4.41" integer="16"/>
<define name="GYRO_Q_SENS" value=" 4.41" integer="16"/>
<define name="GYRO_R_SENS" value=" 4.41" integer="16"/>
<define name="GYRO_P_SENS" value="0.5454" integer="16"/>
<define name="GYRO_Q_SENS" value="0.5454" integer="16"/>
<define name="GYRO_R_SENS" value="0.5454" integer="16"/>
<define name="ACCEL_X_NEUTRAL" value="-24"/>
<define name="ACCEL_Y_NEUTRAL" value="122"/>
<define name="ACCEL_Z_NEUTRAL" value="-130"/>
<define name="ACCEL_X_SENS" value="4.90555515454" integer="16"/>
<define name="ACCEL_Y_SENS" value="4.90893349017" integer="16"/>
<define name="ACCEL_Z_SENS" value="4.75238978393" integer="16"/>
<define name="ACCEL_X_NEUTRAL" value="-48"/>
<define name="ACCEL_Y_NEUTRAL" value="86"/>
<define name="ACCEL_Z_NEUTRAL" value="-479"/>
<define name="ACCEL_X_SENS" value="0.6131" integer="16"/>
<define name="ACCEL_Y_SENS" value="0.6131" integer="16"/>
<define name="ACCEL_Z_SENS" value="0.6131" integer="16"/>
<define name="MAG_X_NEUTRAL" value="-136"/>
<define name="MAG_Y_NEUTRAL" value="-103"/>
<define name="MAG_Z_NEUTRAL" value="6"/>
<define name="MAG_X_SENS" value="3.18492472198" integer="16"/>
<define name="MAG_Y_SENS" value="3.42471474617" integer="16"/>
<define name="MAG_Z_SENS" value="3.42088446936" integer="16"/>
<define name="MAG_X_NEUTRAL" value="-282"/>
<define name="MAG_Y_NEUTRAL" value="-78"/>
<define name="MAG_Z_NEUTRAL" value="109"/>
<define name="MAG_X_SENS" value="3.33430456557" integer="16"/>
<define name="MAG_Y_SENS" value="3.53180739126" integer="16"/>
<define name="MAG_Z_SENS" value="3.68297143457" integer="16"/>
<define name="BODY_TO_IMU_PHI" value="0." unit="deg"/>
<define name="BODY_TO_IMU_THETA" value="0." unit="deg"/>
@@ -123,7 +123,7 @@
</section>
<section name="INS" prefix="INS_">
<define name="BARO_SENS" value="20." integer="16"/>
<define name="BARO_SENS" value="22." integer="16"/>
</section>
<section name="STABILIZATION_RATE" prefix="STABILIZATION_RATE_">
conf/airframes/examples/krooz_sd/krooz_sd_okto_mkk.xml
Executable → Regular
+17 -17
View File
@@ -17,7 +17,7 @@
<define name="I2C_TRANSACTION_QUEUE_LEN" value="10"/>
<define name="I2C1_CLOCK_SPEED" value="42000"/>
</subsystem>
<subsystem name="stabilization" type="euler"/>
<subsystem name="stabilization" type="int_euler"/>
<subsystem name="gps" type="ublox"/>
<subsystem name="imu" type="krooz_sd"/>
<subsystem name="ahrs" type="int_cmpl_quat"/>
@@ -94,23 +94,23 @@
<define name="GYRO_P_NEUTRAL" value="0"/>
<define name="GYRO_Q_NEUTRAL" value="0"/>
<define name="GYRO_R_NEUTRAL" value="0"/>
<define name="GYRO_P_SENS" value=" 4.41" integer="16"/>
<define name="GYRO_Q_SENS" value=" 4.41" integer="16"/>
<define name="GYRO_R_SENS" value=" 4.41" integer="16"/>
<define name="GYRO_P_SENS" value="0.5454" integer="16"/>
<define name="GYRO_Q_SENS" value="0.5454" integer="16"/>
<define name="GYRO_R_SENS" value="0.5454" integer="16"/>
<define name="ACCEL_X_NEUTRAL" value="-15"/>
<define name="ACCEL_Y_NEUTRAL" value="75"/>
<define name="ACCEL_Z_NEUTRAL" value="-110"/>
<define name="ACCEL_X_SENS" value="4.91713002301" integer="16"/>
<define name="ACCEL_Y_SENS" value="4.90474978531" integer="16"/>
<define name="ACCEL_Z_SENS" value="4.69090792281" integer="16"/>
<define name="ACCEL_X_NEUTRAL" value="-48"/>
<define name="ACCEL_Y_NEUTRAL" value="86"/>
<define name="ACCEL_Z_NEUTRAL" value="-479"/>
<define name="ACCEL_X_SENS" value="0.6131" integer="16"/>
<define name="ACCEL_Y_SENS" value="0.6131" integer="16"/>
<define name="ACCEL_Z_SENS" value="0.6131" integer="16"/>
<define name="MAG_X_NEUTRAL" value="-20"/>
<define name="MAG_Y_NEUTRAL" value="5"/>
<define name="MAG_Z_NEUTRAL" value="67"/>
<define name="MAG_X_SENS" value="3.66584881345" integer="16"/>
<define name="MAG_Y_SENS" value="3.71365620466" integer="16"/>
<define name="MAG_Z_SENS" value="3.93483279876" integer="16"/>
<define name="MAG_X_NEUTRAL" value="-282"/>
<define name="MAG_Y_NEUTRAL" value="-78"/>
<define name="MAG_Z_NEUTRAL" value="109"/>
<define name="MAG_X_SENS" value="3.33430456557" integer="16"/>
<define name="MAG_Y_SENS" value="3.53180739126" integer="16"/>
<define name="MAG_Z_SENS" value="3.68297143457" integer="16"/>
<define name="BODY_TO_IMU_PHI" value="0." unit="deg"/>
<define name="BODY_TO_IMU_THETA" value="0." unit="deg"/>
@@ -126,7 +126,7 @@
</section>
<section name="INS" prefix="INS_">
<define name="BARO_SENS" value="20." integer="16"/>
<define name="BARO_SENS" value="22." integer="16"/>
</section>
<section name="STABILIZATION_RATE" prefix="STABILIZATION_RATE_">
conf/airframes/examples/krooz_sd/krooz_sd_quad_mkk.xml
Executable → Regular
+8 -8
View File
@@ -15,7 +15,7 @@
<subsystem name="telemetry" type="transparent"/>
<subsystem name="imu" type="krooz_sd"/>
<subsystem name="gps" type="ublox"/>
<subsystem name="stabilization" type="euler"/>
<subsystem name="stabilization" type="int_euler"/>
<subsystem name="ahrs" type="int_cmpl_quat"/>
<subsystem name="ins" type="hff"/>
<subsystem name="actuators" type="mkk">
@@ -84,16 +84,16 @@
<define name="GYRO_P_NEUTRAL" value="0"/>
<define name="GYRO_Q_NEUTRAL" value="0"/>
<define name="GYRO_R_NEUTRAL" value="0"/>
<define name="GYRO_P_SENS" value=" 4.41" integer="16"/>
<define name="GYRO_Q_SENS" value=" 4.41" integer="16"/>
<define name="GYRO_R_SENS" value=" 4.41" integer="16"/>
<define name="GYRO_P_SENS" value="0.5454" integer="16"/>
<define name="GYRO_Q_SENS" value="0.5454" integer="16"/>
<define name="GYRO_R_SENS" value="0.5454" integer="16"/>
<define name="ACCEL_X_NEUTRAL" value="-48"/>
<define name="ACCEL_Y_NEUTRAL" value="86"/>
<define name="ACCEL_Z_NEUTRAL" value="-479"/>
<define name="ACCEL_X_SENS" value="4.86549375955" integer="16"/>
<define name="ACCEL_Y_SENS" value="4.89290708897" integer="16"/>
<define name="ACCEL_Z_SENS" value="4.83749812474" integer="16"/>
<define name="ACCEL_X_SENS" value="0.6131" integer="16"/>
<define name="ACCEL_Y_SENS" value="0.6131" integer="16"/>
<define name="ACCEL_Z_SENS" value="0.6131" integer="16"/>
<define name="MAG_X_NEUTRAL" value="-282"/>
<define name="MAG_Y_NEUTRAL" value="-78"/>
@@ -116,7 +116,7 @@
</section>
<section name="INS" prefix="INS_">
<define name="BARO_SENS" value="20." integer="16"/>
<define name="BARO_SENS" value="22." integer="16"/>
</section>
<section name="STABILIZATION_RATE" prefix="STABILIZATION_RATE_">
conf/airframes/examples/krooz_sd/krooz_sd_quad_pwm.xml
Executable → Regular
+18 -18
View File
@@ -19,7 +19,7 @@
<subsystem name="telemetry" type="transparent"/>
<subsystem name="imu" type="krooz_sd"/>
<subsystem name="gps" type="ublox"/>
<subsystem name="stabilization" type="euler"/>
<subsystem name="stabilization" type="int_euler"/>
<subsystem name="ahrs" type="int_cmpl_quat"/>
<subsystem name="ins" type="hff"/>
<subsystem name="motor_mixing"/>
@@ -79,27 +79,27 @@
<define name="GYRO_P_NEUTRAL" value="0"/>
<define name="GYRO_Q_NEUTRAL" value="0"/>
<define name="GYRO_R_NEUTRAL" value="0"/>
<define name="GYRO_P_SENS" value=" 4.41" integer="16"/>
<define name="GYRO_Q_SENS" value=" 4.41" integer="16"/>
<define name="GYRO_R_SENS" value=" 4.41" integer="16"/>
<define name="GYRO_P_SENS" value="0.5454" integer="16"/>
<define name="GYRO_Q_SENS" value="0.5454" integer="16"/>
<define name="GYRO_R_SENS" value="0.5454" integer="16"/>
<define name="ACCEL_X_NEUTRAL" value="-17"/>
<define name="ACCEL_Y_NEUTRAL" value="137"/>
<define name="ACCEL_Z_NEUTRAL" value="-112"/>
<define name="ACCEL_X_SENS" value="4.90163520863" integer="16"/>
<define name="ACCEL_Y_SENS" value="4.92641947248" integer="16"/>
<define name="ACCEL_Z_SENS" value="4.75672568963" integer="16"/>
<define name="ACCEL_X_NEUTRAL" value="-48"/>
<define name="ACCEL_Y_NEUTRAL" value="86"/>
<define name="ACCEL_Z_NEUTRAL" value="-479"/>
<define name="ACCEL_X_SENS" value="0.6131" integer="16"/>
<define name="ACCEL_Y_SENS" value="0.6131" integer="16"/>
<define name="ACCEL_Z_SENS" value="0.6131" integer="16"/>
<define name="MAG_X_NEUTRAL" value="-183"/>
<define name="MAG_Y_NEUTRAL" value="-106"/>
<define name="MAG_Z_NEUTRAL" value="-75"/>
<define name="MAG_X_SENS" value="3.22557142458" integer="16"/>
<define name="MAG_Y_SENS" value="3.52708642214" integer="16"/>
<define name="MAG_Z_SENS" value="3.54627027156" integer="16"/>
<define name="MAG_X_NEUTRAL" value="-282"/>
<define name="MAG_Y_NEUTRAL" value="-78"/>
<define name="MAG_Z_NEUTRAL" value="109"/>
<define name="MAG_X_SENS" value="3.33430456557" integer="16"/>
<define name="MAG_Y_SENS" value="3.53180739126" integer="16"/>
<define name="MAG_Z_SENS" value="3.68297143457" integer="16"/>
<define name="BODY_TO_IMU_PHI" value="0." unit="deg"/>
<define name="BODY_TO_IMU_THETA" value="0." unit="deg"/>
<define name="BODY_TO_IMU_PSI" value="45." unit="deg"/>
<define name="BODY_TO_IMU_PSI" value="0." unit="deg"/>
</section>
<section name="AHRS" prefix="AHRS_">
@@ -111,7 +111,7 @@
</section>
<section name="INS" prefix="INS_">
<define name="BARO_SENS" value="20." integer="16"/>
<define name="BARO_SENS" value="22." integer="16"/>
</section>
<section name="STABILIZATION_RATE" prefix="STABILIZATION_RATE_">
conf/airframes/examples/quadshot_asp21_spektrum.xml
+1 -1
View File
@@ -246,7 +246,7 @@ More information on the Quadshot can be found at transition-robotics.com -->
<!--Use an airspeed sensor and get the measured airspeed in the messages-->
<load name="airspeed_ets.xml">
<define name="SENSOR_SYNC_SEND"/>
<define name="AIRSPEED_ETS_SYNC_SEND"/>
</load>
<!-- Load this module to use multiple gain sets, which have to be specified in the gain sets section -->
conf/airframes/flixr_discovery.xml
+1 -1
View File
@@ -67,7 +67,7 @@
<configure name="ADC_CHANNEL_GENERIC1" value="ADC_4"/> <!-- voltage sensor -->
</load>
<!--load name="airspeed_ets.xml">
<define name="SENSOR_SYNC_SEND"/>
<define name="AIRSPEED_ETS_SYNC_SEND"/>
<define name="USE_AIRSPEED"/>
</load-->
</modules>
conf/airframes/obsolete/mm/fixed-wing/slowfast2.xml
+1 -1
View File
@@ -36,7 +36,7 @@
<modules>
<load name="airspeed_ets.xml">
<!--define name="USE_AIRSPEED" value="1"/-->
<define name="SENSOR_SYNC_SEND" value="1"/>
<define name="AIRSPEED_ETS_SYNC_SEND" value="1"/>
</load>
<load name="infrared_adc.xml"/>
</modules>
conf/firmwares/rotorcraft.makefile
+4
View File
@@ -201,6 +201,10 @@ ap.CFLAGS += -DUSE_SPI1
ap.srcs += peripherals/mcp355x.c
ap.srcs += $(SRC_BOARD)/baro_board.c
# krooz baro
else ifeq ($(BOARD), krooz)
ap.srcs += $(SRC_BOARD)/baro_board.c
# apogee baro
else ifeq ($(BOARD), apogee)
ap.CFLAGS += -DUSE_I2C1
conf/firmwares/subsystems/shared/imu_aspirin_v2.1_old.makefile
-73
View File
@@ -1,73 +0,0 @@
# Hey Emacs, this is a -*- makefile -*-
#
# Aspirin IMU v2.1
#
#
# required xml:
# <section name="IMU" prefix="IMU_">
#
# <!-- these gyro and accel calib values are the defaults for aspirin2.1 -->
# <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_CFLAGS = -DUSE_IMU
endif
IMU_ASPIRIN_CFLAGS += -DIMU_TYPE_H=\"imu/imu_aspirin2.h\"
IMU_ASPIRIN_SRCS = $(SRC_SUBSYSTEMS)/imu.c \
$(SRC_SUBSYSTEMS)/imu/imu_aspirin2.c
include $(CFG_SHARED)/spi_master.makefile
ifeq ($(ARCH), lpc21)
IMU_ASPIRIN_CFLAGS += -DUSE_SPI1
IMU_ASPIRIN_CFLAGS += -DUSE_SPI_SLAVE0
else ifeq ($(ARCH), stm32)
IMU_ASPIRIN_CFLAGS += -DUSE_SPI2
# Slave select configuration
# SLAVE2 is on PB12 (NSS) (MPU600 CS)
IMU_ASPIRIN_CFLAGS += -DUSE_SPI_SLAVE2
endif
IMU_ASPIRIN_CFLAGS += -DIMU_ASPIRIN_VERSION_2_1
# 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
ap.CFLAGS += $(IMU_ASPIRIN_CFLAGS)
ap.srcs += $(IMU_ASPIRIN_SRCS)
#
# NPS simulator
#
include $(CFG_SHARED)/imu_nps.makefile
conf/firmwares/subsystems/shared/imu_krooz_sd.makefile
+29
View File
@@ -0,0 +1,29 @@
# Hey Emacs, this is a -*- makefile -*-
#
# KroozSD IMU
#
IMU_KROOZ_CFLAGS = -DUSE_IMU
IMU_KROOZ_CFLAGS += -DIMU_TYPE_H=\"boards/krooz/imu_krooz.h\"
IMU_KROOZ_SRCS = $(SRC_SUBSYSTEMS)/imu.c \
$(SRC_BOARD)/imu_krooz.c \
$(SRC_ARCH)/subsystems/imu/imu_krooz_sd_arch.c
IMU_KROOZ_I2C_DEV=i2c2
IMU_KROOZ_CFLAGS += -DUSE_I2C -DUSE_I2C2 -DI2C2_CLOCK_SPEED=400000
IMU_KROOZ_CFLAGS += -DIMU_KROOZ_I2C_DEV=$(IMU_KROOZ_I2C_DEV)
IMU_KROOZ_CFLAGS += -DMS5611_I2C_DEV=$(IMU_KROOZ_I2C_DEV) -DMS5611_SLAVE_ADDR=0xEC
IMU_KROOZ_SRCS += peripherals/mpu60x0.c
IMU_KROOZ_SRCS += peripherals/mpu60x0_i2c.c
IMU_KROOZ_SRCS += peripherals/hmc58xx.c
IMU_KROOZ_SRCS += modules/sensors/baro_ms5611_i2c.c
AHRS_PROPAGATE_FREQUENCY ?= 512
AHRS_CORRECT_FREQUENCY ?= 512
ap.CFLAGS += -DAHRS_PROPAGATE_FREQUENCY=$(AHRS_PROPAGATE_FREQUENCY)
ap.CFLAGS += -DAHRS_CORRECT_FREQUENCY=$(AHRS_CORRECT_FREQUENCY)
ap.CFLAGS += $(IMU_KROOZ_CFLAGS)
ap.srcs += $(IMU_KROOZ_SRCS)
conf/modules/airspeed_ets.xml
+5 -4
View File
@@ -17,11 +17,12 @@
- Yellow wire: SDA
- Brown wire: SCL
</description>
<define name="AIRSPEED_ETS_I2C_DEV" value="i2c0" description="change default i2c peripheral"/>
<define name="AIRSPEED_ETS_OFFSET" value="0" description="sensor offset (default: 0)"/>
<define name="AIRSPEED_ETS_SCALE" value="1.8" description="sensor scale factor (default: 1.8)"/>
<define name="AIRSPEED_ETS_I2C_DEV" value="i2cX" description="set i2c peripheral (default: i2c0)"/>
<define name="AIRSPEED_ETS_OFFSET" value="offset" description="sensor reading offset (default: 0)"/>
<define name="AIRSPEED_ETS_SCALE" value="scale" description="sensor scale factor (default: 1.8)"/>
<define name="AIRSPEED_ETS_START_DELAY" value="delay" description="set initial start delay in seconds"/>
<define name="USE_AIRSPEED" description="flag to use the data for airspeed control"/>
<define name="SENSOR_SYNC_SEND" description="flag to transmit the data as it is acquired"/>
<define name="AIRSPEED_ETS_SYNC_SEND" description="flag to transmit the data as it is acquired"/>
</doc>
<header>
conf/modules/baro_amsys.xml
+4
View File
@@ -7,6 +7,10 @@
Module to read a Amsys AMS 5812-0150-A barometric sensor via I2C.
</description>
<define name="BARO_AMSYS_I2C_DEV" value="i2cX" description="select which i2c peripheral to use (default i2c0)"/>
<define name="BARO_AMSYS_MAX_PRESSURE" value="103400" description="max sensor pressure (Pa) (default: 103400 for -0150)"/>
<define name="BARO_AMSYS_SCALE" value="1" description="sensor scale factor (default: 1)"/>
<define name="BARO_AMSYS_FILTER" value="0." description="sensor filter (default: 0. max:1)"/>
<define name="BARO_AMSYS_SYNC_SEND" description="flag to transmit the data as it is acquired"/>
</doc>
<header>
conf/modules/baro_ets.xml
+20 -4
View File
@@ -2,10 +2,26 @@
<module name="baro_ets" dir="sensors">
<doc>
<description>Baro ETS (I2C)</description>
<define name="BARO_ETS_I2C_DEV" value="i2cX" description="select i2c peripheral to use (default i2c0)"/>
<define name="BARO_ETS_SCALE" value="sensor scale factor"/>
<define name="BARO_ETS_TELEMETRY" description="flag to transmit the data as it is acquired"/>
<description>
Baro ETS (I2C).
Driver for the EagleTree Systems Baro Sensor.
Has only been tested with V3 of the sensor hardware.
Notes:
Connect directly to TWOG/Tiny I2C port. Multiple sensors can be chained together.
Sensor should be in the proprietary mode (default) and not in 3rd party mode.
Sensor module wire assignments:
- Red wire: 5V
- White wire: Ground
- Yellow wire: SDA
- Brown wire: SCL
</description>
<define name="BARO_ETS_I2C_DEV" value="i2cX" description="set i2c peripheral (default: i2c0)"/>
<define name="BARO_ETS_SCALE" value="scale" description="sensor scale factor (default: 1.8)"/>
<define name="BARO_ETS_START_DELAY" value="delay" description="set initial start delay in seconds"/>
<define name="BARO_ETS_SYNC_SEND" description="flag to transmit the data as it is acquired"/>
</doc>
<header>
sw/airborne/arch/stm32/mcu_periph/spi_arch.c
+192 -6
View File
@@ -84,15 +84,17 @@ struct spi_periph_dma {
uint32_t spi; ///< SPI peripheral identifier
uint32_t spidr; ///< SPI DataRegister address for DMA
uint32_t dma; ///< DMA controller base address (DMA1 or DMA2)
uint8_t rx_chan; ///< receive DMA channel number
uint8_t tx_chan; ///< transmit DMA channel number
uint8_t rx_chan; ///< receive DMA channel (or stream on F4) number
uint8_t tx_chan; ///< transmit DMA channel (or stream on F4) number
uint32_t rx_chan_sel; ///< F4 only: actual receive DMA channel number
uint32_t tx_chan_sel; ///< F4 only: actual transmit DMA channel number
uint8_t rx_nvic_irq; ///< receive interrupt
uint8_t tx_nvic_irq; ///< transmit interrupt
uint16_t tx_dummy_buf; ///< dummy tx buffer for receive only cases
bool_t tx_extra_dummy_dma; ///< extra tx dummy dma flag for tx_len < rx_len
bool_t tx_extra_dummy_dma; ///< extra tx dummy dma flag for tx_len < rx_len
uint16_t rx_dummy_buf; ///< dummy rx buffer for receive only cases
bool_t rx_extra_dummy_dma; ///< extra rx dummy dma flag for tx_len > rx_len
struct locm3_spi_comm comm; ///< current communication paramters
bool_t rx_extra_dummy_dma; ///< extra rx dummy dma flag for tx_len > rx_len
struct locm3_spi_comm comm; ///< current communication paramters
uint8_t comm_sig; ///< comm config signature used to check for changes
};
@@ -439,12 +441,17 @@ static void set_comm_from_transaction(struct locm3_spi_comm* c, struct spi_trans
static void spi_configure_dma(uint32_t dma, uint8_t chan, uint32_t periph_addr, uint32_t buf_addr,
uint16_t len, enum SPIDataSizeSelect dss, bool_t increment)
{
#ifdef STM32F1
dma_channel_reset(dma, chan);
#elif defined STM32F4
dma_stream_reset(dma, chan);
#endif
dma_set_peripheral_address(dma, chan, periph_addr);
dma_set_memory_address(dma, chan, buf_addr);
dma_set_number_of_data(dma, chan, len);
/* Set the dma transfer size based on SPI transaction DSS */
#ifdef STM32F1
if (dss == SPIDss8bit) {
dma_set_peripheral_size(dma, chan, DMA_CCR_PSIZE_8BIT);
dma_set_memory_size(dma, chan, DMA_CCR_MSIZE_8BIT);
@@ -452,6 +459,15 @@ static void spi_configure_dma(uint32_t dma, uint8_t chan, uint32_t periph_addr,
dma_set_peripheral_size(dma, chan, DMA_CCR_PSIZE_16BIT);
dma_set_memory_size(dma, chan, DMA_CCR_MSIZE_16BIT);
}
#elif defined STM32F4
if (dss == SPIDss8bit) {
dma_set_peripheral_size(dma, chan, DMA_SxCR_PSIZE_8BIT);
dma_set_memory_size(dma, chan, DMA_SxCR_MSIZE_8BIT);
} else {
dma_set_peripheral_size(dma, chan, DMA_SxCR_PSIZE_16BIT);
dma_set_memory_size(dma, chan, DMA_SxCR_MSIZE_16BIT);
}
#endif
if (increment)
dma_enable_memory_increment_mode(dma, chan);
@@ -566,8 +582,14 @@ static void spi_start_dma_transaction(struct spi_periph* periph, struct spi_tran
dma->rx_extra_dummy_dma = TRUE;
}
}
#ifdef STM32F1
dma_set_read_from_peripheral(dma->dma, dma->rx_chan);
dma_set_priority(dma->dma, dma->rx_chan, DMA_CCR_PL_VERY_HIGH);
#elif defined STM32F4
dma_channel_select(dma->dma, dma->rx_chan, dma->rx_chan_sel);
dma_set_transfer_mode(dma->dma, dma->rx_chan, DMA_SxCR_DIR_PERIPHERAL_TO_MEM);
dma_set_priority(dma->dma, dma->rx_chan, DMA_SxCR_PL_VERY_HIGH);
#endif
/*
@@ -591,17 +613,27 @@ static void spi_start_dma_transaction(struct spi_periph* periph, struct spi_tran
dma->tx_extra_dummy_dma = TRUE;
}
}
#ifdef STM32F1
dma_set_read_from_memory(dma->dma, dma->tx_chan);
dma_set_priority(dma->dma, dma->tx_chan, DMA_CCR_PL_MEDIUM);
#elif defined STM32F4
dma_channel_select(dma->dma, dma->tx_chan, dma->tx_chan_sel);
dma_set_transfer_mode(dma->dma, dma->tx_chan, DMA_SxCR_DIR_MEM_TO_PERIPHERAL);
dma_set_priority(dma->dma, dma->tx_chan, DMA_SxCR_PL_MEDIUM);
#endif
/* Enable DMA transfer complete interrupts. */
dma_enable_transfer_complete_interrupt(dma->dma, dma->rx_chan);
dma_enable_transfer_complete_interrupt(dma->dma, dma->tx_chan);
/* Enable DMA channels */
#ifdef STM32F1
dma_enable_channel(dma->dma, dma->rx_chan);
dma_enable_channel(dma->dma, dma->tx_chan);
#elif defined STM32F4
dma_enable_stream(dma->dma, dma->rx_chan);
dma_enable_stream(dma->dma, dma->tx_chan);
#endif
/* Enable SPI transfers via DMA */
spi_enable_rx_dma((uint32_t)periph->reg_addr);
@@ -620,11 +652,22 @@ void spi1_arch_init(void) {
// set dma options
spi1_dma.spidr = (uint32_t)&SPI1_DR;
#ifdef STM32F1
spi1_dma.dma = DMA1;
spi1_dma.rx_chan = DMA_CHANNEL2;
spi1_dma.tx_chan = DMA_CHANNEL3;
spi1_dma.rx_nvic_irq = NVIC_DMA1_CHANNEL2_IRQ;
spi1_dma.tx_nvic_irq = NVIC_DMA1_CHANNEL3_IRQ;
#elif defined STM32F4
spi1_dma.dma = DMA2;
// TODO make a macro to configure this from board/airframe file ?
spi1_dma.rx_chan = DMA_STREAM0;
spi1_dma.tx_chan = DMA_STREAM3;
spi1_dma.rx_chan_sel = DMA_SxCR_CHSEL_3;
spi1_dma.tx_chan_sel = DMA_SxCR_CHSEL_3;
spi1_dma.rx_nvic_irq = NVIC_DMA2_STREAM0_IRQ;
spi1_dma.tx_nvic_irq = NVIC_DMA2_STREAM3_IRQ;
#endif
spi1_dma.tx_dummy_buf = 0;
spi1_dma.tx_extra_dummy_dma = FALSE;
spi1_dma.rx_dummy_buf = 0;
@@ -646,12 +689,22 @@ void spi1_arch_init(void) {
rcc_peripheral_enable_clock(&RCC_APB2ENR, RCC_APB2ENR_SPI1EN);
// Configure GPIOs: SCK, MISO and MOSI
#ifdef STM32F1
// TODO configure lisa board files to use gpio_setup_pin_af function
gpio_set_mode(GPIO_BANK_SPI1_SCK, GPIO_MODE_OUTPUT_50_MHZ,
GPIO_CNF_OUTPUT_ALTFN_PUSHPULL,
GPIO_SPI1_SCK | GPIO_SPI1_MOSI);
gpio_set_mode(GPIO_BANK_SPI1_MISO, GPIO_MODE_INPUT, GPIO_CNF_INPUT_FLOAT,
GPIO_SPI1_MISO);
#elif defined STM32F4
gpio_setup_pin_af(SPI1_GPIO_PORT_MISO, SPI1_GPIO_MISO, SPI1_GPIO_AF, FALSE);
gpio_setup_pin_af(SPI1_GPIO_PORT_MOSI, SPI1_GPIO_MOSI, SPI1_GPIO_AF, TRUE);
gpio_setup_pin_af(SPI1_GPIO_PORT_SCK, SPI1_GPIO_SCK, SPI1_GPIO_AF, TRUE);
gpio_set_output_options(SPI1_GPIO_PORT_MOSI, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ, SPI1_GPIO_MOSI);
gpio_set_output_options(SPI1_GPIO_PORT_SCK, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ, SPI1_GPIO_SCK);
#endif
// reset SPI
spi_reset(SPI1);
@@ -677,7 +730,11 @@ void spi1_arch_init(void) {
spi_set_nss_high(SPI1);
// Enable SPI_1 DMA clock
#ifdef STM32F1
rcc_peripheral_enable_clock(&RCC_AHBENR, RCC_AHBENR_DMA1EN);
#elif defined STM32F4
rcc_peripheral_enable_clock(&RCC_AHB1ENR, RCC_AHB1ENR_DMA2EN);
#endif
// Enable SPI1 periph.
spi_enable(SPI1);
@@ -692,10 +749,19 @@ void spi2_arch_init(void) {
// set dma options
spi2_dma.spidr = (uint32_t)&SPI2_DR;
spi2_dma.dma = DMA1;
#ifdef STM32F1
spi2_dma.rx_chan = DMA_CHANNEL4;
spi2_dma.tx_chan = DMA_CHANNEL5;
spi2_dma.rx_nvic_irq = NVIC_DMA1_CHANNEL4_IRQ;
spi2_dma.tx_nvic_irq = NVIC_DMA1_CHANNEL5_IRQ;
#elif defined STM32F4
spi2_dma.rx_chan = DMA_STREAM3;
spi2_dma.tx_chan = DMA_STREAM4;
spi2_dma.rx_chan_sel = DMA_SxCR_CHSEL_0;
spi2_dma.tx_chan_sel = DMA_SxCR_CHSEL_0;
spi2_dma.rx_nvic_irq = NVIC_DMA1_STREAM3_IRQ;
spi2_dma.tx_nvic_irq = NVIC_DMA1_STREAM4_IRQ;
#endif
spi2_dma.tx_dummy_buf = 0;
spi2_dma.tx_extra_dummy_dma = FALSE;
spi2_dma.rx_dummy_buf = 0;
@@ -717,12 +783,22 @@ void spi2_arch_init(void) {
rcc_peripheral_enable_clock(&RCC_APB1ENR, RCC_APB1ENR_SPI2EN);
// Configure GPIOs: SCK, MISO and MOSI
#ifdef STM32F1
// TODO configure lisa board files to use gpio_setup_pin_af function
gpio_set_mode(GPIO_BANK_SPI2_SCK, GPIO_MODE_OUTPUT_50_MHZ,
GPIO_CNF_OUTPUT_ALTFN_PUSHPULL,
GPIO_SPI2_SCK | GPIO_SPI2_MOSI);
gpio_set_mode(GPIO_BANK_SPI2_MISO, GPIO_MODE_INPUT, GPIO_CNF_INPUT_FLOAT,
GPIO_SPI2_MISO);
#elif defined STM32F4
gpio_setup_pin_af(SPI2_GPIO_PORT_MISO, SPI2_GPIO_MISO, SPI2_GPIO_AF, FALSE);
gpio_setup_pin_af(SPI2_GPIO_PORT_MOSI, SPI2_GPIO_MOSI, SPI2_GPIO_AF, TRUE);
gpio_setup_pin_af(SPI2_GPIO_PORT_SCK, SPI2_GPIO_SCK, SPI2_GPIO_AF, TRUE);
gpio_set_output_options(SPI2_GPIO_PORT_MOSI, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ, SPI2_GPIO_MOSI);
gpio_set_output_options(SPI2_GPIO_PORT_SCK, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ, SPI2_GPIO_SCK);
#endif
// reset SPI
spi_reset(SPI2);
@@ -747,7 +823,11 @@ void spi2_arch_init(void) {
spi_set_nss_high(SPI2);
// Enable SPI_2 DMA clock
#ifdef STM32F1
rcc_peripheral_enable_clock(&RCC_AHBENR, RCC_AHBENR_DMA1EN);
#elif defined STM32F4
rcc_peripheral_enable_clock(&RCC_AHB1ENR, RCC_AHB1ENR_DMA1EN);
#endif
// Enable SPI2 periph.
spi_enable(SPI2);
@@ -761,11 +841,21 @@ void spi3_arch_init(void) {
// set the default configuration
spi3_dma.spidr = (uint32_t)&SPI3_DR;
#ifdef STM32F1
spi3_dma.dma = DMA2;
spi3_dma.rx_chan = DMA_CHANNEL1;
spi3_dma.tx_chan = DMA_CHANNEL2;
spi3_dma.rx_nvic_irq = NVIC_DMA2_CHANNEL1_IRQ;
spi3_dma.tx_nvic_irq = NVIC_DMA2_CHANNEL2_IRQ;
#elif defined STM32F4
spi3_dma.dma = DMA1;
spi3_dma.rx_chan = DMA_STREAM0;
spi3_dma.tx_chan = DMA_STREAM5;
spi3_dma.rx_chan_sel = DMA_SxCR_CHSEL_0;
spi3_dma.tx_chan_sel = DMA_SxCR_CHSEL_0;
spi3_dma.rx_nvic_irq = NVIC_DMA1_STREAM0_IRQ;
spi3_dma.tx_nvic_irq = NVIC_DMA1_STREAM5_IRQ;
#endif
spi3_dma.tx_dummy_buf = 0;
spi3_dma.tx_extra_dummy_dma = FALSE;
spi3_dma.rx_dummy_buf = 0;
@@ -787,12 +877,22 @@ void spi3_arch_init(void) {
rcc_peripheral_enable_clock(&RCC_APB1ENR, RCC_APB1ENR_SPI3EN);
// Configure GPIOs: SCK, MISO and MOSI
#ifdef STM32F1
// TODO configure lisa board files to use gpio_setup_pin_af function
gpio_set_mode(GPIO_BANK_SPI3_SCK, GPIO_MODE_OUTPUT_50_MHZ,
GPIO_CNF_OUTPUT_ALTFN_PUSHPULL,
GPIO_SPI3_SCK | GPIO_SPI3_MOSI);
gpio_set_mode(GPIO_BANK_SPI3_MISO, GPIO_MODE_INPUT, GPIO_CNF_INPUT_FLOAT,
GPIO_SPI3_MISO);
#elif defined STM32F4
gpio_setup_pin_af(SPI3_GPIO_PORT_MISO, SPI3_GPIO_MISO, SPI3_GPIO_AF, FALSE);
gpio_setup_pin_af(SPI3_GPIO_PORT_MOSI, SPI3_GPIO_MOSI, SPI3_GPIO_AF, TRUE);
gpio_setup_pin_af(SPI3_GPIO_PORT_SCK, SPI3_GPIO_SCK, SPI3_GPIO_AF, TRUE);
gpio_set_output_options(SPI3_GPIO_PORT_MOSI, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ, SPI3_GPIO_MOSI);
gpio_set_output_options(SPI3_GPIO_PORT_SCK, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ, SPI3_GPIO_SCK);
#endif
/// @todo disable JTAG so the pins can be used?
@@ -819,7 +919,11 @@ void spi3_arch_init(void) {
spi_set_nss_high(SPI3);
// Enable SPI_3 DMA clock
#ifdef STM32F1
rcc_peripheral_enable_clock(&RCC_AHBENR, RCC_AHBENR_DMA2EN);
#elif defined STM32F4
rcc_peripheral_enable_clock(&RCC_AHB1ENR, RCC_AHB1ENR_DMA1EN);
#endif
// Enable SPI3 periph.
spi_enable(SPI3);
@@ -838,22 +942,40 @@ void spi3_arch_init(void) {
*****************************************************************************/
#ifdef USE_SPI1
/// receive transferred over DMA
#ifdef STM32F1
void dma1_channel2_isr(void)
{
if ((DMA1_ISR & DMA_ISR_TCIF2) != 0) {
// clear int pending bit
DMA1_IFCR |= DMA_IFCR_CTCIF2;
}
#elif defined STM32F4
void dma2_stream0_isr(void)
{
if ((DMA2_LISR & DMA_LISR_TCIF0) != 0) {
// clear int pending bit
DMA2_LIFCR |= DMA_LIFCR_CTCIF0;
}
#endif
process_rx_dma_interrupt(&spi1);
}
/// transmit transferred over DMA
#ifdef STM32F1
void dma1_channel3_isr(void)
{
if ((DMA1_ISR & DMA_ISR_TCIF3) != 0) {
// clear int pending bit
DMA1_IFCR |= DMA_IFCR_CTCIF3;
}
#elif defined STM32F4
void dma2_stream3_isr(void)
{
if ((DMA2_LISR & DMA_LISR_TCIF3) != 0) {
// clear int pending bit
DMA2_LIFCR |= DMA_LIFCR_CTCIF3;
}
#endif
process_tx_dma_interrupt(&spi1);
}
@@ -861,22 +983,40 @@ void dma1_channel3_isr(void)
#ifdef USE_SPI2
/// receive transferred over DMA
#ifdef STM32F1
void dma1_channel4_isr(void)
{
if ((DMA1_ISR & DMA_ISR_TCIF4) != 0) {
// clear int pending bit
DMA1_IFCR |= DMA_IFCR_CTCIF4;
}
#elif defined STM32F4
void dma1_stream3_isr(void)
{
if ((DMA1_LISR & DMA_LISR_TCIF3) != 0) {
// clear int pending bit
DMA1_LIFCR |= DMA_LIFCR_CTCIF3;
}
#endif
process_rx_dma_interrupt(&spi2);
}
/// transmit transferred over DMA
#ifdef STM32F1
void dma1_channel5_isr(void)
{
if ((DMA1_ISR & DMA_ISR_TCIF5) != 0) {
// clear int pending bit
DMA1_IFCR |= DMA_IFCR_CTCIF5;
}
#elif defined STM32F4
void dma1_stream4_isr(void)
{
if ((DMA1_HISR & DMA_HISR_TCIF4) != 0) {
// clear int pending bit
DMA1_HIFCR |= DMA_HIFCR_CTCIF4;
}
#endif
process_tx_dma_interrupt(&spi2);
}
@@ -884,22 +1024,40 @@ void dma1_channel5_isr(void)
#if USE_SPI3
/// receive transferred over DMA
#ifdef STM32F1
void dma2_channel1_isr(void)
{
if ((DMA2_ISR & DMA_ISR_TCIF1) != 0) {
// clear int pending bit
DMA2_IFCR |= DMA_IFCR_CTCIF1;
}
#elif defined STM32F4
void dma1_stream0_isr(void)
{
if ((DMA1_LISR & DMA_LISR_TCIF0) != 0) {
// clear int pending bit
DMA1_LIFCR |= DMA_LIFCR_CTCIF0;
}
#endif
process_rx_dma_interrupt(&spi3);
}
/// transmit transferred over DMA
#ifdef STM32F1
void dma2_channel2_isr(void)
{
if ((DMA2_ISR & DMA_ISR_TCIF2) != 0) {
// clear int pending bit
DMA2_IFCR |= DMA_IFCR_CTCIF2;
}
#elif defined STM32F4
void dma1_stream5_isr(void)
{
if ((DMA1_HISR & DMA_HISR_TCIF5) != 0) {
// clear int pending bit
DMA1_HIFCR |= DMA_HIFCR_CTCIF5;
}
#endif
process_tx_dma_interrupt(&spi3);
}
@@ -917,7 +1075,11 @@ void process_rx_dma_interrupt(struct spi_periph *periph) {
spi_disable_rx_dma((uint32_t)periph->reg_addr);
/* Disable DMA rx channel */
#ifdef STM32F1
dma_disable_channel(dma->dma, dma->rx_chan);
#elif defined STM32F4
dma_disable_stream(dma->dma, dma->rx_chan);
#endif
if (dma->rx_extra_dummy_dma) {
@@ -935,13 +1097,23 @@ void process_rx_dma_interrupt(struct spi_periph *periph) {
spi_configure_dma(dma->dma, dma->rx_chan, (uint32_t)dma->spidr,
(uint32_t)&(dma->rx_dummy_buf), len_remaining, trans->dss, FALSE);
#ifdef STM32F1
dma_set_read_from_peripheral(dma->dma, dma->rx_chan);
dma_set_priority(dma->dma, dma->rx_chan, DMA_CCR_PL_HIGH);
#elif defined STM32F4
dma_channel_select(dma->dma, dma->rx_chan, dma->rx_chan_sel);
dma_set_transfer_mode(dma->dma, dma->rx_chan, DMA_SxCR_DIR_PERIPHERAL_TO_MEM);
dma_set_priority(dma->dma, dma->rx_chan, DMA_SxCR_PL_HIGH);
#endif
/* Enable DMA transfer complete interrupts. */
dma_enable_transfer_complete_interrupt(dma->dma, dma->rx_chan);
/* Enable DMA channels */
#ifdef STM32F1
dma_enable_channel(dma->dma, dma->rx_chan);
#elif defined STM32F4
dma_enable_stream(dma->dma, dma->rx_chan);
#endif
/* Enable SPI transfers via DMA */
spi_enable_rx_dma((uint32_t)periph->reg_addr);
}
@@ -979,7 +1151,11 @@ void process_tx_dma_interrupt(struct spi_periph *periph) {
spi_disable_tx_dma((uint32_t)periph->reg_addr);
/* Disable DMA tx channel */
#ifdef STM32F1
dma_disable_channel(dma->dma, dma->tx_chan);
#elif defined STM32F4
dma_disable_stream(dma->dma, dma->tx_chan);
#endif
if (dma->tx_extra_dummy_dma) {
/*
@@ -996,13 +1172,23 @@ void process_tx_dma_interrupt(struct spi_periph *periph) {
spi_configure_dma(dma->dma, dma->tx_chan, (uint32_t)dma->spidr,
(uint32_t)&(dma->tx_dummy_buf), len_remaining, trans->dss, FALSE);
#ifdef STM32F1
dma_set_read_from_memory(dma->dma, dma->tx_chan);
dma_set_priority(dma->dma, dma->tx_chan, DMA_CCR_PL_MEDIUM);
#elif defined STM32F4
dma_channel_select(dma->dma, dma->tx_chan, dma->tx_chan_sel);
dma_set_transfer_mode(dma->dma, dma->tx_chan, DMA_SxCR_DIR_MEM_TO_PERIPHERAL);
dma_set_priority(dma->dma, dma->tx_chan, DMA_SxCR_PL_MEDIUM);
#endif
/* Enable DMA transfer complete interrupts. */
dma_enable_transfer_complete_interrupt(dma->dma, dma->tx_chan);
/* Enable DMA channels */
#ifdef STM32F1
dma_enable_channel(dma->dma, dma->tx_chan);
#elif defined STM32F4
dma_enable_stream(dma->dma, dma->tx_chan);
#endif
/* Enable SPI transfers via DMA */
spi_enable_tx_dma((uint32_t)periph->reg_addr);
sw/airborne/arch/stm32/subsystems/actuators/actuators_pwm_arch.c
+6
View File
@@ -208,6 +208,9 @@ void actuators_pwm_arch_init(void) {
#ifdef PWM_SERVO_8
set_servo_gpio(PWM_SERVO_8_GPIO, PWM_SERVO_8_PIN, PWM_SERVO_8_AF, PWM_SERVO_8_RCC_IOP);
#endif
#ifdef PWM_SERVO_9
set_servo_gpio(PWM_SERVO_9_GPIO, PWM_SERVO_9_PIN, PWM_SERVO_9_AF, PWM_SERVO_9_RCC_IOP);
#endif
#if PWM_USE_TIM1
@@ -262,5 +265,8 @@ void actuators_pwm_commit(void) {
#ifdef PWM_SERVO_8
timer_set_oc_value(PWM_SERVO_8_TIMER, PWM_SERVO_8_OC, actuators_pwm_values[PWM_SERVO_8]);
#endif
#ifdef PWM_SERVO_9
timer_set_oc_value(PWM_SERVO_9_TIMER, PWM_SERVO_9_OC, actuators_pwm_values[PWM_SERVO_9]);
#endif
}
sw/airborne/arch/stm32/subsystems/imu/imu_krooz_sd_arch.c
+37
View File
@@ -0,0 +1,37 @@
#include "subsystems/imu.h"
#include <libopencm3/stm32/f4/rcc.h>
#include <libopencm3/stm32/f4/gpio.h>
#include <libopencm3/stm32/f4/nvic.h>
#include <libopencm3/stm32/exti.h>
#include "subsystems/imu/imu_krooz_sd_arch.h"
void imu_krooz_sd_arch_init(void) {
rcc_peripheral_enable_clock(&RCC_APB2ENR, RCC_APB2ENR_SYSCFGEN);
rcc_peripheral_enable_clock(&RCC_AHB1ENR, RCC_AHB1ENR_IOPBEN);
rcc_peripheral_enable_clock(&RCC_AHB1ENR, RCC_AHB1ENR_IOPCEN);
gpio_mode_setup(GPIOB, GPIO_MODE_INPUT, GPIO_PUPD_NONE, GPIO5);
gpio_mode_setup(GPIOC, GPIO_MODE_INPUT, GPIO_PUPD_NONE, GPIO6);
nvic_enable_irq(NVIC_EXTI9_5_IRQ);
exti_select_source(EXTI5, GPIOB);
exti_select_source(EXTI6, GPIOC);
exti_set_trigger(EXTI5, EXTI_TRIGGER_RISING);
exti_set_trigger(EXTI6, EXTI_TRIGGER_FALLING);
exti_enable_request(EXTI5);
exti_enable_request(EXTI6);
nvic_set_priority(NVIC_EXTI9_5_IRQ, 0x0F);
}
void exti9_5_isr(void) {
/* clear EXTI */
if(EXTI_PR & EXTI6) {
exti_reset_request(EXTI6);
hmc58xx_read(&imu_krooz.hmc);
}
if(EXTI_PR & EXTI5) {
exti_reset_request(EXTI5);
mpu60x0_i2c_read(&imu_krooz.mpu);
}
}
sw/airborne/arch/stm32/subsystems/imu/imu_krooz_sd_arch.h
+8
View File
@@ -0,0 +1,8 @@
#ifndef IMU_KROOZ_SD_ARCH_H
#define IMU_KROOZ_SD_ARCH_H
#include "subsystems/imu.h"
void imu_krooz_sd_arch_init(void);
#endif /* IMU_KROOZ_SD_ARCH_H */
sw/airborne/boards/apogee_0.99.h
+12
View File
@@ -60,6 +60,18 @@
#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
sw/airborne/boards/ardrone/at_com.h
+1 -1
View File
@@ -178,7 +178,7 @@ typedef struct _navdata_gps_t {
uint8_t unk_2[16];
struct{
uint8_t sat;
uint8_t unk;
uint8_t cn0;
}channels[12];
int32_t gps_plugged; /*!< When the gps is plugged */
uint8_t unk_3[108];
sw/airborne/boards/krooz/baro_board.c
+32
View File
@@ -0,0 +1,32 @@
#include "subsystems/sensors/baro.h"
#include "baro_board.h"
/*
#include "subsystems/datalink/downlink.h"
#include "mcu_periph/uart.h"
#include "mcu_periph/sys_time.h"
*/
struct Baro baro;
void baro_init(void) {
baro_ms5611_init();
}
void baro_periodic(void) {
static uint8_t cnt;
switch(cnt) {
case 0:
baro_ms5611_periodic();
cnt++;
break;
case 1:
baro_ms5611_d1();
cnt++;
break;
case 2:
baro_ms5611_d2();
cnt = 0;
break;
}
}
sw/airborne/boards/krooz/baro_board.h
+30
View File
@@ -0,0 +1,30 @@
/*
* board specific fonctions for the KroozSD board
*
*/
#ifndef BOARDS_KROOZ_BARO_H
#define BOARDS_KROOZ_BARO_H
#include "std.h"
#include "mcu_periph/i2c.h"
#include "modules/sensors/baro_ms5611_i2c.h"
#include "math/pprz_algebra_int.h"
//#include "led.h"
static inline void baro_event(void (*b_abs_handler)(void), void (*b_diff_handler)(void))
{
baro_ms5611_event();
if(baro_ms5611_valid) {
baro.status = BS_RUNNING;
baro.absolute = (int32_t)baroms;
b_abs_handler();
baro_ms5611_valid = FALSE;
}
}
#define BaroEvent(_b_abs_handler, _b_diff_handler) baro_event(_b_abs_handler,_b_diff_handler)
#endif /* BOARDS_KROOZ_SD_BARO_H */
sw/airborne/boards/krooz/imu_krooz.c
+160
View File
@@ -0,0 +1,160 @@
/*
* Copyright (C) 2013 Sergey Krukowski <softsr@yahoo.de>
*
* 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 boards/krooz/imu_krooz.c
*
* Driver for the IMU on the KroozSD board.
*
* Invensense MPU-6050
* Honeywell HMC-5883
*/
#include <math.h>
#include "boards/krooz/imu_krooz.h"
#include "subsystems/imu/imu_krooz_sd_arch.h"
#include "mcu_periph/i2c.h"
#include "led.h"
// Downlink
#include "mcu_periph/uart.h"
#include "messages.h"
#include "subsystems/datalink/downlink.h"
#ifndef DOWNLINK_DEVICE
#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
#endif
#if !defined KROOZ_LOWPASS_FILTER && !defined KROOZ_SMPLRT_DIV
#define KROOZ_LOWPASS_FILTER MPU60X0_DLPF_256HZ
#define KROOZ_SMPLRT_DIV 1
#endif
PRINT_CONFIG_VAR(KROOZ_SMPLRT_DIV)
PRINT_CONFIG_VAR(KROOZ_LOWPASS_FILTER)
#ifndef KROOZ_GYRO_RANGE
#define KROOZ_GYRO_RANGE MPU60X0_GYRO_RANGE_250
#endif
PRINT_CONFIG_VAR(KROOZ_GYRO_RANGE)
#ifndef KROOZ_ACCEL_RANGE
#define KROOZ_ACCEL_RANGE MPU60X0_ACCEL_RANGE_2G
#endif
PRINT_CONFIG_VAR(KROOZ_ACCEL_RANGE)
struct ImuKrooz imu_krooz;
#if KROOZ_USE_MEDIAN_FILTER
#include "filters/median_filter.h"
struct MedianFilter3Int median_gyro, median_accel, median_mag;
#endif
void imu_impl_init( void )
{
/////////////////////////////////////////////////////////////////////
// MPU-60X0
mpu60x0_i2c_init(&imu_krooz.mpu, &(IMU_KROOZ_I2C_DEV), MPU60X0_ADDR);
// change the default configuration
imu_krooz.mpu.config.smplrt_div = KROOZ_SMPLRT_DIV;
imu_krooz.mpu.config.dlpf_cfg = KROOZ_LOWPASS_FILTER;
imu_krooz.mpu.config.gyro_range = KROOZ_GYRO_RANGE;
imu_krooz.mpu.config.accel_range = KROOZ_ACCEL_RANGE;
imu_krooz.mpu.config.drdy_int_enable = TRUE;
hmc58xx_init(&imu_krooz.hmc, &(IMU_KROOZ_I2C_DEV), HMC58XX_ADDR);
#if KROOZ_USE_MEDIAN_FILTER
// Init median filters
InitMedianFilterRatesInt(median_gyro);
InitMedianFilterVect3Int(median_accel);
InitMedianFilterVect3Int(median_mag);
#endif
RATES_ASSIGN(imu_krooz.rates_sum, 0, 0, 0);
VECT3_ASSIGN(imu_krooz.accel_sum, 0, 0, 0);
imu_krooz.meas_nb = 0;
imu_krooz.gyr_valid = FALSE;
imu_krooz.acc_valid = FALSE;
imu_krooz.mag_valid = FALSE;
imu_krooz_sd_arch_init();
}
void imu_periodic( void )
{
// Start reading the latest gyroscope data
if (!imu_krooz.mpu.config.initialized)
mpu60x0_i2c_start_configure(&imu_krooz.mpu);
if (!imu_krooz.hmc.initialized)
hmc58xx_start_configure(&imu_krooz.hmc);
if (imu_krooz.meas_nb) {
RATES_ASSIGN(imu.gyro_unscaled, imu_krooz.rates_sum.q / imu_krooz.meas_nb, imu_krooz.rates_sum.p / imu_krooz.meas_nb, imu_krooz.rates_sum.r / imu_krooz.meas_nb);
#if KROOZ_USE_MEDIAN_FILTER
UpdateMedianFilterRatesInt(median_gyro, imu.gyro_unscaled);
#endif
VECT3_ASSIGN(imu.accel_unscaled, imu_krooz.accel_sum.y / imu_krooz.meas_nb, imu_krooz.accel_sum.x / imu_krooz.meas_nb, imu_krooz.accel_sum.z / imu_krooz.meas_nb);
#if KROOZ_USE_MEDIAN_FILTER
UpdateMedianFilterVect3Int(median_accel, imu.accel_unscaled);
#endif
RATES_ASSIGN(imu_krooz.rates_sum, 0, 0, 0);
VECT3_ASSIGN(imu_krooz.accel_sum, 0, 0, 0);
imu_krooz.meas_nb = 0;
imu_krooz.gyr_valid = TRUE;
imu_krooz.acc_valid = TRUE;
}
//RunOnceEvery(10,imu_krooz_downlink_raw());
}
void imu_krooz_downlink_raw( void )
{
DOWNLINK_SEND_IMU_GYRO_RAW(DefaultChannel, DefaultDevice,&imu.gyro_unscaled.p,&imu.gyro_unscaled.q,&imu.gyro_unscaled.r);
DOWNLINK_SEND_IMU_ACCEL_RAW(DefaultChannel, DefaultDevice,&imu.accel_unscaled.x,&imu.accel_unscaled.y,&imu.accel_unscaled.z);
}
void imu_krooz_event( void )
{
// If the MPU6050 I2C transaction has succeeded: convert the data
mpu60x0_i2c_event(&imu_krooz.mpu);
if (imu_krooz.mpu.data_available) {
RATES_ADD(imu_krooz.rates_sum, imu_krooz.mpu.data_rates.rates);
VECT3_ADD(imu_krooz.accel_sum, imu_krooz.mpu.data_accel.vect);
imu_krooz.meas_nb++;
imu_krooz.mpu.data_available = FALSE;
}
// If the HMC5883 I2C transaction has succeeded: convert the data
hmc58xx_event(&imu_krooz.hmc);
if (imu_krooz.hmc.data_available) {
VECT3_COPY(imu.mag_unscaled, imu_krooz.hmc.data.vect);
#if KROOZ_USE_MEDIAN_FILTER
UpdateMedianFilterVect3Int(median_mag, imu.mag_unscaled);
#endif
imu_krooz.hmc.data_available = FALSE;
imu_krooz.mag_valid = TRUE;
}
}
sw/airborne/boards/krooz/imu_krooz.h
+142
View File
@@ -0,0 +1,142 @@
/*
* Copyright (C) 2013 Sergey Krukowski <softsr@yahoo.de>
*
* 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 boards/krooz/imu_krooz.h
*
* Driver for the IMU on the KroozSD board.
*
* Invensense MPU-6050
*/
#ifndef IMU_KROOZ_H
#define IMU_KROOZ_H
#include "std.h"
#include "generated/airframe.h"
#include "subsystems/imu.h"
#include "peripherals/mpu60x0_i2c.h"
#include "peripherals/hmc58xx.h"
// Default configuration
#if !defined IMU_GYRO_P_SIGN & !defined IMU_GYRO_Q_SIGN & !defined IMU_GYRO_R_SIGN
#define IMU_GYRO_P_SIGN -1
#define IMU_GYRO_Q_SIGN 1
#define IMU_GYRO_R_SIGN 1
#endif
#if !defined IMU_ACCEL_X_SIGN & !defined IMU_ACCEL_Y_SIGN & !defined IMU_ACCEL_Z_SIGN
#define IMU_ACCEL_X_SIGN -1
#define IMU_ACCEL_Y_SIGN 1
#define IMU_ACCEL_Z_SIGN 1
#endif
#if !defined IMU_MAG_X_SIGN & !defined IMU_MAG_Y_SIGN & !defined IMU_MAG_Z_SIGN
#define IMU_MAG_X_SIGN 1
#define IMU_MAG_Y_SIGN 1
#define IMU_MAG_Z_SIGN 1
#endif
/** default gyro sensitivy and neutral from the datasheet
* MPU with 250 deg/s has 131.072 LSB/(deg/s)
* sens = 1/131.072 * pi/180 * 2^INT32_RATE_FRAC
* sens = 1/131.072 * pi/180 * 4096 = 0.5454
I*/
#if !defined IMU_GYRO_P_SENS & !defined IMU_GYRO_Q_SENS & !defined IMU_GYRO_R_SENS
// FIXME
#define IMU_GYRO_P_SENS 0.5454
#define IMU_GYRO_P_SENS_NUM 2727
#define IMU_GYRO_P_SENS_DEN 5000
#define IMU_GYRO_Q_SENS 0.5454
#define IMU_GYRO_Q_SENS_NUM 2727
#define IMU_GYRO_Q_SENS_DEN 5000
#define IMU_GYRO_R_SENS 0.5454
#define IMU_GYRO_R_SENS_NUM 2727
#define IMU_GYRO_R_SENS_DEN 5000
#endif
#if !defined IMU_GYRO_P_NEUTRAL & !defined IMU_GYRO_Q_NEUTRAL & !defined IMU_GYRO_R_NEUTRAL
#define IMU_GYRO_P_NEUTRAL 0
#define IMU_GYRO_Q_NEUTRAL 0
#define IMU_GYRO_R_NEUTRAL 0
#endif
/** default accel sensitivy from the datasheet
* MPU with 2g has 16384 LSB/g
* sens = 9.81 [m/s^2] / 16384 [LSB/g] * 2^INT32_ACCEL_FRAC = 0.6131
*/
#if !defined IMU_ACCEL_X_SENS & !defined IMU_ACCEL_Y_SENS & !defined IMU_ACCEL_Z_SENS
// FIXME
#define IMU_ACCEL_X_SENS 0.6131
#define IMU_ACCEL_X_SENS_NUM 6131
#define IMU_ACCEL_X_SENS_DEN 10000
#define IMU_ACCEL_Y_SENS 0.6131
#define IMU_ACCEL_Y_SENS_NUM 6131
#define IMU_ACCEL_Y_SENS_DEN 10000
#define IMU_ACCEL_Z_SENS 0.6131
#define IMU_ACCEL_Z_SENS_NUM 6131
#define IMU_ACCEL_Z_SENS_DEN 10000
#endif
#if !defined IMU_ACCEL_X_NEUTRAL & !defined IMU_ACCEL_Y_NEUTRAL & !defined IMU_ACCEL_Z_NEUTRAL
#define IMU_ACCEL_X_NEUTRAL 0
#define IMU_ACCEL_Y_NEUTRAL 0
#define IMU_ACCEL_Z_NEUTRAL 0
#endif
struct ImuKrooz {
volatile bool_t gyr_valid;
volatile bool_t acc_valid;
volatile bool_t mag_valid;
struct Mpu60x0_I2c mpu;
struct Hmc58xx hmc;
struct Int32Rates rates_sum;
struct Int32Vect3 accel_sum;
volatile uint8_t meas_nb;
};
extern struct ImuKrooz imu_krooz;
/* must be defined in order to be IMU code: declared in imu.h
extern void imu_impl_init(void);
extern void imu_periodic(void);
*/
/* Own Extra Functions */
extern void imu_krooz_event( void );
extern void imu_krooz_downlink_raw( void );
static inline void ImuEvent(void (* _gyro_handler)(void), void (* _accel_handler)(void), void (* _mag_handler)(void) __attribute__((unused))) {
imu_krooz_event();
if (imu_krooz.gyr_valid) {
imu_krooz.gyr_valid = FALSE;
_gyro_handler();
}
if (imu_krooz.acc_valid) {
imu_krooz.acc_valid = FALSE;
_accel_handler();
}
if (imu_krooz.mag_valid) {
imu_krooz.mag_valid = FALSE;
_mag_handler();
}
}
#endif // IMU_KROOZ_H
sw/airborne/boards/krooz_sd.h
+81 -59
View File
@@ -1,5 +1,5 @@
#ifndef CONFIG_KROOZ_1_0_H
#define CONFIG_KROOZ_1_0_H
#ifndef CONFIG_KROOZ_SD_H
#define CONFIG_KROOZ_SD_H
#define BOARD_KROOZ
@@ -99,6 +99,29 @@
#define UART5_GPIO_PORT_TX GPIOC
#define UART5_GPIO_TX GPIO12
/* 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 SPI2_GPIO_AF GPIO_AF5
#define SPI2_GPIO_PORT_MISO GPIOB
#define SPI2_GPIO_MISO GPIO14
#define SPI2_GPIO_PORT_MOSI GPIOB
#define SPI2_GPIO_MOSI GPIO15
#define SPI2_GPIO_PORT_SCK GPIOB
#define SPI2_GPIO_SCK GPIO13
#define SPI_SELECT_SLAVE0_PORT GPIOA
#define SPI_SELECT_SLAVE0_PIN GPIO4
#define SPI_SELECT_SLAVE1_PORT GPIOB
#define SPI_SELECT_SLAVE1_PIN GPIO12
#define SPI_SELECT_SLAVE2_PORT GPIOB
#define SPI_SELECT_SLAVE2_PIN GPIO2
/* I2C mapping */
#define I2C1_GPIO_PORT GPIOB
@@ -109,6 +132,10 @@
#define I2C2_GPIO_SCL GPIO10
#define I2C2_GPIO_SDA GPIO11
#define I2C3_GPIO_PORT_SCL GPIOA
#define I2C3_GPIO_PORT_SDA GPIOC
#define I2C3_GPIO_SCL GPIO8
#define I2C3_GPIO_SDA GPIO9
/* Onboard ADCs */
#define USE_AD_TIM1 1
@@ -197,20 +224,10 @@
}
#endif // USE_AD1
/* I2C mapping */
#define GPIO_I2C1_SCL GPIO8
#define GPIO_I2C1_SDA GPIO9
#define GPIO_I2C2_SCL GPIO10
#define GPIO_I2C2_SDA GPIO11
#define GPIO_I2C3_SCL GPIO8 //PA8
#define GPIO_I2C3_SDA GPIO9 //PC9
/* Activate onboard baro */
#define BOARD_HAS_BARO 1
/* PWM */
//#define PWM_USE_TIM2 1
#define PWM_USE_TIM3 1
#define PWM_USE_TIM4 1
#define PWM_USE_TIM5 1
@@ -227,18 +244,23 @@
#define USE_PWM9 1
//#define USE_PWM10 1
#if USE_PWM10
#define ACTUATORS_PWM_NB 11
#define PWM_USE_TIM2 1
#else
#define ACTUATORS_PWM_NB 10
#endif
// 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_IOPCEN
#define PWM_SERVO_0_GPIO GPIOC
#define PWM_SERVO_0_PIN GPIO6
#define PWM_SERVO_0_AF GPIO_AF1
#define PWM_SERVO_0_OC TIM_OC1
#define PWM_SERVO_0_OC_BIT (1<<0)
#define PWM_SERVO_0_RCC_IOP RCC_AHB1ENR_IOPBEN
#define PWM_SERVO_0_GPIO GPIOB
#define PWM_SERVO_0_PIN GPIO1
#define PWM_SERVO_0_AF GPIO_AF2
#define PWM_SERVO_0_OC TIM_OC4
#define PWM_SERVO_0_OC_BIT (1<<3)
#else
#define PWM_SERVO_0_OC_BIT 0
#endif
@@ -248,10 +270,10 @@
#define PWM_SERVO_1_TIMER TIM3
#define PWM_SERVO_1_RCC_IOP RCC_AHB1ENR_IOPCEN
#define PWM_SERVO_1_GPIO GPIOC
#define PWM_SERVO_1_PIN GPIO7
#define PWM_SERVO_1_AF GPIO_AF1
#define PWM_SERVO_1_OC TIM_OC2
#define PWM_SERVO_1_OC_BIT (1<<1)
#define PWM_SERVO_1_PIN GPIO8
#define PWM_SERVO_1_AF GPIO_AF2
#define PWM_SERVO_1_OC TIM_OC3
#define PWM_SERVO_1_OC_BIT (1<<2)
#else
#define PWM_SERVO_1_OC_BIT 0
#endif
@@ -261,23 +283,23 @@
#define PWM_SERVO_2_TIMER TIM3
#define PWM_SERVO_2_RCC_IOP RCC_AHB1ENR_IOPCEN
#define PWM_SERVO_2_GPIO GPIOC
#define PWM_SERVO_2_PIN GPIO8
#define PWM_SERVO_2_AF GPIO_AF1
#define PWM_SERVO_2_OC TIM_OC3
#define PWM_SERVO_2_OC_BIT (1<<2)
#define PWM_SERVO_2_PIN GPIO7
#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 3
#define PWM_SERVO_3_TIMER TIM3
#define PWM_SERVO_3_RCC_IOP RCC_AHB1ENR_IOPCEN
#define PWM_SERVO_3_GPIO GPIOC
#define PWM_SERVO_3_PIN GPIO9
#define PWM_SERVO_3_AF GPIO_AF1
#define PWM_SERVO_3_OC TIM_OC4
#define PWM_SERVO_3_OC_BIT (1<<3)
#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
@@ -287,10 +309,10 @@
#define PWM_SERVO_4_TIMER TIM4
#define PWM_SERVO_4_RCC_IOP RCC_AHB1ENR_IOPBEN
#define PWM_SERVO_4_GPIO GPIOB
#define PWM_SERVO_4_PIN GPIO6
#define PWM_SERVO_4_AF GPIO_AF1
#define PWM_SERVO_4_OC TIM_OC1
#define PWM_SERVO_4_OC_BIT (1<<0)
#define PWM_SERVO_4_PIN GPIO7
#define PWM_SERVO_4_AF GPIO_AF2
#define PWM_SERVO_4_OC TIM_OC2
#define PWM_SERVO_4_OC_BIT (1<<1)
#else
#define PWM_SERVO_4_OC_BIT 0
#endif
@@ -300,10 +322,10 @@
#define PWM_SERVO_5_TIMER TIM4
#define PWM_SERVO_5_RCC_IOP RCC_AHB1ENR_IOPBEN
#define PWM_SERVO_5_GPIO GPIOB
#define PWM_SERVO_5_PIN GPIO7
#define PWM_SERVO_5_AF GPIO_AF1
#define PWM_SERVO_5_OC TIM_OC2
#define PWM_SERVO_5_OC_BIT (1<<1)
#define PWM_SERVO_5_PIN GPIO6
#define PWM_SERVO_5_AF GPIO_AF2
#define PWM_SERVO_5_OC TIM_OC1
#define PWM_SERVO_5_OC_BIT (1<<0)
#else
#define PWM_SERVO_5_OC_BIT 0
#endif
@@ -313,10 +335,10 @@
#define PWM_SERVO_6_TIMER TIM5
#define PWM_SERVO_6_RCC_IOP RCC_AHB1ENR_IOPAEN
#define PWM_SERVO_6_GPIO GPIOA
#define PWM_SERVO_6_PIN GPIO0
#define PWM_SERVO_6_AF GPIO_AF1
#define PWM_SERVO_6_OC TIM_OC1
#define PWM_SERVO_6_OC_BIT (1<<0)
#define PWM_SERVO_6_PIN GPIO3
#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
@@ -326,10 +348,10 @@
#define PWM_SERVO_7_TIMER TIM5
#define PWM_SERVO_7_RCC_IOP RCC_AHB1ENR_IOPAEN
#define PWM_SERVO_7_GPIO GPIOA
#define PWM_SERVO_7_PIN GPIO1
#define PWM_SERVO_7_AF GPIO_AF1
#define PWM_SERVO_7_OC TIM_OC2
#define PWM_SERVO_7_OC_BIT (1<<1)
#define PWM_SERVO_7_PIN GPIO2
#define PWM_SERVO_7_AF GPIO_AF2
#define PWM_SERVO_7_OC TIM_OC3
#define PWM_SERVO_7_OC_BIT (1<<2)
#else
#define PWM_SERVO_7_OC_BIT 0
#endif
@@ -339,10 +361,10 @@
#define PWM_SERVO_8_TIMER TIM5
#define PWM_SERVO_8_RCC_IOP RCC_AHB1ENR_IOPAEN
#define PWM_SERVO_8_GPIO GPIOA
#define PWM_SERVO_8_PIN GPIO2
#define PWM_SERVO_8_AF GPIO_AF1
#define PWM_SERVO_8_OC TIM_OC3
#define PWM_SERVO_8_OC_BIT (1<<2)
#define PWM_SERVO_8_PIN GPIO1
#define PWM_SERVO_8_AF GPIO_AF2
#define PWM_SERVO_8_OC TIM_OC2
#define PWM_SERVO_8_OC_BIT (1<<1)
#else
#define PWM_SERVO_8_OC_BIT 0
#endif
@@ -352,10 +374,10 @@
#define PWM_SERVO_9_TIMER TIM5
#define PWM_SERVO_9_RCC_IOP RCC_AHB1ENR_IOPAEN
#define PWM_SERVO_9_GPIO GPIOA
#define PWM_SERVO_9_PIN GPIO3
#define PWM_SERVO_9_AF GPIO_AF1
#define PWM_SERVO_9_OC TIM_OC4
#define PWM_SERVO_9_OC_BIT (1<<3)
#define PWM_SERVO_9_PIN GPIO0
#define PWM_SERVO_9_AF GPIO_AF2
#define PWM_SERVO_9_OC TIM_OC1
#define PWM_SERVO_9_OC_BIT (1<<0)
#else
#define PWM_SERVO_9_OC_BIT 0
#endif
@@ -397,7 +419,7 @@
* Spektrum
*/
/* The line that is pulled low at power up to initiate the bind process */
#define SPEKTRUM_BIND_PIN GPIO8
#define SPEKTRUM_BIND_PIN GPIO9
#define SPEKTRUM_BIND_PIN_PORT GPIOA
#endif /* CONFIG_KROOZ_1_0_H */
#endif /* CONFIG_KROOZ_SD_H */
sw/airborne/firmwares/rotorcraft/autopilot.c
+13 -2
View File
@@ -65,6 +65,7 @@ static inline int ahrs_is_aligned(void) {
return (ahrs.status == AHRS_RUNNING);
}
#else
PRINT_CONFIG_MSG("Using AUTOPILOT_DISABLE_AHRS_KILL")
static inline int ahrs_is_aligned(void) {
return TRUE;
}
@@ -80,7 +81,7 @@ static inline int ahrs_is_aligned(void) {
#ifndef MODE_STARTUP
#define MODE_STARTUP AP_MODE_KILL
PRINT_CONFIG_MSG("Using AP_MODE_KILL as MODE_STARTUP")
PRINT_CONFIG_MSG("Using default AP_MODE_KILL as MODE_STARTUP")
#endif
static void send_alive(void) {
@@ -173,7 +174,8 @@ static void send_baro_raw(void) {
}
void autopilot_init(void) {
autopilot_mode = MODE_STARTUP;
/* mode is finally set at end of init if MODE_STARTUP is not KILL */
autopilot_mode = AP_MODE_KILL;
autopilot_motors_on = FALSE;
kill_throttle = ! autopilot_motors_on;
autopilot_in_flight = FALSE;
@@ -187,8 +189,17 @@ void autopilot_init(void) {
#ifdef POWER_SWITCH_LED
LED_ON(POWER_SWITCH_LED); // POWER OFF
#endif
autopilot_arming_init();
nav_init();
guidance_h_init();
guidance_v_init();
stabilization_init();
/* set startup mode, propagats through to guidance h/v */
autopilot_set_mode(MODE_STARTUP);
register_periodic_telemetry(DefaultPeriodic, "ALIVE", send_alive);
register_periodic_telemetry(DefaultPeriodic, "ROTORCRAFT_STATUS", send_status);
register_periodic_telemetry(DefaultPeriodic, "ROTORCRAFT_FP", send_fp);
sw/airborne/firmwares/rotorcraft/main.c
+3 -5
View File
@@ -140,11 +140,6 @@ STATIC_INLINE void main_init( void ) {
baro_init();
imu_init();
autopilot_init();
nav_init();
guidance_h_init();
guidance_v_init();
stabilization_init();
ahrs_aligner_init();
ahrs_init();
@@ -155,6 +150,8 @@ STATIC_INLINE void main_init( void ) {
gps_init();
#endif
autopilot_init();
modules_init();
settings_init();
@@ -227,6 +224,7 @@ STATIC_INLINE void failsafe_check( void ) {
#if USE_GPS
if (autopilot_mode == AP_MODE_NAV &&
autopilot_motors_on &&
#if NO_GPS_LOST_WITH_RC_VALID
radio_control.status != RC_OK &&
#endif
sw/airborne/modules/digital_cam/led_cam_ctrl.c
+6
View File
@@ -52,6 +52,12 @@ void dc_send_command(uint8_t cmd)
case DC_ON:
DC_PUSH(DC_POWER_LED);
break;
#endif
#ifdef DC_POWER_OFF_LED
case DC_OFF:
DC_PUSH(DC_POWER_OFF_LED);
dc_timer = DC_POWER_OFF_DELAY;
break;
#endif
default:
break;
sw/airborne/modules/digital_cam/led_cam_ctrl.h
+11
View File
@@ -35,6 +35,7 @@
* <define name="DC_ZOOM_IN_LED" value="7"/>
* <define name="DC_ZOOM_OUT_LED" value="8"/>
* <define name="DC_POWER_LED" value="9"/>
* <define name="DC_POWER_OFF_LED" value="10"/>
* @endverbatim
* Related bank and pin must also be defined:
* @verbatim
@@ -68,6 +69,10 @@ extern uint8_t dc_timer;
#define DC_SHUTTER_DELAY 2 /* 4Hz -> 0.5s */
#endif
#ifndef DC_POWER_OFF_DELAY
#define DC_POWER_OFF_DELAY 3
#endif
#ifndef DC_SHUTTER_LED
#error DC: Please specify at least a SHUTTER LED
#endif
@@ -90,6 +95,9 @@ static inline void led_cam_ctrl_init(void)
#ifdef DC_POWER_LED
DC_RELEASE(DC_POWER_LED);
#endif
#ifdef DC_POWER_OFF_LED
DC_RELEASE(DC_POWER_OFF_LED);
#endif
}
@@ -114,6 +122,9 @@ static inline void led_cam_ctrl_periodic( void )
#endif
#ifdef DC_POWER_LED
DC_RELEASE(DC_POWER_LED);
#endif
#ifdef DC_POWER_OFF_LED
DC_RELEASE(DC_POWER_OFF_LED);
#endif
}
sw/airborne/modules/sensors/airspeed_ets.c
+21 -4
View File
@@ -42,13 +42,14 @@
#include "state.h"
#include "mcu_periph/i2c.h"
#include "mcu_periph/uart.h"
#include "mcu_periph/sys_time.h"
#include "messages.h"
#include "subsystems/datalink/downlink.h"
#include <math.h>
#if !USE_AIRSPEED
#ifndef SENSOR_SYNC_SEND
#warning either set USE_AIRSPEED or SENSOR_SYNC_SEND to use ets_airspeed
#ifndef AIRSPEED_ETS_SYNC_SEND
#warning either set USE_AIRSPEED or AIRSPEED_ETS_SYNC_SEND to use ets_airspeed
#endif
#endif
@@ -68,6 +69,13 @@
#ifndef AIRSPEED_ETS_I2C_DEV
#define AIRSPEED_ETS_I2C_DEV i2c0
#endif
PRINT_CONFIG_VAR(AIRSPEED_ETS_I2C_DEV)
/** delay in seconds until sensor is read after startup */
#ifndef AIRSPEED_ETS_START_DELAY
#define AIRSPEED_ETS_START_DELAY 0.2
#endif
PRINT_CONFIG_VAR(AIRSPEED_ETS_START_DELAY)
// Global variables
@@ -85,6 +93,8 @@ volatile bool_t airspeed_ets_i2c_done;
bool_t airspeed_ets_offset_init;
uint32_t airspeed_ets_offset_tmp;
uint16_t airspeed_ets_cnt;
uint32_t airspeed_ets_delay_time;
bool_t airspeed_ets_delay_done;
void airspeed_ets_init( void ) {
int n;
@@ -93,7 +103,7 @@ void airspeed_ets_init( void ) {
airspeed_ets_offset = 0;
airspeed_ets_offset_tmp = 0;
airspeed_ets_i2c_done = TRUE;
airspeed_ets_valid = TRUE;
airspeed_ets_valid = FALSE;
airspeed_ets_offset_init = FALSE;
airspeed_ets_cnt = AIRSPEED_ETS_OFFSET_NBSAMPLES_INIT + AIRSPEED_ETS_OFFSET_NBSAMPLES_AVRG;
@@ -102,10 +112,17 @@ void airspeed_ets_init( void ) {
airspeed_ets_buffer[n] = 0.0;
airspeed_ets_i2c_trans.status = I2CTransDone;
airspeed_ets_delay_done = FALSE;
SysTimeTimerStart(airspeed_ets_delay_time);
}
void airspeed_ets_read_periodic( void ) {
#ifndef SITL
if (!airspeed_ets_delay_done) {
if (SysTimeTimer(airspeed_ets_delay_time) < USEC_OF_SEC(AIRSPEED_ETS_START_DELAY)) return;
else airspeed_ets_delay_done = TRUE;
}
if (airspeed_ets_i2c_trans.status == I2CTransDone)
i2c_receive(&AIRSPEED_ETS_I2C_DEV, &airspeed_ets_i2c_trans, AIRSPEED_ETS_ADDR, 2);
#else // SITL
@@ -170,7 +187,7 @@ void airspeed_ets_read_event( void ) {
#if USE_AIRSPEED
stateSetAirspeed_f(&airspeed_ets);
#endif
#ifdef SENSOR_SYNC_SEND
#ifdef AIRSPEED_ETS_SYNC_SEND
DOWNLINK_SEND_AIRSPEED_ETS(DefaultChannel, DefaultDevice, &airspeed_ets_raw, &airspeed_ets_offset, &airspeed_ets);
#endif
} else {
sw/airborne/modules/sensors/baro_amsys.c
+98 -84
View File
@@ -45,12 +45,21 @@
#define BARO_AMSYS_ADDR 0xE4
#define BARO_AMSYS_REG 0x07
#define BARO_AMSYS_SCALE 0.32
#ifndef BARO_AMSYS_SCALE
#define BARO_AMSYS_SCALE 1
#endif
#ifndef BARO_AMSYS_MAX_PRESSURE
#define BARO_AMSYS_MAX_PRESSURE 103400 // Pascal
#endif
#define BARO_AMSYS_OFFSET_MAX 29491
#define BARO_AMSYS_OFFSET_MIN 3277
#define BARO_AMSYS_OFFSET_NBSAMPLES_INIT 40
#define BARO_AMSYS_OFFSET_NBSAMPLES_AVRG 60
#ifndef BARO_AMSYS_FILTER
#define BARO_AMSYS_FILTER 0
#endif
#define BARO_AMSYS_R 0.5
#define BARO_AMSYS_SIGMA2 0.1
#ifdef MEASURE_AMSYS_TEMPERATURE
#define TEMPERATURE_AMSYS_OFFSET_MAX 29491
@@ -59,8 +68,6 @@
#define TEMPERATURE_AMSYS_MIN -25
#endif
//#define BARO_AMSYS_R 0.5
//#define BARO_AMSYS_SIGMA2 0.1
//#define BARO_ALT_CORRECTION 0.0
#ifndef BARO_AMSYS_I2C_DEV
#define BARO_AMSYS_I2C_DEV i2c0
@@ -73,17 +80,18 @@ uint16_t baro_amsys_adc;
float baro_amsys_offset;
bool_t baro_amsys_valid;
float baro_amsys_altitude;
bool_t baro_amsys_enabled;
float baro_amsys_r;
float baro_amsys_sigma2;
float baro_amsys_temp;
float baro_amsys_p;
float baro_amsys_offset_altitude;
float baro_amsys_abs_altitude;
float ref_alt_init; //Altitude by initialising
float baro_scale;
float baro_filter;
float baro_old;
//float baro_amsys_r;
//float baro_amsys_sigma2;
struct i2c_transaction baro_amsys_i2c_trans;
@@ -93,106 +101,112 @@ double baro_amsys_offset_tmp;
uint16_t baro_amsys_cnt;
void baro_amsys_init( void ) {
baro_filter=BARO_FILTER;
pBaroRaw = 0;
tBaroRaw = 0;
baro_amsys_altitude = 0.0;
baro_amsys_p=0.0;
baro_amsys_offset = 0;
baro_amsys_offset_tmp = 0;
baro_amsys_valid = TRUE;
baro_amsys_offset_init = FALSE;
// baro_amsys_enabled = TRUE;
baro_amsys_cnt = BARO_AMSYS_OFFSET_NBSAMPLES_INIT + BARO_AMSYS_OFFSET_NBSAMPLES_AVRG;
// baro_amsys_r = BARO_AMSYS_R;
// baro_amsys_sigma2 = BARO_AMSYS_SIGMA2;
// baro_head=0;
ref_alt_init = 0;
baro_amsys_i2c_trans.status = I2CTransDone;
baro_filter=BARO_AMSYS_FILTER;
pBaroRaw = 0;
tBaroRaw = 0;
baro_amsys_altitude = 0.0;
baro_amsys_p=0.0;
baro_amsys_offset = 0;
baro_amsys_offset_tmp = 0;
baro_amsys_valid = TRUE;
baro_amsys_offset_init = FALSE;
baro_amsys_enabled = TRUE;
baro_scale = BARO_AMSYS_SCALE;
baro_amsys_cnt = BARO_AMSYS_OFFSET_NBSAMPLES_INIT + BARO_AMSYS_OFFSET_NBSAMPLES_AVRG;
baro_amsys_r = BARO_AMSYS_R;
baro_amsys_sigma2 = BARO_AMSYS_SIGMA2;
// baro_head=0;
ref_alt_init = 0;
baro_amsys_i2c_trans.status = I2CTransDone;
}
void baro_amsys_read_periodic( void ) {
// Initiate next read
#ifndef SITL
if (baro_amsys_i2c_trans.status == I2CTransDone){
if (baro_amsys_i2c_trans.status == I2CTransDone){
#ifndef MEASURE_AMSYS_TEMPERATURE
i2c_receive(&BARO_AMSYS_I2C_DEV, &baro_amsys_i2c_trans, BARO_AMSYS_ADDR, 2);
i2c_receive(&BARO_AMSYS_I2C_DEV, &baro_amsys_i2c_trans, BARO_AMSYS_ADDR, 2);
#else
i2c_receive(&BARO_AMSYS_I2C_DEV, &baro_amsys_i2c_trans, BARO_AMSYS_ADDR, 4);
i2c_receive(&BARO_AMSYS_I2C_DEV, &baro_amsys_i2c_trans, BARO_AMSYS_ADDR, 4);
#endif
}
}
#else // SITL
pBaroRaw = 0;
baro_amsys_altitude = gps.hmsl / 1000.0;
baro_amsys_adc = baro_amsys_altitude / BARO_AMSYS_SCALE;
baro_amsys_valid = TRUE;
/* fake an offset so sim works for under hmsl as well */
if (!baro_amsys_offset_init) {
baro_amsys_offset = BARO_AMSYS_OFFSET_MAX;
baro_amsys_offset_init = TRUE;
}
pBaroRaw = 0;
baro_amsys_altitude = gps.hmsl / 1000.0;
baro_amsys_adc = baro_amsys_offset - ((baro_amsys_altitude - ground_alt) / INS_BARO_SENS);
baro_amsys_valid = TRUE;
#endif
#ifdef BARO_AMSYS_SYNC_SEND
DOWNLINK_SEND_AMSYS_BARO(DefaultChannel, DefaultDevice, &pBaroRaw, &baro_amsys_p, &baro_amsys_offset, &ref_alt_init, &baro_amsys_abs_altitude, &baro_amsys_altitude, &baro_amsys_temp);
#else
RunOnceEvery(10, DOWNLINK_SEND_AMSYS_BARO(DefaultChannel, DefaultDevice, &pBaroRaw, &baro_amsys_p, &baro_amsys_offset, &ref_alt_init, &baro_amsys_abs_altitude, &baro_amsys_altitude, &baro_amsys_temp));
#endif
}
void baro_amsys_read_event( void ) {
pBaroRaw = 0;
// Get raw altimeter from buffer
pBaroRaw = (baro_amsys_i2c_trans.buf[0] << 8) | baro_amsys_i2c_trans.buf[1];
pBaroRaw = 0;
// Get raw altimeter from buffer
pBaroRaw = (baro_amsys_i2c_trans.buf[0] << 8) | baro_amsys_i2c_trans.buf[1];
#ifdef MEASURE_AMSYS_TEMPERATURE
tBaroRaw = (baro_amsys_i2c_trans.buf[2] << 8) | baro_amsys_i2c_trans.buf[3];
baro_amsys_temp = (float)(tBaroRaw-TEMPERATURE_AMSYS_OFFSET_MIN)*TEMPERATURE_AMSYS_MAX/(float)(TEMPERATURE_AMSYS_OFFSET_MAX-TEMPERATURE_AMSYS_OFFSET_MIN)+(float)TEMPERATURE_AMSYS_MIN;
tBaroRaw = (baro_amsys_i2c_trans.buf[2] << 8) | baro_amsys_i2c_trans.buf[3];
baro_amsys_temp = (float)(tBaroRaw-TEMPERATURE_AMSYS_OFFSET_MIN)*TEMPERATURE_AMSYS_MAX/(float)(TEMPERATURE_AMSYS_OFFSET_MAX-TEMPERATURE_AMSYS_OFFSET_MIN)+(float)TEMPERATURE_AMSYS_MIN;
#endif
// Check if this is valid altimeter
if (pBaroRaw == 0)
baro_amsys_valid = FALSE;
else
baro_amsys_valid = TRUE;
// Check if this is valid altimeter
if (pBaroRaw == 0)
baro_amsys_valid = FALSE;
else
baro_amsys_valid = TRUE;
baro_amsys_adc = pBaroRaw;
// Continue only if a new altimeter value was received
//if (baro_amsys_valid && GpsFixValid()) {
if (baro_amsys_valid) {
//Cut RAW Min and Max
if (pBaroRaw < BARO_AMSYS_OFFSET_MIN)
pBaroRaw = BARO_AMSYS_OFFSET_MIN;
if (pBaroRaw > BARO_AMSYS_OFFSET_MAX)
pBaroRaw = BARO_AMSYS_OFFSET_MAX;
// Continue only if a new altimeter value was received
//if (baro_amsys_valid && GpsFixValid()) {
if (baro_amsys_valid) {
//Cut RAW Min and Max
if (pBaroRaw < BARO_AMSYS_OFFSET_MIN)
pBaroRaw = BARO_AMSYS_OFFSET_MIN;
if (pBaroRaw > BARO_AMSYS_OFFSET_MAX)
pBaroRaw = BARO_AMSYS_OFFSET_MAX;
//Convert to pressure
baro_amsys_p = (float)(pBaroRaw-BARO_AMSYS_OFFSET_MIN)*BARO_AMSYS_MAX_PRESSURE/(float)(BARO_AMSYS_OFFSET_MAX-BARO_AMSYS_OFFSET_MIN);
if (!baro_amsys_offset_init) {
--baro_amsys_cnt;
// Check if averaging completed
if (baro_amsys_cnt == 0) {
// Calculate average
baro_amsys_offset = (float)(baro_amsys_offset_tmp / BARO_AMSYS_OFFSET_NBSAMPLES_AVRG);
ref_alt_init = GROUND_ALT;
baro_amsys_offset_init = TRUE;
//Convert to pressure
baro_amsys_p = (float)(pBaroRaw-BARO_AMSYS_OFFSET_MIN)*BARO_AMSYS_MAX_PRESSURE/(float)(BARO_AMSYS_OFFSET_MAX-BARO_AMSYS_OFFSET_MIN);
if (!baro_amsys_offset_init) {
--baro_amsys_cnt;
// Check if averaging completed
if (baro_amsys_cnt == 0) {
// Calculate average
baro_amsys_offset = (float)(baro_amsys_offset_tmp / BARO_AMSYS_OFFSET_NBSAMPLES_AVRG);
ref_alt_init = GROUND_ALT;
baro_amsys_offset_init = TRUE;
// hight over Sea level at init point
//baro_amsys_offset_altitude = 288.15 / 0.0065 * (1 - pow((baro_amsys_p)/1013.25 , 1/5.255));
}
// Check if averaging needs to continue
else if (baro_amsys_cnt <= BARO_AMSYS_OFFSET_NBSAMPLES_AVRG)
baro_amsys_offset_tmp += baro_amsys_p;
// hight over Sea level at init point
//baro_amsys_offset_altitude = 288.15 / 0.0065 * (1 - pow((baro_amsys_p)/1013.25 , 1/5.255));
}
// Check if averaging needs to continue
else if (baro_amsys_cnt <= BARO_AMSYS_OFFSET_NBSAMPLES_AVRG)
baro_amsys_offset_tmp += baro_amsys_p;
baro_amsys_altitude = 0.0;
baro_amsys_altitude = 0.0;
}
else {
// Lowpassfiltering and convert pressure to altitude
baro_amsys_altitude = baro_filter * baro_old + (1 - baro_filter) * (baro_amsys_offset-baro_amsys_p)/(1.2041*9.81);
baro_old = baro_amsys_altitude;
//New value available
}
baro_amsys_abs_altitude=baro_amsys_altitude+ref_alt_init;
} /*else {
baro_amsys_abs_altitude = 0.0;
}*/
}
else {
// Lowpassfiltering and convert pressure to altitude
baro_amsys_altitude = baro_filter * baro_old + (1 - baro_filter) * (baro_amsys_offset-baro_amsys_p)*baro_scale/(1.2041*9.81);
baro_old = baro_amsys_altitude;
//New value available
}
baro_amsys_abs_altitude=baro_amsys_altitude+ref_alt_init;
} /*else {
baro_amsys_abs_altitude = 0.0;
}*/
// Transaction has been read
baro_amsys_i2c_trans.status = I2CTransDone;
#ifdef SENSOR_SYNC_SEND
DOWNLINK_SEND_AMSYS_BARO(DefaultChannel, DefaultDevice, &pBaroRaw, &baro_amsys_p, &baro_amsys_offset, &ref_alt_init, &baro_amsys_abs_altitude, &baro_amsys_altitude, &baro_amsys_temp)
#else
RunOnceEvery(10, DOWNLINK_SEND_AMSYS_BARO(DefaultChannel, DefaultDevice, &pBaroRaw, &baro_amsys_p, &baro_amsys_offset, &ref_alt_init, &baro_amsys_abs_altitude, &baro_amsys_altitude, &baro_amsys_temp));
#endif
// Transaction has been read
baro_amsys_i2c_trans.status = I2CTransDone;
}
sw/airborne/modules/sensors/baro_amsys.h
+3 -3
View File
@@ -33,10 +33,10 @@
extern uint16_t baro_amsys_adc;
// extern float baro_amsys_offset;
extern bool_t baro_amsys_valid;
// extern bool_t baro_amsys_enabled;
extern bool_t baro_amsys_enabled;
extern float baro_amsys_altitude;
// extern float baro_amsys_r;
// extern float baro_amsys_sigma2;
extern float baro_amsys_r;
extern float baro_amsys_sigma2;
extern float baro_filter;
extern struct i2c_transaction baro_amsys_i2c_trans;
sw/airborne/modules/sensors/baro_ets.c
+24 -7
View File
@@ -43,6 +43,7 @@
#include "mcu_periph/i2c.h"
#include "state.h"
#include <math.h>
#include "mcu_periph/sys_time.h"
#include "subsystems/nav.h"
@@ -50,12 +51,12 @@
#include "subsystems/gps.h"
#endif
#ifdef BARO_ETS_TELEMETRY
#ifdef BARO_ETS_SYNC_SEND
#include "mcu_periph/uart.h"
#include "messages.h"
#include "subsystems/datalink/downlink.h"
#endif //BARO_ETS_TELEMETRY
#endif //BARO_ETS_SYNC_SEND
#define BARO_ETS_ADDR 0xE8
#define BARO_ETS_REG 0x07
@@ -72,6 +73,13 @@
#ifndef BARO_ETS_I2C_DEV
#define BARO_ETS_I2C_DEV i2c0
#endif
PRINT_CONFIG_VAR(BARO_ETS_I2C_DEV)
/** delay in seconds until sensor is read after startup */
#ifndef BARO_ETS_START_DELAY
#define BARO_ETS_START_DELAY 0.2
#endif
PRINT_CONFIG_VAR(BARO_ETS_START_DELAY)
// Global variables
uint16_t baro_ets_adc;
@@ -85,16 +93,18 @@ float baro_ets_sigma2;
struct i2c_transaction baro_ets_i2c_trans;
// Local variables
bool_t baro_ets_offset_init;
bool_t baro_ets_offset_init;
uint32_t baro_ets_offset_tmp;
uint16_t baro_ets_cnt;
uint32_t baro_ets_delay_time;
bool_t baro_ets_delay_done;
void baro_ets_init( void ) {
baro_ets_adc = 0;
baro_ets_altitude = 0.0;
baro_ets_offset = 0;
baro_ets_offset_tmp = 0;
baro_ets_valid = TRUE;
baro_ets_valid = FALSE;
baro_ets_offset_init = FALSE;
baro_ets_enabled = TRUE;
baro_ets_cnt = BARO_ETS_OFFSET_NBSAMPLES_INIT + BARO_ETS_OFFSET_NBSAMPLES_AVRG;
@@ -102,15 +112,22 @@ void baro_ets_init( void ) {
baro_ets_sigma2 = BARO_ETS_SIGMA2;
baro_ets_i2c_trans.status = I2CTransDone;
baro_ets_delay_done = FALSE;
SysTimeTimerStart(baro_ets_delay_time);
}
void baro_ets_read_periodic( void ) {
// Initiate next read
#ifndef SITL
if (!baro_ets_delay_done) {
if (SysTimeTimer(baro_ets_delay_time) < USEC_OF_SEC(BARO_ETS_START_DELAY)) return;
else baro_ets_delay_done = TRUE;
}
if (baro_ets_i2c_trans.status == I2CTransDone)
i2c_receive(&BARO_ETS_I2C_DEV, &baro_ets_i2c_trans, BARO_ETS_ADDR, 2);
#else // SITL
/* fake an offset so sim works for under hmsl as well */
/* fake an offset so sim works as well */
if (!baro_ets_offset_init) {
baro_ets_offset = 12400;
baro_ets_offset_init = TRUE;
@@ -120,7 +137,7 @@ void baro_ets_read_periodic( void ) {
baro_ets_valid = TRUE;
#endif
#ifdef BARO_ETS_TELEMETRY
#ifdef BARO_ETS_SYNC_SEND
DOWNLINK_SEND_BARO_ETS(DefaultChannel, DefaultDevice, &baro_ets_adc, &baro_ets_offset, &baro_ets_altitude);
#endif
}
@@ -158,7 +175,7 @@ void baro_ets_read_event( void ) {
if (baro_ets_offset_init) {
baro_ets_altitude = ground_alt + BARO_ETS_SCALE * (float)(baro_ets_offset-baro_ets_adc);
// New value available
#ifdef BARO_ETS_TELEMETRY
#ifdef BARO_ETS_SYNC_SEND
DOWNLINK_SEND_BARO_ETS(DefaultChannel, DefaultDevice, &baro_ets_adc, &baro_ets_offset, &baro_ets_altitude);
#endif
} else {
sw/airborne/modules/sensors/baro_ms5611_i2c.c
+2 -1
View File
@@ -52,6 +52,7 @@ uint8_t ms5611_status;
uint16_t ms5611_c[PROM_NB];
uint32_t ms5611_d1, ms5611_d2;
int32_t prom_cnt;
int64_t baroms;
float fbaroms, ftempms;
float baro_ms5611_alt;
bool_t baro_ms5611_enabled;
@@ -190,7 +191,7 @@ void baro_ms5611_event( void ) {
break;
case MS5611_ADC_D2: {
int64_t dt, baroms, tempms, off, sens, t2, off2, sens2;
int64_t dt, tempms, off, sens, t2, off2, sens2;
/* read D2 (temperature) */
ms5611_d2 = (ms5611_trans.buf[0] << 16) |
(ms5611_trans.buf[1] << 8) |
sw/airborne/modules/sensors/baro_ms5611_i2c.h
+6 -5
View File
@@ -20,6 +20,7 @@ extern bool_t baro_ms5611_valid;
extern bool_t baro_ms5611_enabled;
extern float baro_ms5611_r;
extern float baro_ms5611_sigma2;
extern int64_t baroms;
enum ms5611_stat{
MS5611_UNINIT,
@@ -35,11 +36,11 @@ enum ms5611_stat{
MS5611_ADC_D2
};
void baro_ms5611_init(void);
void baro_ms5611_periodic(void);
void baro_ms5611_d1(void);
void baro_ms5611_d2(void);
void baro_ms5611_event(void);
extern void baro_ms5611_init(void);
extern void baro_ms5611_periodic(void);
extern void baro_ms5611_d1(void);
extern void baro_ms5611_d2(void);
extern void baro_ms5611_event(void);
#define BaroMs5611Update(_b) { if (baro_ms5611_valid) { _b = baro_ms5611_alt; baro_ms5611_valid = FALSE; } }
sw/airborne/subsystems/actuators/motor_mixing.c
+8 -4
View File
@@ -172,13 +172,17 @@ void motor_mixing_run(bool_t motors_on, bool_t override_on, pprz_t in_cmd[] ) {
if (min_cmd < MOTOR_MIXING_MIN_MOTOR && max_cmd > MOTOR_MIXING_MAX_MOTOR)
motor_mixing.nb_failure++;
if (min_cmd < MOTOR_MIXING_MIN_MOTOR) {
int32_t saturation_offset = MOTOR_MIXING_MIN_MOTOR - min_cmd;
/* In case of both min and max saturation, only lower the throttle
* instead of applying both. This should prevent your quad shooting up,
* but it might loose altitude in case of such a saturation failure.
*/
if (max_cmd > MOTOR_MIXING_MAX_MOTOR) {
int32_t saturation_offset = MOTOR_MIXING_MAX_MOTOR - max_cmd;
BoundAbs(saturation_offset, MOTOR_MIXING_MAX_SATURATION_OFFSET);
offset_commands(saturation_offset);
}
if (max_cmd > MOTOR_MIXING_MAX_MOTOR) {
int32_t saturation_offset = MOTOR_MIXING_MAX_MOTOR - max_cmd;
else if (min_cmd < MOTOR_MIXING_MIN_MOTOR) {
int32_t saturation_offset = MOTOR_MIXING_MIN_MOTOR - min_cmd;
BoundAbs(saturation_offset, MOTOR_MIXING_MAX_SATURATION_OFFSET);
offset_commands(saturation_offset);
}
sw/airborne/subsystems/gps/gps_ardrone2.c
+7
View File
@@ -36,6 +36,7 @@ void gps_impl_init( void ) {
}
void gps_ardrone2_parse(navdata_gps_t *navdata_gps) {
int i;
// Set the lla double struct from the navdata
struct LlaCoor_d gps_lla_d;
gps_lla_d.lat = RadOfDeg(navdata_gps->lat);
@@ -51,6 +52,12 @@ void gps_ardrone2_parse(navdata_gps_t *navdata_gps) {
LLA_BFP_OF_REAL(gps.lla_pos, gps_lla_d);
// TODO: parse other stuff
gps.nb_channels = GPS_NB_CHANNELS;
for(i = 0; i < GPS_NB_CHANNELS; i++) {
gps.svinfos[i].svid = navdata_gps->channels[i].sat;
gps.svinfos[i].cno = navdata_gps->channels[i].cn0;
}
// Check if we have a fix TODO: check if 2D or 3D fix?
if (navdata_gps->gps_state == 1)
sw/airborne/subsystems/gps/gps_ardrone2.h
+1 -1
View File
@@ -30,7 +30,7 @@
#include "boards/ardrone/at_com.h"
//#define GPS_NB_CHANNELS 12 // TODO: Get channels out of packet
#define GPS_NB_CHANNELS 12
extern bool_t gps_ardrone2_available;
/*
sw/airborne/subsystems/imu/imu_aspirin2.c
-346
View File
@@ -1,346 +0,0 @@
/*
* Copyright (C) 2012 Christophe DeWagter
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
#include "subsystems/imu.h"
#include "led.h"
#include "mcu_periph/spi.h"
// Peripherials
#include "peripherals/mpu60x0_regs.h"
#include "peripherals/hmc58xx_regs.h"
#include "peripherals/ms5611.h"
#ifndef MPU6000_SLAVE_IDX
#define MPU6000_SLAVE_IDX SPI_SLAVE2
#endif
#ifndef MPU6000_SPI_DEV
#define MPU6000_SPI_DEV spi2
#endif
/* HMC58XX default conf */
#ifndef HMC58XX_DO
#define HMC58XX_DO 0x6 // Data Output Rate (6 -> 50Hz with HMC5843, 75Hz with HMC5883)
#endif
#ifndef HMC58XX_MS
#define HMC58XX_MS 0x0 // Measurement configuration
#endif
#ifndef HMC58XX_GN
#define HMC58XX_GN 0x1 // Gain configuration (1 -> +- 1 Gauss)
#endif
#ifndef HMC58XX_MD
#define HMC58XX_MD 0x0 // Continious measurement mode
#endif
#define HMC58XX_CRA ((HMC58XX_DO<<2)|(HMC58XX_MS))
#define HMC58XX_CRB (HMC58XX_GN<<5)
struct ImuAspirin2 imu_aspirin2;
struct spi_transaction aspirin2_mpu60x0;
// initialize peripherals
static void mpu_configure(void);
static void trans_cb( struct spi_transaction *trans );
void imu_impl_init(void) {
aspirin2_mpu60x0.select = SPISelectUnselect;
aspirin2_mpu60x0.cpol = SPICpolIdleHigh;
aspirin2_mpu60x0.cpha = SPICphaEdge2;
aspirin2_mpu60x0.dss = SPIDss8bit;
aspirin2_mpu60x0.bitorder = SPIMSBFirst;
aspirin2_mpu60x0.cdiv = SPIDiv64;
aspirin2_mpu60x0.slave_idx = MPU6000_SLAVE_IDX;
aspirin2_mpu60x0.output_length = IMU_ASPIRIN_BUFFER_LEN;
aspirin2_mpu60x0.input_length = IMU_ASPIRIN_BUFFER_LEN;
aspirin2_mpu60x0.after_cb = trans_cb;
imu_aspirin2.status = Aspirin2StatusUninit;
imu_aspirin2.imu_available = FALSE;
aspirin2_mpu60x0.input_buf = &imu_aspirin2.input_buf_p[0];
aspirin2_mpu60x0.output_buf = &imu_aspirin2.output_buf_p[0];
}
void imu_periodic(void)
{
if (imu_aspirin2.status == Aspirin2StatusUninit) {
mpu_configure();
imu_aspirin2.status = Aspirin2StatusIdle;
aspirin2_mpu60x0.output_length = 22;
aspirin2_mpu60x0.input_length = 22;
aspirin2_mpu60x0.output_buf[0] = MPU60X0_REG_INT_STATUS + MPU60X0_SPI_READ;
for (int i=1; i<aspirin2_mpu60x0.output_length; i++) {
aspirin2_mpu60x0.output_buf[i] = 0;
}
}
else {
spi_submit(&(MPU6000_SPI_DEV), &aspirin2_mpu60x0);
}
}
static void trans_cb(struct spi_transaction *trans __attribute__ ((unused))) {
if ( imu_aspirin2.status != Aspirin2StatusUninit ) {
imu_aspirin2.imu_available = TRUE;
}
}
static inline void mpu_set(uint8_t _reg, uint8_t _val)
{
aspirin2_mpu60x0.output_buf[0] = _reg;
aspirin2_mpu60x0.output_buf[1] = _val;
spi_submit(&(MPU6000_SPI_DEV), &aspirin2_mpu60x0);
// FIXME: no busy waiting! if really needed add a timeout!!!!
while(aspirin2_mpu60x0.status != SPITransSuccess);
}
static inline void mpu_wait_slave4_ready(void)
{
uint8_t ret = 0x80;
while (ret & 0x80)
{
aspirin2_mpu60x0.output_buf[0] = MPU60X0_REG_I2C_SLV4_CTRL | MPU60X0_SPI_READ ;
aspirin2_mpu60x0.output_buf[1] = 0;
spi_submit(&(MPU6000_SPI_DEV), &aspirin2_mpu60x0);
// FIXME: no busy waiting! if really needed add a timeout!!!!
while(aspirin2_mpu60x0.status != SPITransSuccess);
ret = aspirin2_mpu60x0.input_buf[1];
}
}
static void mpu_configure(void)
{
aspirin2_mpu60x0.output_length = 2;
aspirin2_mpu60x0.input_length = 2;
///////////////////
// Reset the MPU
mpu_set( MPU60X0_REG_PWR_MGMT_1,
0x01 << 7); // -device reset
mpu_set( MPU60X0_REG_USER_CTRL,
(1 << 2) | // Trigger a FIFO_RESET
(1 << 1) | // Trigger a I2C_MST_RESET
(1 << 0) ); // Trigger a SIG_COND_RESET
///////////////////
// Configure power:
// MPU60X0_REG_PWR_MGMT_1
mpu_set( MPU60X0_REG_PWR_MGMT_1,
0x01); // -switch to gyroX clock
// Wait for the new clock to stabilize.
// FIXME: This must not be a delay!
// It should be done using the MPU-6000 interrupt!
{for (int i = 0; i < 1000000; i++) { asm("nop"); }}
// MPU60X0_REG_PWR_MGMT_2: Nothing should be in standby: default OK
// -No standby and no wake timer
/////////////////////////
// Measurement Settings
#if PERIODIC_FREQUENCY == 60
// Accelerometer: Bandwidth 44Hz, Delay 4.9ms
// Gyroscope: Bandwidth 42Hz, Delay 4.8ms sampling 1Khz
# define MPU_DIG_FILTER 3
// -100Hz output = 1kHz / (9 + 1)
# define MPU_SMPLRT_DIV 9
#else
# if PERIODIC_FREQUENCY == 120
// Accelerometer: Bandwidth 44Hz, Delay 4.9ms
// Gyroscope: Bandwidth 42Hz, Delay 4.8ms sampling 1Khz
# define MPU_DIG_FILTER 3
// -100Hz output = 1kHz / (9 + 1)
# define MPU_SMPLRT_DIV 9
# else
# if PERIODIC_FREQUENCY == 512
// Accelerometer: Bandwidth 260Hz, Delay 0ms
// Gyroscope: Bandwidth 256Hz, Delay 0.89ms sampling 8Khz
# define MPU_DIG_FILTER 0
// -500Hz output = 1kHz / (1 + 1)
# define MPU_SMPLRT_DIV 1
# else
# error PERIODIC_FREQUENCY should be either 60Hz, 120Hz or 512Hz. Otherwise manually fix the sensor rates
# endif
# endif
#endif
aspirin2_mpu60x0.output_buf[1] = (2 << 3) | (MPU_DIG_FILTER << 0);
spi_submit(&(MPU6000_SPI_DEV), &aspirin2_mpu60x0);
mpu_set( MPU60X0_REG_CONFIG,
(2 << 3) | // Fsync / ext sync on gyro X (bit 3->6)
(MPU_DIG_FILTER << 0) ); // Low-Pass Filter
// MPU60X0_REG_SMPLRT_DIV
mpu_set( MPU60X0_REG_SMPLRT_DIV, MPU_SMPLRT_DIV);
// MPU60X0_REG_GYRO_CONFIG
mpu_set( MPU60X0_REG_GYRO_CONFIG,
(3 << 3) ); // -2000deg/sec
// MPU60X0_REG_ACCEL_CONFIG
mpu_set( MPU60X0_REG_ACCEL_CONFIG,
(0 << 0) | // No HPFL
(3 << 3) ); // Full Scale = 16g
#ifndef MPU6000_NO_SLAVES
PRINT_CONFIG_MSG("Reading MPU slaves")
/////////////////////////////////////
// SPI Slave Configuration Section
// Power the Aux I2C Circuit:
// MPU60X0_REG_AUX_VDDIO = 0 (good on startup): (0 << 7); // MPU6000: 0=Vdd. MPU6050 : 0=VLogic 1=Vdd
// MPU60X0_REG_USER_CTRL:
mpu_set( MPU60X0_REG_USER_CTRL,
(1 << 5) | // I2C_MST_EN: Enable Aux I2C Master Mode
(1 << 4) | // I2C_IF_DIS: Disable I2C on primary interface
(0 << 1) ); // Trigger a I2C_MST_RESET
// Enable the aux i2c
mpu_set( MPU60X0_REG_I2C_MST_CTRL,
(0 << 7) | // no multimaster
(0 << 6) | // do not delay IRQ waiting for all external slaves
(0 << 5) | // no slave 3 FIFO
(0 << 4) | // restart or stop/start from one slave to another: read -> write is always stop/start
(8 << 0) ); // 0=348kHz 8=256kHz, 9=500kHz
mpu_set( MPU60X0_REG_I2C_MST_DELAY,
(0 << 2) | // No Delay Slave 2
(1 << 3) ); // Delay Slave 3
#if defined IMU_ASPIRIN_VERSION_2_1 && USE_IMU_ASPIRIN2_BARO_SLAVE
// MS5611 Send Reset
mpu_set( MPU60X0_REG_I2C_SLV4_ADDR, (MS5611_ADDR0));
mpu_set( MPU60X0_REG_I2C_SLV4_DO, MS5611_REG_RESET);
mpu_set( MPU60X0_REG_I2C_SLV4_CTRL,
(1 << 7) | // Slave 4 enable
(0 << 6) | // Byte Swap
(1 << 5) | // Reg_Dis: do not write the register, just the data
(0 << 0) ); // Full Speed
mpu_wait_slave4_ready();
// Wait at least 2.8ms
#endif // read MS5611 as MPU slave
// HMC5883 Magnetometer Configuration
mpu_set( MPU60X0_REG_I2C_SLV4_ADDR, (HMC58XX_ADDR >> 1));
mpu_set( MPU60X0_REG_I2C_SLV4_REG, HMC58XX_REG_CFGA);
mpu_set( MPU60X0_REG_I2C_SLV4_DO, HMC58XX_CRA);
mpu_set( MPU60X0_REG_I2C_SLV4_CTRL,
(1 << 7) | // Slave 4 enable
(0 << 6) | // Byte Swap
(0 << 0) ); // Full Speed
mpu_wait_slave4_ready();
mpu_set( MPU60X0_REG_I2C_SLV4_ADDR, (HMC58XX_ADDR >> 1));
mpu_set( MPU60X0_REG_I2C_SLV4_REG, HMC58XX_REG_CFGB);
mpu_set( MPU60X0_REG_I2C_SLV4_DO, HMC58XX_CRB);
mpu_set( MPU60X0_REG_I2C_SLV4_CTRL,
(1 << 7) | // Slave 4 enable
(0 << 6) | // Byte Swap
(0 << 0) ); // Full Speed
mpu_wait_slave4_ready();
mpu_set( MPU60X0_REG_I2C_SLV4_ADDR, (HMC58XX_ADDR >> 1));
mpu_set( MPU60X0_REG_I2C_SLV4_REG, HMC58XX_REG_MODE);
mpu_set( MPU60X0_REG_I2C_SLV4_DO, HMC58XX_MD);
mpu_set( MPU60X0_REG_I2C_SLV4_CTRL,
(1 << 7) | // Slave 4 enable
(0 << 6) | // Byte Swap
(3 << 0) ); // From now on a delayed rate of 1/4 is defined...
// HMC5883 Reading:
// a) write hmc-register to HMC
// b) read 6 bytes from HMC
mpu_set( MPU60X0_REG_I2C_SLV0_ADDR, (HMC58XX_ADDR >> 1) | MPU60X0_SPI_READ);
mpu_set( MPU60X0_REG_I2C_SLV0_REG, HMC58XX_REG_DATXM);
// Put the enable command as last.
mpu_set( MPU60X0_REG_I2C_SLV0_CTRL,
(1 << 7) | // Slave 0 enable
(0 << 6) | // Byte Swap
(6 << 0) ); // Read 6 bytes
// Slave 0 Control:
#if defined IMU_ASPIRIN_VERSION_2_1 && USE_IMU_ASPIRIN2_BARO_SLAVE
PRINT_CONFIG_MSG("Reading the MS5611 as MPU slave")
/*
// Read MS5611 Calibration
mpu_set( MPU60X0_REG_I2C_SLV1_ADDR, (MS5611_ADDR0) | MPU60X0_SPI_READ);
mpu_set( MPU60X0_REG_I2C_SLV1_REG, MS5611_REG_ADCREAD);
// Put the enable command as last.
mpu_set( MPU60X0_REG_I2C_SLV1_CTRL,
(1 << 7) | // Slave 1 enable
(0 << 6) | // Byte Swap
(3 << 0) ); // Read 6 bytes
*/
// Full Rate Request For Pressure
mpu_set( MPU60X0_REG_I2C_SLV2_ADDR, (MS5611_ADDR0));
mpu_set( MPU60X0_REG_I2C_SLV2_DO, 0x48);
// Put the enable command as last.
mpu_set( MPU60X0_REG_I2C_SLV2_CTRL,
(1 << 7) | // Slave 2 enable
(0 << 6) | // Byte Swap
(1 << 5) | // Rig Dis: Write Only
(1 << 0) ); // Write 1 byte
// Reduced rate request For Temperature: Overwrites the Pressure Request
mpu_set( MPU60X0_REG_I2C_SLV3_ADDR, (MS5611_ADDR0));
mpu_set( MPU60X0_REG_I2C_SLV3_DO, 0x58);
// Put the enable command as last.
mpu_set( MPU60X0_REG_I2C_SLV3_CTRL,
(1 << 7) | // Slave 2 enable
(0 << 6) | // Byte Swap
(1 << 5) | // Rig Dis: Write Only
(1 << 0) ); // Write 1 byte
mpu_set( MPU60X0_REG_I2C_SLV1_ADDR, (MS5611_ADDR0) | MPU60X0_SPI_READ);
mpu_set( MPU60X0_REG_I2C_SLV1_REG, MS5611_REG_ADCREAD);
// Put the enable command as last.
mpu_set( MPU60X0_REG_I2C_SLV1_CTRL,
(1 << 7) | // Slave 1 enable
(0 << 6) | // Byte Swap
(3 << 0) ); // Read 6 bytes
#endif // read MS5611 as MPU slave
#endif
}
sw/airborne/subsystems/imu/imu_aspirin2.h
-167
View File
@@ -1,167 +0,0 @@
/*
* Copyright (C) 2012 Christophe DeWagter
*
* 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 IMU_ASPIRIN_2_H
#define IMU_ASPIRIN_2_H
#include "generated/airframe.h"
#include "subsystems/imu.h"
#if defined IMU_ASPIRIN_VERSION_2_1 || defined IMU_ASPIRIN_VERSION_2_2
#if !defined IMU_MAG_X_SIGN & !defined IMU_MAG_Y_SIGN & !defined IMU_MAG_Z_SIGN
#define IMU_MAG_X_SIGN 1
#define IMU_MAG_Y_SIGN 1
#define IMU_MAG_Z_SIGN 1
#endif
#endif
#if !defined IMU_GYRO_P_SIGN & !defined IMU_GYRO_Q_SIGN & !defined IMU_GYRO_R_SIGN
#define IMU_GYRO_P_SIGN 1
#define IMU_GYRO_Q_SIGN 1
#define IMU_GYRO_R_SIGN 1
#endif
#if !defined IMU_ACCEL_X_SIGN & !defined IMU_ACCEL_Y_SIGN & !defined IMU_ACCEL_Z_SIGN
#define IMU_ACCEL_X_SIGN 1
#define IMU_ACCEL_Y_SIGN 1
#define IMU_ACCEL_Z_SIGN 1
#endif
/** default gyro sensitivy and neutral from the datasheet
* MPU60X0 has 16.4 LSB/(deg/s) at 2000deg/s range
* sens = 1/16.4 * pi/180 * 2^INT32_RATE_FRAC
* sens = 1/16.4 * pi/180 * 4096 = 4.359066229
*/
#if !defined IMU_GYRO_P_SENS & !defined IMU_GYRO_Q_SENS & !defined IMU_GYRO_R_SENS
#define IMU_GYRO_P_SENS 4.359
#define IMU_GYRO_P_SENS_NUM 4359
#define IMU_GYRO_P_SENS_DEN 1000
#define IMU_GYRO_Q_SENS 4.359
#define IMU_GYRO_Q_SENS_NUM 4359
#define IMU_GYRO_Q_SENS_DEN 1000
#define IMU_GYRO_R_SENS 4.359
#define IMU_GYRO_R_SENS_NUM 4359
#define IMU_GYRO_R_SENS_DEN 1000
#endif
#if !defined IMU_GYRO_P_NEUTRAL & !defined IMU_GYRO_Q_NEUTRAL & !defined IMU_GYRO_R_NEUTRAL
#define IMU_GYRO_P_NEUTRAL 0
#define IMU_GYRO_Q_NEUTRAL 0
#define IMU_GYRO_R_NEUTRAL 0
#endif
/** default accel sensitivy from the datasheet
* MPU60X0 has 2048 LSB/g
* fixed point sens: 9.81 [m/s^2] / 2048 [LSB/g] * 2^INT32_ACCEL_FRAC
* sens = 9.81 / 2048 * 1024 = 4.905
*/
#if !defined IMU_ACCEL_X_SENS & !defined IMU_ACCEL_Y_SENS & !defined IMU_ACCEL_Z_SENS
#define IMU_ACCEL_X_SENS 4.905
#define IMU_ACCEL_X_SENS_NUM 4905
#define IMU_ACCEL_X_SENS_DEN 1000
#define IMU_ACCEL_Y_SENS 4.905
#define IMU_ACCEL_Y_SENS_NUM 4905
#define IMU_ACCEL_Y_SENS_DEN 1000
#define IMU_ACCEL_Z_SENS 4.905
#define IMU_ACCEL_Z_SENS_NUM 4905
#define IMU_ACCEL_Z_SENS_DEN 1000
#endif
#if !defined IMU_ACCEL_X_NEUTRAL & !defined IMU_ACCEL_Y_NEUTRAL & !defined IMU_ACCEL_Z_NEUTRAL
#define IMU_ACCEL_X_NEUTRAL 0
#define IMU_ACCEL_Y_NEUTRAL 0
#define IMU_ACCEL_Z_NEUTRAL 0
#endif
enum Aspirin2Status
{ Aspirin2StatusUninit,
Aspirin2StatusIdle,
Aspirin2StatusReading
};
#define IMU_ASPIRIN_BUFFER_LEN 32
struct ImuAspirin2 {
volatile enum Aspirin2Status status;
volatile uint8_t imu_available;
volatile uint8_t input_buf_p[IMU_ASPIRIN_BUFFER_LEN];
volatile uint8_t output_buf_p[IMU_ASPIRIN_BUFFER_LEN];
};
extern struct ImuAspirin2 imu_aspirin2;
static inline int imu_from_buff(volatile uint8_t *buf)
{
int32_t x, y, z, p, q, r, Mx, My, Mz;
#define MPU_OFFSET_STATUS 1
if (!(buf[MPU_OFFSET_STATUS] & 0x01)) {
return 0;
}
#define MPU_OFFSET_GYRO 10
p = (int16_t) ((buf[0+MPU_OFFSET_GYRO] << 8) | buf[1+MPU_OFFSET_GYRO]);
q = (int16_t) ((buf[2+MPU_OFFSET_GYRO] << 8) | buf[3+MPU_OFFSET_GYRO]);
r = (int16_t) ((buf[4+MPU_OFFSET_GYRO] << 8) | buf[5+MPU_OFFSET_GYRO]);
#define MPU_OFFSET_ACC 2
x = (int16_t) ((buf[0+MPU_OFFSET_ACC] << 8) | buf[1+MPU_OFFSET_ACC]);
y = (int16_t) ((buf[2+MPU_OFFSET_ACC] << 8) | buf[3+MPU_OFFSET_ACC]);
z = (int16_t) ((buf[4+MPU_OFFSET_ACC] << 8) | buf[5+MPU_OFFSET_ACC]);
#define MPU_OFFSET_MAG 16
Mx = (int16_t) ((buf[0+MPU_OFFSET_MAG] << 8) | buf[1+MPU_OFFSET_MAG]);
My = (int16_t) ((buf[2+MPU_OFFSET_MAG] << 8) | buf[3+MPU_OFFSET_MAG]);
Mz = (int16_t) ((buf[4+MPU_OFFSET_MAG] << 8) | buf[5+MPU_OFFSET_MAG]);
#ifdef LISA_M_LONGITUDINAL_X
RATES_ASSIGN(imu.gyro_unscaled, q, -p, r);
VECT3_ASSIGN(imu.accel_unscaled, y, -x, z);
VECT3_ASSIGN(imu.mag_unscaled, -Mx, -Mz, My);
#else
RATES_ASSIGN(imu.gyro_unscaled, p, q, r);
VECT3_ASSIGN(imu.accel_unscaled, x, y, z);
VECT3_ASSIGN(imu.mag_unscaled, Mz, -Mx, My);
#endif
return 1;
}
static inline void imu_aspirin2_event(void (* _gyro_handler)(void), void (* _accel_handler)(void), void (* _mag_handler)(void))
{
if (imu_aspirin2.status == Aspirin2StatusUninit) return;
if (imu_aspirin2.imu_available) {
imu_aspirin2.imu_available = FALSE;
if (imu_from_buff(imu_aspirin2.input_buf_p)) {
_gyro_handler();
_accel_handler();
_mag_handler();
}
}
}
#define ImuEvent(_gyro_handler, _accel_handler, _mag_handler) { \
imu_aspirin2_event(_gyro_handler, _accel_handler, _mag_handler); \
}
#endif /* IMU_ASPIRIN_2_H */
sw/airborne/subsystems/imu/imu_aspirin_2_spi.c
+5 -4
View File
@@ -145,10 +145,11 @@ void imu_aspirin2_event(void)
{
mpu60x0_spi_event(&imu_aspirin2.mpu);
if (imu_aspirin2.mpu.data_available) {
/* HMC5883 has xzy order of axes in returned data */
struct Int32Vect3 mag;
mag.x = Int16FromBuf(imu_aspirin2.mpu.data_ext, 0);
mag.y = Int16FromBuf(imu_aspirin2.mpu.data_ext, 2);
mag.z = Int16FromBuf(imu_aspirin2.mpu.data_ext, 4);
mag.z = Int16FromBuf(imu_aspirin2.mpu.data_ext, 2);
mag.y = Int16FromBuf(imu_aspirin2.mpu.data_ext, 4);
#ifdef LISA_M_LONGITUDINAL_X
RATES_ASSIGN(imu.gyro_unscaled,
imu_aspirin2.mpu.data_rates.rates.q,
@@ -158,11 +159,11 @@ void imu_aspirin2_event(void)
imu_aspirin2.mpu.data_accel.vect.y,
-imu_aspirin2.mpu.data_accel.vect.x,
imu_aspirin2.mpu.data_accel.vect.z);
VECT3_ASSIGN(imu.mag_unscaled, -mag.x, -mag.z, mag.y);
VECT3_ASSIGN(imu.mag_unscaled, -mag.x, -mag.y, mag.z);
#else
RATES_COPY(imu.gyro_unscaled, imu_aspirin2.mpu.data_rates.rates);
VECT3_COPY(imu.accel_unscaled, imu_aspirin2.mpu.data_accel.vect);
VECT3_ASSIGN(imu.mag_unscaled, mag.z, -mag.x, mag.y)
VECT3_ASSIGN(imu.mag_unscaled, mag.y, -mag.x, mag.z)
#endif
imu_aspirin2.mpu.data_available = FALSE;
imu_aspirin2.gyro_valid = TRUE;
sw/airborne/subsystems/ins/ins_alt_float.c
+6
View File
@@ -192,6 +192,12 @@ void alt_kalman(float z_meas) {
R = baro_ms5611_r;
SIGMA2 = baro_ms5611_sigma2;
} else
#elif USE_BARO_AMSYS
if (baro_amsys_enabled) {
DT = BARO_AMSYS_DT;
R = baro_amsys_r;
SIGMA2 = baro_amsys_sigma2;
} else
#elif USE_BARO_BMP
if (baro_bmp_enabled) {
DT = BARO_BMP_DT;
sw/airborne/subsystems/ins/ins_alt_float.h
+4
View File
@@ -52,6 +52,10 @@
#include "modules/sensors/baro_ms5611_i2c.h"
#endif
#if USE_BARO_AMSYS
#include "modules/sensors/baro_amsys.h"
#endif
extern int32_t ins_qfe;
extern float ins_baro_alt;
extern bool_t ins_baro_initialised;
sw/extras/chdk/pictuav.lua
+168
View File
@@ -0,0 +1,168 @@
--[[
@title PictUAV
@param d Display off frame 0=never
@default d 0
--]]
-- Developed on IXUS 230 HS. Other cameras should work as well, but some errors or crashes are possible
-- also considering that some firmwares still have CHDK bugs.
--
-- Other settings in the camera that you should try to manipulate to reduce the time between taking pictures:
-- CHDK menu
-- Extra Photo overrides
-- Disable overrides : Off
-- Do not override shutter speed ( I allow the camera to determine exposure time, but feel free to experiment)
-- ND filter state: Out (removes ND filter to allow more light to fall on the sensor )
-- Override subj. dist. value: 65535. (good to keep it there)
-- Do not save RAW images, they take longer
--
-- Camera menu
-- Use P mode, certainly not automatic
-- If you can hardset the focus, do this towards infinity
-- Play around with aperture. Smaller apertures are better for more sharpness, but not every camera allows you to.
-- (Some cameras have really bad behaviour on larger apertures that allow more light through, especially at edges).
-- Disable IS (plane vibrations mess up its function and increases the chance of blur)
-- Take Large images with Fine resolution.
-- Turn "Review" mode off.
-- Consider hard-setting ISO, but consider local weather. If set too high, shutter time goes up, which causes blur.
-- Blur can then also occur in areas where there is little light reflection from the earth.
--
-- How to use the script:
-- Load this on the card under "CHDK/SCRIPTS"
-- Enter the CHDK menu through your "ALT" button
-- Under scripts, select the script and specify "Autostart: on"
--
-- As the camera starts up, this also loads. with the shutter button pressed, you can interrupt the script
-- Then press the "ALT" button to disable the scripting actuator.
-- Press the shutter button to extend the lens
-- Press "ALT" again to bring up the scripting actuator.
-- Press the shutter button to reinitiate the script.
-- If you have a IXUS 230HS like me, the focus can't be set automatically. Point the camera at a distant object while the script
-- is starting. It should say "Focused", after which it's ready for use.
--
-- Example paparazzi airframe configuration:
--
-- <section name="DIGITAL_CAMERA" prefix="DC_">
-- <define name="AUTOSHOOT_QUARTERSEC_PERIOD" value="8" unit="quarter_second"/>
-- <define name="AUTOSHOOT_METER_GRID" value="60" unit="meter"/>
-- <define name="SHUTTER_DELAY" value="0" unit="quarter_second"/>
-- <define name="POWER_OFF_DELAY" value="3" unit="quarter_second"/>
-- </section>
--
-- <load name="digital_cam.xml" >
-- <define name="DC_SHUTTER_LED" value="4"/>
-- <define name="DC_POWER_OFF_LED" value="4"/>
-- <define name="DC_RELEASE" value="LED_ON" />
-- <define name="DC_PUSH" value="LED_OFF" />
-- </load>
--
print( "PictUAV Started " )
function print_status (frame)
local free = get_jpg_count()
print("#" .. frame )
end
-- switch to autofocus mode, pre-focus, then go to manual focus mode (locking focus there).
-- this helps to reduce the delay between the signal and taking the picture.
function pre_focus()
local focused = false
local try = 1
while not focused and try <= 5 do
print("Pre-focus attempt " .. try)
press("shoot_half")
sleep(2000)
if get_prop(18) > 0 then
print("Focused")
focused = true
set_aflock(1)
end
release("shoot_half")
sleep(500)
try = try + 1
end
return focused
end
-- set aperture/shooting mode to landscape
set_prop(6,3)
ap = get_prop(6)
print ("AF(3=inf,4=MF) "..ap)
-- Turn IS off
set_prop(145, 3)
-- set P mode
set_capture_mode(2)
sleep(1000)
-- Get focusing mode
p = get_prop(6)
if (p==3) then
print "Inf set."
else
-- on ixus230hs, no explicit MF.
-- so set to infinity (3)
while (p ~= 3) do
press "left"
release "left"
p = get_prop (6)
end
print "Focus set to infinity"
end
-- on ixus230hs set focus doesn't fail, but doesn't do anything.
set_focus(100)
print "set_focus 100"
sleep(2000)
f = get_focus
-- on ixus230hs set focus doesn't fail, but doesn't do anything.
set_focus( 65535 )
print "set_focus 65535"
sleep( 2000)
g = get_focus
if (f==g) then
print "set_focus inop"
-- if focusing until here didn't work, pre-focus using a different method.
pre_focus()
else
-- set_aflock(1) fails when the camera isn't knowingly focused.
print( "Setting aflock 1" )
sleep(1000)
set_aflock( 1 )
end
-- measuring the pulse on CHDK isn't necessarily that accurate, they can easily vary by 40ms.
-- Since I'm using a 100ms wait here, the variance for a shoot is around 140ms.
-- If the pulse is 250ms, then I should allow for anything down to 110.
-- For Paparazzi, taking a single picture using the button may not work because the timing may be very off
-- considering the 4Hz loop (this requires a change in the cam module for paparazzi).
print( "PictUAV loop " )
a = -1
repeat
a = get_usb_power(0)
-- a pulse is detected longer than ~610ms.
if (a>61) then
print( "shutting down " )
shut_down()
sleep(1500)
break
end
-- a pulse longer than ~100ms is detected.
if (a>10) then
frame = 1
shoot()
frame = frame + 1
end
sleep(100)
until ( false )
print( "PictUAV ended " )