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

Merge pull request #1452 from paparazzi/compressed_trig

Browse Source 
[math] compressed sine table for flash and RAM, support for 20K RAM boards

Needed for boards like CC3D and naze32 (STM32F103CBT6, 128K flash, 20K RAM).

Also adds preliminary support for the naze32 rev4 and rev5, CC3D and CJMCU boards.
This commit is contained in:
Felix Ruess
2015-12-03 15:13:37 +01:00
parent 6a4e0fa8c6 6d6e59e5c9
commit 6c2cdabd55
27 changed files with 2740 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/ENAC/fixed-wing/funjet2.xml
+4
View File
@@ -10,6 +10,10 @@
<airframe name="Funjet II">
<firmware name="test_progs">
<target name="test_math_trig_compressed" board="apogee_1.0"/>
</firmware>
<modules>
<load name="mag_hmc58xx.xml">
<define name="MODULE_HMC58XX_UPDATE_AHRS"/>
conf/airframes/flixr/conf.xml
+26 -4
View File
@@ -18,19 +18,19 @@
telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/rotorcraft_basic.xml"
settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/estimation/ahrs_float_mlkf.xml settings/control/stabilization_att_int_quat.xml"
settings_modules="modules/geo_mag.xml modules/air_data.xml modules/video_thread.xml modules/video_rtp_stream.xml"
gui_color="blue"
settings_modules="modules/geo_mag.xml modules/air_data.xml modules/video_thread.xml modules/video_rtp_stream.xml"
/>
<aircraft
name="cc3d"
ac_id="21"
name="demo_cc3d"
ac_id="2"
airframe="airframes/untested/demo_cc3d.xml"
radio="radios/cockpitSX.xml"
telemetry="telemetry/demo.xml"
flight_plan="flight_plans/dummy.xml"
settings="settings/estimation/ahrs_int_cmpl_quat.xml settings/estimation/body_to_imu.xml settings/test_actuators_pwm.xml"
gui_color="blue"
settings_modules=""
gui_color="blue"
/>
<aircraft
name="discovery"
@@ -76,6 +76,28 @@
settings_modules="modules/gps_ubx_ucenter.xml"
gui_color="blue"
/>
<aircraft
name="quad_cc3d"
ac_id="3"
airframe="airframes/untested/quad_cc3d.xml"
radio="radios/dummy.xml"
telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/rotorcraft_basic.xml"
settings="settings/rotorcraft_basic.xml settings/control/stabilization_att_int.xml settings/control/rotorcraft_guidance.xml settings/estimation/ahrs_int_cmpl_quat.xml settings/estimation/body_to_imu.xml"
settings_modules=""
gui_color="blue"
/>
<aircraft
name="quad_flip32"
ac_id="32"
airframe="airframes/untested/quad_flip32.xml"
radio="radios/TGY9x_jeti.xml"
telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/rotorcraft_basic.xml"
settings="settings/rotorcraft_basic.xml settings/control/stabilization_att_int.xml settings/control/rotorcraft_guidance.xml settings/estimation/ahrs_int_cmpl_quat.xml settings/estimation/body_to_imu.xml"
settings_modules="modules/air_data.xml modules/gps_ubx_ucenter.xml"
gui_color="blue"
/>
<aircraft
name="test_bbb"
ac_id="1"
conf/airframes/untested/hex_naze32.xml
+212
View File
@@ -0,0 +1,212 @@
<!DOCTYPE airframe SYSTEM "../airframe.dtd">
<!-- this is a test file for the Abusemark Naze32 rev5 board:
* Autopilot: Abusemark Naze32 https://code.google.com/p/afrodevices/wiki/AfroFlight32
* Actuators: PWM motor controllers http://wiki.paparazziuav.org/wiki/Subsystem/actuators#PWM
* RC: PPM input http://wiki.paparazziuav.org/wiki/Subsystem/radio_control#PPM
hex copter configuration (USE_SERVOS_5AND6) moves the PPM input from "1" to "6" on RC input
pwm motor connectors are in "standard mode" M1-M6 on the servo headers
-->
<airframe name="Hexcopter Naze32">
<firmware name="rotorcraft">
<define name="USE_SERVOS_5AND6"/>
<define name="PPRZ_TRIG_INT_COMPR_FLASH"/>
<define name="PPRZ_TRIG_INT_COMPR_HIGHEST"/>
<target name="ap" board="naze32_rev5">
</target>
<subsystem name="radio_control" type="ppm"/>
<subsystem name="motor_mixing"/>
<subsystem name="actuators" type="pwm">
<define name="SERVO_HZ" value="400"/>
</subsystem>
<subsystem name="telemetry" type="transparent"/>
<subsystem name="imu" type="mpu60x0_i2c">
<configure name="IMU_MPU60X0_I2C_DEV" value="i2c2"/>
<configure name="USE_MAGNETOMETER" value="FALSE"/>
</subsystem>
<subsystem name="stabilization" type="int_quat"/>
<subsystem name="ahrs" type="int_cmpl_quat">
<define name="AHRS_GRAVITY_HEURISTIC_FACTOR" value="30"/>
</subsystem>
<subsystem name="ins"/>
<subsystem name="gps" type="ublox"/>
</firmware>
<firmware name="test_progs">
<target name="test_sys_time_timer" board="naze32_rev5"/>
<target name="test_sys_time_usleep" board="naze32_rev5"/>
<target name="test_telemetry" board="naze32_rev5"/>
<target name="test_math_trig_lut" board="naze32_rev5"/>
<target name="test_imu" board="naze32_rev5">
<subsystem name="imu" type="mpu6000"/>
</target>
<target name="test_ahrs" board="naze32_rev5">
<subsystem name="imu" type="mpu6000"/>
<subsystem name="ahrs" type="int_cmpl_quat"/>
<configure name="USE_MAGNETOMETER" value="FALSE"/>
</target>
</firmware>
<modules>
<load name="mag_hmc58xx.xml">
<define name="MODULE_HMC58XX_UPDATE_AHRS"/>
<configure name="MAG_HMC58XX_I2C_DEV" value="i2c2"/>
</load>
<load name="gps_ubx_ucenter.xml"/>
</modules>
<servos driver="Pwm">
<!-- Naze32 Hexa-X standard -->
<servo name="BACK_RIGHT" no="0" min="1000" neutral="1100" max="1900"/>
<servo name="FRONT_RIGHT" no="1" min="1000" neutral="1100" max="1900"/>
<servo name="BACK_LEFT" no="2" min="1000" neutral="1100" max="1900"/>
<servo name="FRONT_LEFT" no="3" min="1000" neutral="1100" max="1900"/>
<servo name="CENTER_RIGHT" no="4" min="1000" neutral="1100" max="1900"/>
<servo name="CENTER_LEFT" no="5" min="1000" neutral="1100" max="1900"/>
</servos>
<commands>
<axis name="ROLL" failsafe_value="0"/>
<axis name="PITCH" failsafe_value="0"/>
<axis name="YAW" failsafe_value="0"/>
<axis name="THRUST" failsafe_value="0"/>
</commands>
<section name="MIXING" prefix="MOTOR_MIXING_">
<define name="TRIM_ROLL" value="0"/>
<define name="TRIM_PITCH" value="0"/>
<define name="TRIM_YAW" value="0"/>
<define name="NB_MOTOR" value="6"/>
<define name="SCALE" value="256"/>
<!-- https://github.com/gearwolf-OP/Mini-Hexacopter >
< Naze32 Hexa-X standard BR_CC FR_CC BL_C FL_C CR_C CL_CC -->
<define name="ROLL_COEF" value="{ 106, 150, -106, -150, 256, -256}"/>
<define name="PITCH_COEF" value="{-256, 256, -256, 256, 0, 0}"/>
<define name="YAW_COEF" value="{ 199, 192, -199, -192, -256, 256}"/>
<define name="THRUST_COEF" value="{ 256, 256, 256, 256, 256, 256}"/>
</section>
<command_laws>
<call fun="motor_mixing_run(autopilot_motors_on,FALSE,values)"/>
<set servo="BACK_RIGHT" value="motor_mixing.commands[0]"/>
<set servo="FRONT_RIGHT" value="motor_mixing.commands[1]"/>
<set servo="BACK_LEFT" value="motor_mixing.commands[2]"/>
<set servo="FRONT_LEFT" value="motor_mixing.commands[3]"/>
<set servo="CENTER_RIGHT" value="motor_mixing.commands[4]"/>
<set servo="CENTER_LEFT" value="motor_mixing.commands[5]"/>
</command_laws>
<section name="IMU" prefix="IMU_">
<define name="BODY_TO_IMU_PHI" value="180." 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="STABILIZATION_RATE" prefix="STABILIZATION_RATE_">
<!-- setpoints -->
<define name="SP_MAX_P" value="10000"/>
<define name="SP_MAX_Q" value="10000"/>
<define name="SP_MAX_R" value="10000"/>
<define name="DEADBAND_P" value="20"/>
<define name="DEADBAND_Q" value="20"/>
<define name="DEADBAND_R" value="200"/>
<define name="REF_TAU" value="4"/>
<!-- feedback -->
<define name="GAIN_P" value="400"/>
<define name="GAIN_Q" value="400"/>
<define name="GAIN_R" value="350"/>
<define name="IGAIN_P" value="75"/>
<define name="IGAIN_Q" value="75"/>
<define name="IGAIN_R" value="50"/>
<!-- feedforward -->
<define name="DDGAIN_P" value="300"/>
<define name="DDGAIN_Q" value="300"/>
<define name="DDGAIN_R" value="300"/>
</section>
<section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
<!-- setpoints -->
<define name="SP_MAX_PHI" value="45." unit="deg"/>
<define name="SP_MAX_THETA" value="45." unit="deg"/>
<define name="SP_MAX_R" value="90." unit="deg/s"/>
<define name="DEADBAND_A" value="0"/>
<define name="DEADBAND_E" value="0"/>
<define name="DEADBAND_R" value="250"/>
<!-- reference -->
<define name="REF_OMEGA_P" value="400" unit="deg/s"/>
<define name="REF_ZETA_P" value="0.85"/>
<define name="REF_MAX_P" value="400." unit="deg/s"/>
<define name="REF_MAX_PDOT" value="RadOfDeg(8000.)"/>
<define name="REF_OMEGA_Q" value="400" unit="deg/s"/>
<define name="REF_ZETA_Q" value="0.85"/>
<define name="REF_MAX_Q" value="400." unit="deg/s"/>
<define name="REF_MAX_QDOT" value="RadOfDeg(8000.)"/>
<define name="REF_OMEGA_R" value="250" unit="deg/s"/>
<define name="REF_ZETA_R" value="0.85"/>
<define name="REF_MAX_R" value="180." unit="deg/s"/>
<define name="REF_MAX_RDOT" value="RadOfDeg(1800.)"/>
<!-- feedback -->
<define name="PHI_PGAIN" value="1000"/>
<define name="PHI_DGAIN" value="400"/>
<define name="PHI_IGAIN" value="200"/>
<define name="THETA_PGAIN" value="1000"/>
<define name="THETA_DGAIN" value="400"/>
<define name="THETA_IGAIN" value="200"/>
<define name="PSI_PGAIN" value="500"/>
<define name="PSI_DGAIN" value="300"/>
<define name="PSI_IGAIN" value="10"/>
<!-- feedforward -->
<define name="PHI_DDGAIN" value="300"/>
<define name="THETA_DDGAIN" value="300"/>
<define name="PSI_DDGAIN" value="300"/>
</section>
<section name="GUIDANCE_V" prefix="GUIDANCE_V_">
<define name="HOVER_KP" value="150"/>
<define name="HOVER_KD" value="80"/>
<define name="HOVER_KI" value="20"/>
<define name="NOMINAL_HOVER_THROTTLE" value="0.5"/>
<define name="ADAPT_THROTTLE_ENABLED" value="TRUE"/>
</section>
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="MAX_BANK" value="20" unit="deg"/>
<define name="USE_SPEED_REF" value="TRUE"/>
<define name="PGAIN" value="50"/>
<define name="DGAIN" value="100"/>
<define name="AGAIN" value="70"/>
<define name="IGAIN" value="20"/>
</section>
<section name="AUTOPILOT">
<define name="MODE_MANUAL" value="AP_MODE_RATE_DIRECT"/>
<define name="MODE_AUTO1" value="AP_MODE_ATTITUDE_DIRECT"/>
<define name="MODE_AUTO2" value="AP_MODE_HOVER_Z_HOLD"/>
</section>
<section name="BAT">
<define name="CATASTROPHIC_BAT_LEVEL" value="9.3" unit="V"/>
<define name="CRITIC_BAT_LEVEL" value="9.6" unit="V"/>
<define name="LOW_BAT_LEVEL" value="10.1" unit="V"/>
<define name="MAX_BAT_LEVEL" value="12.4" unit="V"/>
<define name="MILLIAMP_AT_FULL_THROTTLE" value="30000"/>
</section>
</airframe>
conf/airframes/untested/quad_cc3d.xml
+216
View File
@@ -0,0 +1,216 @@
<!DOCTYPE airframe SYSTEM "../airframe.dtd">
<!-- this is a quadrotor frame with the OpenPilot CC3D board:
* Autopilot: OpenPilot CC3D https://www.openpilot.org/product/coptercontrol/
-->
<airframe name="QuadCC3D">
<firmware name="rotorcraft">
<target name="ap" board="cc3d">
</target>
<target name="nps" board="pc">
<subsystem name="fdm" type="jsbsim"/>
</target>
<subsystem name="radio_control" type="spektrum">
<define name="RADIO_MODE" value="RADIO_AUX1"/>
<define name="RADIO_KILL_SWITCH" value="RADIO_GEAR"/>
</subsystem>
<subsystem name="motor_mixing"/>
<subsystem name="actuators" type="pwm">
<define name="SERVO_HZ" value="400"/>
</subsystem>
<subsystem name="telemetry" type="transparent"/>
<subsystem name="stabilization" type="int_quat"/>
<subsystem name="gps" type="ublox"/>
<subsystem name="imu" type="mpu6000"/>
<subsystem name="ahrs" type="int_cmpl_quat">
<define name="AHRS_GRAVITY_HEURISTIC_FACTOR" value="30"/>
</subsystem>
<subsystem name="ins"/>
<!-- CC3D board does not have a mag -->
<configure name="USE_MAGNETOMETER" value="FALSE"/>
<!-- compress trig tables so it fits in board -->
<define name="PPRZ_TRIG_INT_COMPR_FLASH"/>
<define name="PPRZ_TRIG_INT_COMPR_LOW"/>
</firmware>
<firmware name="setup">
<target name="tunnel" board="cc3d"/>
</firmware>
<firmware name="test_progs">
<target name="test_sys_time_timer" board="cc3d"/>
<target name="test_sys_time_usleep" board="cc3d"/>
<target name="test_telemetry" board="cc3d"/>
<target name="test_imu" board="cc3d">
<subsystem name="imu" type="mpu6000"/>
</target>
<target name="test_ahrs" board="cc3d">
<subsystem name="imu" type="mpu6000"/>
<subsystem name="ahrs" type="int_cmpl_quat"/>
<configure name="USE_MAGNETOMETER" value="FALSE"/>
</target>
<target name="test_actuators_pwm" board="cc3d"/>
<target name="test_math_trig_compressed" board="cc3d"/>
</firmware>
<modules main_freq="512">
</modules>
<servos driver="Pwm">
<servo name="FL" no="0" min="1000" neutral="1100" max="2000"/>
<servo name="FR" no="1" min="1000" neutral="1100" max="2000"/>
<servo name="BR" no="2" min="1000" neutral="1100" max="2000"/>
<servo name="BL" no="3" min="1000" neutral="1100" max="2000"/>
</servos>
<section name="MIXING" prefix="MOTOR_MIXING_">
<!-- front left (CW), front right (CCW), back right (CW), back left (CCW) -->
<define name="TYPE" value="QUAD_X"/>
</section>
<commands>
<axis name="PITCH" failsafe_value="0"/>
<axis name="ROLL" failsafe_value="0"/>
<axis name="YAW" failsafe_value="0"/>
<axis name="THRUST" failsafe_value="0"/>
</commands>
<command_laws>
<call fun="motor_mixing_run(autopilot_motors_on,FALSE,values)"/>
<set servo="FL" value="motor_mixing.commands[MOTOR_FRONT_LEFT]"/>
<set servo="FR" value="motor_mixing.commands[MOTOR_FRONT_RIGHT]"/>
<set servo="BR" value="motor_mixing.commands[MOTOR_BACK_RIGHT]"/>
<set servo="BL" value="motor_mixing.commands[MOTOR_BACK_LEFT]"/>
</command_laws>
<section name="IMU" prefix="IMU_">
<define name="MAG_X_NEUTRAL" value="22"/>
<define name="MAG_Y_NEUTRAL" value="-314"/>
<define name="MAG_Z_NEUTRAL" value="13"/>
<define name="MAG_X_SENS" value="3.68160078446" integer="16"/>
<define name="MAG_Y_SENS" value="3.6093630842" integer="16"/>
<define name="MAG_Z_SENS" value="3.48306842466" integer="16"/>
<define name="BODY_TO_IMU_PHI" value="180." unit="deg"/>
<define name="BODY_TO_IMU_THETA" value="0." unit="deg"/>
<define name="BODY_TO_IMU_PSI" value="-90." unit="deg"/>
</section>
<section name="AHRS" prefix="AHRS_">
<define name="H_X" value="0.3770441"/>
<define name="H_Y" value="0.0193986"/>
<define name="H_Z" value="0.9259921"/>
</section>
<section name="INS" prefix="INS_">
</section>
<section name="STABILIZATION_RATE" prefix="STABILIZATION_RATE_">
<!-- setpoints -->
<define name="SP_MAX_P" value="10000"/>
<define name="SP_MAX_Q" value="10000"/>
<define name="SP_MAX_R" value="10000"/>
<define name="DEADBAND_P" value="20"/>
<define name="DEADBAND_Q" value="20"/>
<define name="DEADBAND_R" value="200"/>
<!-- feedback -->
<define name="GAIN_P" value="400"/>
<define name="GAIN_Q" value="400"/>
<define name="GAIN_R" value="350"/>
<define name="IGAIN_P" value="75"/>
<define name="IGAIN_Q" value="75"/>
<define name="IGAIN_R" value="50"/>
</section>
<section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
<!-- setpoints -->
<define name="SP_MAX_PHI" value="60." unit="deg"/>
<define name="SP_MAX_THETA" value="60." unit="deg"/>
<define name="SP_MAX_R" value="90." unit="deg/s"/>
<define name="DEADBAND_A" value="0"/>
<define name="DEADBAND_E" value="0"/>
<define name="DEADBAND_R" value="250"/>
<!-- reference -->
<define name="REF_OMEGA_P" value="400" unit="deg/s"/>
<define name="REF_ZETA_P" value="0.85"/>
<define name="REF_MAX_P" value="400." unit="deg/s"/>
<define name="REF_MAX_PDOT" value="RadOfDeg(8000.)"/>
<define name="REF_OMEGA_Q" value="400" unit="deg/s"/>
<define name="REF_ZETA_Q" value="0.85"/>
<define name="REF_MAX_Q" value="400." unit="deg/s"/>
<define name="REF_MAX_QDOT" value="RadOfDeg(8000.)"/>
<define name="REF_OMEGA_R" value="250" unit="deg/s"/>
<define name="REF_ZETA_R" value="0.85"/>
<define name="REF_MAX_R" value="180." unit="deg/s"/>
<define name="REF_MAX_RDOT" value="RadOfDeg(1800.)"/>
<!-- feedback -->
<define name="PHI_PGAIN" value="1000"/>
<define name="PHI_DGAIN" value="400"/>
<define name="PHI_IGAIN" value="200"/>
<define name="THETA_PGAIN" value="1000"/>
<define name="THETA_DGAIN" value="400"/>
<define name="THETA_IGAIN" value="200"/>
<define name="PSI_PGAIN" value="500"/>
<define name="PSI_DGAIN" value="300"/>
<define name="PSI_IGAIN" value="10"/>
<!-- feedforward -->
<define name="PHI_DDGAIN" value="300"/>
<define name="THETA_DDGAIN" value="300"/>
<define name="PSI_DDGAIN" value="300"/>
</section>
<section name="GUIDANCE_V" prefix="GUIDANCE_V_">
<define name="HOVER_KP" value="150"/>
<define name="HOVER_KD" value="80"/>
<define name="HOVER_KI" value="20"/>
<define name="NOMINAL_HOVER_THROTTLE" value="0.5"/>
<define name="ADAPT_THROTTLE_ENABLED" value="TRUE"/>
</section>
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="APPROX_FORCE_BY_THRUST" value="TRUE"/>
<define name="MAX_BANK" value="30" unit="deg"/>
<define name="USE_SPEED_REF" value="TRUE"/>
<define name="PGAIN" value="50"/>
<define name="DGAIN" value="100"/>
<define name="AGAIN" value="70"/>
<define name="IGAIN" value="20"/>
</section>
<section name="SIMULATOR" prefix="NPS_">
<define name="ACTUATOR_NAMES" value="nw_motor, ne_motor, se_motor, sw_motor" type="string[]"/>
<define name="JSBSIM_MODEL" value="simple_x_quad_ccw" type="string"/>
<define name="SENSORS_PARAMS" value="nps_sensors_params_default.h" type="string"/>
<!-- mode switch on joystick channel 5 (axis numbering starting at zero) -->
<define name="JS_AXIS_MODE" value="4"/>
</section>
<section name="AUTOPILOT">
<define name="MODE_MANUAL" value="AP_MODE_ATTITUDE_DIRECT"/>
<define name="MODE_AUTO1" value="AP_MODE_ATTITUDE_Z_HOLD"/>
<define name="MODE_AUTO2" value="AP_MODE_NAV"/>
</section>
<section name="BAT">
<define name="CATASTROPHIC_BAT_LEVEL" value="9.3" unit="V"/>
<define name="CRITIC_BAT_LEVEL" value="9.6" unit="V"/>
<define name="LOW_BAT_LEVEL" value="9.7" unit="V"/>
<define name="MAX_BAT_LEVEL" value="12.4" unit="V"/>
</section>
</airframe>
conf/airframes/untested/quad_cjmcu.xml
+233
View File
@@ -0,0 +1,233 @@
<!-- this is a CJMCU quadrotor, it comes with four brushed motors in an X configuration. -->
<!--
The motor and rotor configuration is the following:
Front
^
|
Motor0(NW) Motor1(NE)
CW CCW
\ /
,___,
| |
| |
|___|
/ \
CCW CW
Motor3(SW) Motor2(SE)
-->
<!--
Applicable configuration:
airframe="airframes/examples/cjmcu.xml"
radio="radios/mc24q.xml"
telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/rotorcraft_basic.xml"
settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_att_int.xml settings/control/stabilization_rate.xml"
-->
<airframe name="cjmcu">
<firmware name="rotorcraft">
<define name="PPRZ_TRIG_INT_COMPR_FLASH"/>
<define name="PPRZ_TRIG_INT_COMPR_HIGHEST"/>
<target name="ap" board="cjmcu">
<subsystem name="radio_control" type="ppm"/>
<configure name="AHRS_PROPAGATE_FREQUENCY" value="500"/>
</target>
<target name="nps" board="pc">
<subsystem name="fdm" type="jsbsim"/>
<subsystem name="radio_control" type="ppm"/>
</target>
<subsystem name="motor_mixing"/>
<subsystem name="actuators" type="pwm">
<!-- This airframe is using the servo PWM outputs directly to drive mosfets, which in turn directly drive brushed motors.
With the following settings we will be driving the motors with a 36KHz signal that should be well above anyone's hearing range.
If we went with the 500Hz that woul be the default we would be hearing a very annoying high pitched sound coming from the motors.
-->
<!-- Setting the PWM timer base frequency to 36MHz -->
<define name="PWM_BASE_FREQ" value="36000000"/>
<!-- Setting the PWM interval to 36KHz -->
<define name="SERVO_HZ" value="36000"/>
<!--define name="USE_SERVOS_1AND2"/-->
</subsystem>
<subsystem name="telemetry" type="transparent"/>
<subsystem name="imu" type="mpu60x0_i2c">
<configure name="IMU_MPU60X0_I2C_DEV" value="i2c1"/>
<configure name="USE_MAGNETOMETER" value="FALSE"/>
</subsystem>
<subsystem name="gps" type="ublox"/>
<subsystem name="stabilization" type="int_quat"/>
<subsystem name="ahrs" type="int_cmpl_quat"/>
<subsystem name="ins"/>
</firmware>
<modules main_freq="512">
<load name="gps_ubx_ucenter.xml"/>
<load name="send_imu_mag_current.xml"/>
</modules>
<servos driver="Pwm">
<servo name="NW" no="0" min="0" neutral="50" max="1000"/>
<servo name="NE" no="1" min="0" neutral="50" max="1000"/>
<servo name="SE" no="2" min="0" neutral="50" max="1000"/>
<servo name="SW" no="3" min="0" neutral="50" max="1000"/>
</servos>
<commands>
<axis name="PITCH" failsafe_value="0"/>
<axis name="ROLL" failsafe_value="0"/>
<axis name="YAW" failsafe_value="0"/>
<axis name="THRUST" failsafe_value="0"/>
</commands>
<command_laws>
<call fun="motor_mixing_run(autopilot_motors_on,FALSE,values)"/>
<set servo="NW" value="motor_mixing.commands[MOTOR_FRONT_LEFT]"/>
<set servo="NE" value="motor_mixing.commands[MOTOR_FRONT_RIGHT]"/>
<set servo="SE" value="motor_mixing.commands[MOTOR_BACK_RIGHT]"/>
<set servo="SW" value="motor_mixing.commands[MOTOR_BACK_LEFT]"/>
</command_laws>
<section name="MIXING" prefix="MOTOR_MIXING_">
<!-- front left (CW), front right (CCW), back right (CW), back left (CCW) -->
<define name="TYPE" value="QUAD_X"/>
</section>
<section name="IMU" prefix="IMU_">
<define name="BODY_TO_IMU_PHI" value="180." unit="deg"/>
<define name="BODY_TO_IMU_THETA" value="0." unit="deg"/>
<define name="BODY_TO_IMU_PSI" value="0." unit="deg"/>
<define name="ACCEL_X_NEUTRAL" value="0"/>
<define name="ACCEL_Y_NEUTRAL" value="0"/>
<define name="ACCEL_Z_NEUTRAL" value="0"/>
<!-- MAGNETO CALIBRATION DELFT -->
<define name="MAG_X_NEUTRAL" value="286"/>
<define name="MAG_Y_NEUTRAL" value="-72"/>
<define name="MAG_Z_NEUTRAL" value="97"/>
<define name="MAG_X_SENS" value="3.94431833863" integer="16"/>
<define name="MAG_Y_SENS" value="4.14629702271" integer="16"/>
<define name="MAG_Z_SENS" value="4.54518768636" integer="16"/>
<!-- MAGNETO CURRENT CALIBRATION -->
<define name="MAG_X_CURRENT_COEF" value="0.0103422023767"/>
<define name="MAG_Y_CURRENT_COEF" value="0.0084568317783"/>
<define name="MAG_Z_CURRENT_COEF" value="-0.01935617335"/>
</section>
<section name="AUTOPILOT">
<define name="MODE_STARTUP" value="AP_MODE_NAV"/>
<define name="MODE_MANUAL" value="AP_MODE_ATTITUDE_DIRECT"/>
<define name="MODE_AUTO1" value="AP_MODE_HOVER_Z_HOLD"/>
<define name="MODE_AUTO2" value="AP_MODE_NAV"/>
</section>
<section name="BAT">
<define name="MILLIAMP_AT_FULL_THROTTLE" value="14000"/>
<define name="CATASTROPHIC_BAT_LEVEL" value="3.0" unit="V"/>
<define name="CRITIC_BAT_LEVEL" value="3.3" unit="V"/>
<define name="LOW_BAT_LEVEL" value="3.5" unit="V"/>
<define name="MAX_BAT_LEVEL" value="4.1" unit="V"/>
</section>
<section name="STABILIZATION_RATE" prefix="STABILIZATION_RATE_">
<define name="SP_MAX_P" value="15000"/>
<define name="SP_MAX_Q" value="15000"/>
<define name="SP_MAX_R" value="15000"/>
<define name="GAIN_P" value="400"/>
<define name="GAIN_Q" value="400"/>
<define name="GAIN_R" value="418"/>
<define name="IGAIN_P" value="301"/>
<define name="IGAIN_Q" value="302"/>
<define name="IGAIN_R" value="303"/>
</section>
<section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
<!-- setpoints -->
<define name="SP_MAX_PHI" value="45." unit="deg"/>
<define name="SP_MAX_THETA" value="45." unit="deg"/>
<define name="SP_MAX_PSI" value="45." unit="deg"/>
<define name="SP_MAX_R" value="90." unit="deg/s"/>
<define name="SP_MAX_P" value="90." unit="deg/s"/>
<define name="DEADBAND_R" value="250"/>
<define name="DEADBAND_A" value="250"/>
<!-- reference -->
<define name="REF_OMEGA_P" value="800" unit="deg/s"/>
<define name="REF_ZETA_P" value="0.85"/>
<define name="REF_MAX_P" value="400." unit="deg/s"/>
<define name="REF_MAX_PDOT" value="RadOfDeg(8000.)"/>
<define name="REF_OMEGA_Q" value="800" unit="deg/s"/>
<define name="REF_ZETA_Q" value="0.85"/>
<define name="REF_MAX_Q" value="400." unit="deg/s"/>
<define name="REF_MAX_QDOT" value="RadOfDeg(8000.)"/>
<define name="REF_OMEGA_R" value="500" unit="deg/s"/>
<define name="REF_ZETA_R" value="0.85"/>
<define name="REF_MAX_R" value="90." unit="deg/s"/>
<define name="REF_MAX_RDOT" value="RadOfDeg(900.)"/>
<!-- feedback -->
<define name="PHI_PGAIN" value="400"/>
<define name="PHI_DGAIN" value="200"/>
<define name="PHI_IGAIN" value="100"/>
<define name="THETA_PGAIN" value="400"/>
<define name="THETA_DGAIN" value="200"/>
<define name="THETA_IGAIN" value="100"/>
<define name="PSI_PGAIN" value="500"/>
<define name="PSI_DGAIN" value="300"/>
<define name="PSI_IGAIN" value="10"/>
<!-- feedforward -->
<define name="PHI_DDGAIN" value="0"/>
<define name="THETA_DDGAIN" value="0"/>
<define name="PSI_DDGAIN" value=" 300"/>
<define name="PHI_AGAIN" value="0"/>
<define name="THETA_AGAIN" value="0"/>
<define name="PSI_AGAIN" value="0"/>
</section>
<section name="INS" prefix="INS_">
</section>
<section name="GUIDANCE_V" prefix="GUIDANCE_V_">
<define name="HOVER_KP" value="276"/>
<define name="HOVER_KD" value="455"/>
<define name="HOVER_KI" value="100"/>
<define name="GUIDANCE_V_NOMINAL_HOVER_THROTTLE" value="0.5"/>
</section>
<section name="AHRS" prefix="AHRS_">
<define name="H_X" value=" 0.47577"/>
<define name="H_Y" value=" 0.11811"/>
<define name="H_Z" value=" 0.87161"/>
</section>
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="PGAIN" value="39"/>
<define name="DGAIN" value="50"/>
<define name="IGAIN" value="19"/>
</section>
<section name="SIMULATOR" prefix="NPS_">
<define name="ACTUATOR_NAMES" value="nw_motor, ne_motor, se_motor, sw_motor" type="string[]"/>
<define name="JSBSIM_MODEL" value="simple_x_quad_ccw" type="string"/>
<define name="SENSORS_PARAMS" value="nps_sensors_params_default.h" type="string"/>
</section>
</airframe>
conf/airframes/untested/quad_flip32.xml
+221
View File
@@ -0,0 +1,221 @@
<!DOCTYPE airframe SYSTEM "../airframe.dtd">
<!-- this is a quadrotor frame with the Flip32 10DOF board (naze32 rev4 clone):
* Autopilot: Abusemark Naze32 https://code.google.com/p/afrodevices/wiki/AfroFlight32
* Actuators: PWM motor controllers http://wiki.paparazziuav.org/wiki/Subsystem/actuators#PWM
-->
<airframe name="QuadFlip32">
<firmware name="rotorcraft">
<target name="ap" board="naze32_rev4">
</target>
<target name="nps" board="pc">
<subsystem name="fdm" type="jsbsim"/>
</target>
<subsystem name="radio_control" type="ppm"/>
<subsystem name="motor_mixing"/>
<subsystem name="actuators" type="pwm">
<define name="SERVO_HZ" value="400"/>
</subsystem>
<subsystem name="telemetry" type="transparent"/>
<subsystem name="stabilization" type="int_quat"/>
<subsystem name="gps" type="ublox"/>
<subsystem name="imu" type="mpu60x0_i2c"/>
<configure name="IMU_MPU60X0_I2C_DEV" value="i2c2"/>
<subsystem name="ahrs" type="int_cmpl_quat">
<define name="AHRS_GRAVITY_HEURISTIC_FACTOR" value="30"/>
</subsystem>
<subsystem name="ins"/>
<!-- compress trig tables so it fits in board -->
<define name="PPRZ_TRIG_INT_COMPR_FLASH"/>
<define name="PPRZ_TRIG_INT_COMPR_LOW"/>
</firmware>
<firmware name="setup">
<target name="tunnel" board="naze32_rev4"/>
</firmware>
<firmware name="test_progs">
<target name="test_sys_time_timer" board="naze32_rev4"/>
<target name="test_sys_time_usleep" board="naze32_rev4"/>
<target name="test_telemetry" board="naze32_rev4"/>
<target name="test_imu" board="naze32_rev4">
<subsystem name="imu" type="mpu60x0_i2c"/>
<configure name="IMU_MPU60X0_I2C_DEV" value="i2c2"/>
</target>
<target name="test_ahrs" board="naze32_rev4">
<subsystem name="imu" type="mpu60x0_i2c"/>
<configure name="IMU_MPU60X0_I2C_DEV" value="i2c2"/>
<subsystem name="ahrs" type="int_cmpl_quat"/>
<configure name="USE_MAGNETOMETER" value="FALSE"/>
</target>
<target name="test_actuators_pwm" board="naze32_rev4"/>
<target name="test_math_trig_compressed" board="naze32_rev4"/>
</firmware>
<modules main_freq="512">
<load name="mag_hmc58xx.xml">
<define name="MODULE_HMC58XX_UPDATE_AHRS"/>
<configure name="MAG_HMC58XX_I2C_DEV" value="i2c2"/>
</load>
<load name="air_data.xml"/>
<load name="gps_ubx_ucenter.xml"/>
</modules>
<servos driver="Pwm">
<servo name="FL" no="0" min="1000" neutral="1100" max="2000"/>
<servo name="FR" no="1" min="1000" neutral="1100" max="2000"/>
<servo name="BR" no="2" min="1000" neutral="1100" max="2000"/>
<servo name="BL" no="3" min="1000" neutral="1100" max="2000"/>
</servos>
<section name="MIXING" prefix="MOTOR_MIXING_">
<!-- front left (CW), front right (CCW), back right (CW), back left (CCW) -->
<define name="TYPE" value="QUAD_X"/>
</section>
<commands>
<axis name="PITCH" failsafe_value="0"/>
<axis name="ROLL" failsafe_value="0"/>
<axis name="YAW" failsafe_value="0"/>
<axis name="THRUST" failsafe_value="0"/>
</commands>
<command_laws>
<call fun="motor_mixing_run(autopilot_motors_on,FALSE,values)"/>
<set servo="FL" value="motor_mixing.commands[MOTOR_FRONT_LEFT]"/>
<set servo="FR" value="motor_mixing.commands[MOTOR_FRONT_RIGHT]"/>
<set servo="BR" value="motor_mixing.commands[MOTOR_BACK_RIGHT]"/>
<set servo="BL" value="motor_mixing.commands[MOTOR_BACK_LEFT]"/>
</command_laws>
<section name="IMU" prefix="IMU_">
<define name="MAG_X_NEUTRAL" value="22"/>
<define name="MAG_Y_NEUTRAL" value="-314"/>
<define name="MAG_Z_NEUTRAL" value="13"/>
<define name="MAG_X_SENS" value="3.68160078446" integer="16"/>
<define name="MAG_Y_SENS" value="3.6093630842" integer="16"/>
<define name="MAG_Z_SENS" value="3.48306842466" integer="16"/>
<define name="BODY_TO_IMU_PHI" value="180." 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="AHRS" prefix="AHRS_">
<define name="H_X" value="0.3770441"/>
<define name="H_Y" value="0.0193986"/>
<define name="H_Z" value="0.9259921"/>
</section>
<section name="INS" prefix="INS_">
</section>
<section name="STABILIZATION_RATE" prefix="STABILIZATION_RATE_">
<!-- setpoints -->
<define name="SP_MAX_P" value="10000"/>
<define name="SP_MAX_Q" value="10000"/>
<define name="SP_MAX_R" value="10000"/>
<define name="DEADBAND_P" value="20"/>
<define name="DEADBAND_Q" value="20"/>
<define name="DEADBAND_R" value="200"/>
<!-- feedback -->
<define name="GAIN_P" value="400"/>
<define name="GAIN_Q" value="400"/>
<define name="GAIN_R" value="350"/>
<define name="IGAIN_P" value="75"/>
<define name="IGAIN_Q" value="75"/>
<define name="IGAIN_R" value="50"/>
</section>
<section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
<!-- setpoints -->
<define name="SP_MAX_PHI" value="60." unit="deg"/>
<define name="SP_MAX_THETA" value="60." unit="deg"/>
<define name="SP_MAX_R" value="90." unit="deg/s"/>
<define name="DEADBAND_A" value="0"/>
<define name="DEADBAND_E" value="0"/>
<define name="DEADBAND_R" value="250"/>
<!-- reference -->
<define name="REF_OMEGA_P" value="400" unit="deg/s"/>
<define name="REF_ZETA_P" value="0.85"/>
<define name="REF_MAX_P" value="400." unit="deg/s"/>
<define name="REF_MAX_PDOT" value="RadOfDeg(8000.)"/>
<define name="REF_OMEGA_Q" value="400" unit="deg/s"/>
<define name="REF_ZETA_Q" value="0.85"/>
<define name="REF_MAX_Q" value="400." unit="deg/s"/>
<define name="REF_MAX_QDOT" value="RadOfDeg(8000.)"/>
<define name="REF_OMEGA_R" value="250" unit="deg/s"/>
<define name="REF_ZETA_R" value="0.85"/>
<define name="REF_MAX_R" value="180." unit="deg/s"/>
<define name="REF_MAX_RDOT" value="RadOfDeg(1800.)"/>
<!-- feedback -->
<define name="PHI_PGAIN" value="1000"/>
<define name="PHI_DGAIN" value="400"/>
<define name="PHI_IGAIN" value="200"/>
<define name="THETA_PGAIN" value="1000"/>
<define name="THETA_DGAIN" value="400"/>
<define name="THETA_IGAIN" value="200"/>
<define name="PSI_PGAIN" value="500"/>
<define name="PSI_DGAIN" value="300"/>
<define name="PSI_IGAIN" value="10"/>
<!-- feedforward -->
<define name="PHI_DDGAIN" value="300"/>
<define name="THETA_DDGAIN" value="300"/>
<define name="PSI_DDGAIN" value="300"/>
</section>
<section name="GUIDANCE_V" prefix="GUIDANCE_V_">
<define name="HOVER_KP" value="150"/>
<define name="HOVER_KD" value="80"/>
<define name="HOVER_KI" value="20"/>
<define name="NOMINAL_HOVER_THROTTLE" value="0.5"/>
<define name="ADAPT_THROTTLE_ENABLED" value="TRUE"/>
</section>
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="APPROX_FORCE_BY_THRUST" value="TRUE"/>
<define name="MAX_BANK" value="30" unit="deg"/>
<define name="USE_SPEED_REF" value="TRUE"/>
<define name="PGAIN" value="50"/>
<define name="DGAIN" value="100"/>
<define name="AGAIN" value="70"/>
<define name="IGAIN" value="20"/>
</section>
<section name="SIMULATOR" prefix="NPS_">
<define name="ACTUATOR_NAMES" value="nw_motor, ne_motor, se_motor, sw_motor" type="string[]"/>
<define name="JSBSIM_MODEL" value="simple_x_quad_ccw" type="string"/>
<define name="SENSORS_PARAMS" value="nps_sensors_params_default.h" type="string"/>
<!-- mode switch on joystick channel 5 (axis numbering starting at zero) -->
<define name="JS_AXIS_MODE" value="4"/>
</section>
<section name="AUTOPILOT">
<define name="MODE_MANUAL" value="AP_MODE_ATTITUDE_DIRECT"/>
<define name="MODE_AUTO1" value="AP_MODE_ATTITUDE_Z_HOLD"/>
<define name="MODE_AUTO2" value="AP_MODE_NAV"/>
</section>
<section name="BAT">
<define name="CATASTROPHIC_BAT_LEVEL" value="9.3" unit="V"/>
<define name="CRITIC_BAT_LEVEL" value="9.6" unit="V"/>
<define name="LOW_BAT_LEVEL" value="9.7" unit="V"/>
<define name="MAX_BAT_LEVEL" value="12.4" unit="V"/>
</section>
</airframe>
conf/boards/cjmcu.makefile
+69
View File
@@ -0,0 +1,69 @@
# Hey Emacs, this is a -*- makefile -*-
#
# cjmcu.makefile
#
# https://github.com/cleanflight/cleanflight/issues/22
# http://blog.oscarliang.net/build-fpv-micro-quadcopter-smallest-quad
# hw rev2 ?
#
BOARD=cjmcu
BOARD_CFG=\"boards/$(BOARD).h\"
ARCH=stm32
$(TARGET).ARCHDIR = $(ARCH)
# not needed?
$(TARGET).OOCD_INTERFACE=flossjtag
$(TARGET).LDSCRIPT=$(SRC_ARCH)/cjmcu.ld
# -----------------------------------------------------------------------
# default flash mode is via SWD
# other possibilities: DFU-UTIL, JTAG, SWD, STLINK, SERIAL
FLASH_MODE ?= SWD
HAS_LUFTBOOT ?= 0
ifeq (,$(findstring $(HAS_LUFTBOOT),0 FALSE))
$(TARGET).CFLAGS+=-DLUFTBOOT
$(TARGET).LDFLAGS+=-Wl,-Ttext=0x8002000
endif
#
#
# some default values shared between different firmwares
#
#
#
# default LED configuration
#
RADIO_CONTROL_LED ?= none
BARO_LED ?= none
AHRS_ALIGNER_LED ?= 1
GPS_LED ?= 2
SYS_TIME_LED ?= 3
#
# default uart configuration
#
RADIO_CONTROL_SPEKTRUM_PRIMARY_PORT ?= UART3
RADIO_CONTROL_SPEKTRUM_SECONDARY_PORT ?= UART1
MODEM_PORT ?= UART1
MODEM_BAUD ?= B57600
GPS_PORT ?= UART3
GPS_BAUD ?= B38400
#
# default actuator configuration
#
# you can use different actuators by adding a configure option to your firmware section
# e.g. <configure name="ACTUATORS" value="actuators_ppm/>
# and by setting the correct "driver" attribute in servo section
# e.g. <servo driver="Ppm">
#
ACTUATORS ?= actuators_pwm
conf/boards/naze32_rev4.makefile
+60
View File
@@ -0,0 +1,60 @@
# Hey Emacs, this is a -*- makefile -*-
#
# naze32.makefile
#
# https://code.google.com/p/afrodevices/wiki/AfroFlight32
# hw rev4
#
BOARD=naze32
BOARD_VERSION=rev4
BOARD_CFG=\"boards/$(BOARD)_$(BOARD_VERSION).h\"
ARCH=stm32
$(TARGET).ARCHDIR = $(ARCH)
$(TARGET).LDSCRIPT=$(SRC_ARCH)/naze32.ld
# -----------------------------------------------------------------------
# default flash mode is via SWD
# other possibilities: DFU-UTIL, JTAG, SWD, STLINK, SERIAL
FLASH_MODE ?= SWD
#
#
# some default values shared between different firmwares
#
#
#
# default LED configuration
#
RADIO_CONTROL_LED ?= none
BARO_LED ?= none
AHRS_ALIGNER_LED ?= 2
GPS_LED ?= none
SYS_TIME_LED ?= 1
#
# default uart configuration
#
RADIO_CONTROL_SPEKTRUM_PRIMARY_PORT ?= UART2
MODEM_PORT ?= UART1
MODEM_BAUD ?= B57600
GPS_PORT ?= UART2
GPS_BAUD ?= B38400
#
# default actuator configuration
#
# you can use different actuators by adding a configure option to your firmware section
# e.g. <configure name="ACTUATORS" value="actuators_ppm/>
# and by setting the correct "driver" attribute in servo section
# e.g. <servo driver="Ppm">
#
ACTUATORS ?= actuators_pwm
conf/boards/naze32_rev5.makefile
+60
View File
@@ -0,0 +1,60 @@
# Hey Emacs, this is a -*- makefile -*-
#
# naze32.makefile
#
# https://code.google.com/p/afrodevices/wiki/AfroFlight32
# hw rev5
#
BOARD=naze32
BOARD_VERSION=rev5
BOARD_CFG=\"boards/$(BOARD)_$(BOARD_VERSION).h\"
ARCH=stm32
$(TARGET).ARCHDIR = $(ARCH)
$(TARGET).LDSCRIPT=$(SRC_ARCH)/naze32.ld
# -----------------------------------------------------------------------
# default flash mode is via SWD
# other possibilities: DFU-UTIL, JTAG, SWD, STLINK, SERIAL
FLASH_MODE ?= SWD
#
#
# some default values shared between different firmwares
#
#
#
# default LED configuration
#
RADIO_CONTROL_LED ?= none
BARO_LED ?= none
AHRS_ALIGNER_LED ?= 2
GPS_LED ?= none
SYS_TIME_LED ?= 1
#
# default uart configuration
#
RADIO_CONTROL_SPEKTRUM_PRIMARY_PORT ?= UART2
MODEM_PORT ?= UART1
MODEM_BAUD ?= B57600
GPS_PORT ?= UART2
GPS_BAUD ?= B38400
#
# default actuator configuration
#
# you can use different actuators by adding a configure option to your firmware section
# e.g. <configure name="ACTUATORS" value="actuators_ppm/>
# and by setting the correct "driver" attribute in servo section
# e.g. <servo driver="Ppm">
#
ACTUATORS ?= actuators_pwm
conf/firmwares/subsystems/shared/baro_board.makefile
+9
View File
@@ -189,6 +189,15 @@ else ifeq ($(BOARD), umarim)
BARO_BOARD_SRCS += boards/umarim/baro_board.c
endif
# Naze32
else ifeq ($(BOARD), naze32)
BARO_BOARD_CFLAGS += -DBARO_BOARD=BARO_MS5611_I2C
BARO_BOARD_CFLAGS += -DUSE_I2C2
BARO_BOARD_CFLAGS += -DBB_MS5611_I2C_DEV=i2c2
BARO_BOARD_SRCS += peripherals/ms5611.c
BARO_BOARD_SRCS += peripherals/ms5611_i2c.c
BARO_BOARD_SRCS += boards/baro_board_ms5611_i2c.c
endif # check board
ifneq ($(BARO_LED),none)
conf/firmwares/subsystems/shared/imu_mpu60x0_i2c.makefile
+5 -13
View File
@@ -15,19 +15,6 @@ IMU_MPU60X0_SRCS += peripherals/mpu60x0.c
IMU_MPU60X0_SRCS += peripherals/mpu60x0_i2c.c
# set default i2c bus
ifeq ($(ARCH), lpc21)
IMU_MPU60X0_I2C_DEV ?= i2c0
else ifeq ($(ARCH), stm32)
IMU_MPU60X0_I2C_DEV ?= i2c2
endif
ifeq ($(TARGET), ap)
ifndef MPU60X0_I2C_DEV
$(error Error: MPU60X0_I2C_DEV not configured!)
endif
endif
# convert i2cx to upper/lower case
IMU_MPU60X0_I2C_DEV_UPPER=$(shell echo $(IMU_MPU60X0_I2C_DEV) | tr a-z A-Z)
IMU_MPU60X0_I2C_DEV_LOWER=$(shell echo $(IMU_MPU60X0_I2C_DEV) | tr A-Z a-z)
@@ -38,6 +25,11 @@ IMU_MPU60X0_CFLAGS += -DUSE_$(IMU_MPU60X0_I2C_DEV_UPPER)
# add it for all targets except sim, fbw and nps
ifeq (,$(findstring $(TARGET),sim fbw nps))
ifndef IMU_MPU60X0_I2C_DEV
$(error Error: IMU_MPU60X0_I2C_DEV not configured!)
endif
$(TARGET).CFLAGS += $(IMU_MPU60X0_CFLAGS)
$(TARGET).srcs += $(IMU_MPU60X0_SRCS)
endif
conf/firmwares/test_progs.makefile
+18
View File
@@ -118,6 +118,9 @@ endif
ifeq ($(BOARD), cc3d)
LED_DEFINES = -DLED_BLUE=1
endif
ifeq ($(BOARD), naze32)
LED_DEFINES = -DLED_RED=1 -DLED_GREEN=2
endif
LED_DEFINES ?= -DLED_RED=2 -DLED_GREEN=3
test_sys_time_timer.ARCHDIR = $(ARCH)
@@ -225,6 +228,21 @@ test_telemetry.CFLAGS += $(COMMON_TELEMETRY_CFLAGS)
test_telemetry.srcs += $(COMMON_TELEMETRY_SRCS)
test_telemetry.srcs += test/test_telemetry.c
#
# test_math_trig_compressed: Test math trigonometric using compressed data
#
# configuration
# MODEM_PORT :
# MODEM_BAUD :
#
test_math_trig_compressed.ARCHDIR = $(ARCH)
test_math_trig_compressed.CFLAGS += $(COMMON_TEST_CFLAGS)
test_math_trig_compressed.srcs += $(COMMON_TEST_SRCS)
test_math_trig_compressed.CFLAGS += $(COMMON_TELEMETRY_CFLAGS)
test_math_trig_compressed.CFLAGS += -DPPRZ_TRIG_INT_TEST
test_math_trig_compressed.srcs += $(COMMON_TELEMETRY_SRCS)
test_math_trig_compressed.srcs += test/test_math_trig_compressed.c math/pprz_trig_int.c
#
# test ms2100 mag
conf/flash_modes.xml
+3
View File
@@ -68,6 +68,7 @@
<board name="li[s]?a_mx_.*"/>
<board name="cc3d"/>
<board name="elle*"/>
<board name="naze32*"/>
</boards>
</mode>
<mode name="BlackMagic Probe (SWD)">
@@ -80,6 +81,8 @@
<board name="px4fmu_.*"/>
<board name="cc3d"/>
<board name="elle*"/>
<board name="naze32*"/>
<board name="cjmcu*"/>
</boards>
</mode>
<mode name="BlackMagic Probe (JTAG)">
sw/airborne/arch/stm32/cjmcu.ld
+35
View File
@@ -0,0 +1,35 @@
/*
* Hey Emacs, this is a -*- makefile -*-
*
* Copyright (C) 2015 Martin Mueller <martinmm@pfump.org>
*
* 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.
*/
/* Linker script for CJMCU Quad (STM32F103CBT6, 128K flash, 20K RAM). */
/* Define memory regions. */
MEMORY
{
ram (rwx) : ORIGIN = 0x20000000, LENGTH = 20K
/* Use entire memory, no flash for stored settings */
rom (rx) : ORIGIN = 0x08000000, LENGTH = 128K
}
/* Include the common ld script. */
INCLUDE libopencm3_stm32f1.ld
sw/airborne/arch/stm32/cjmcu.makefile
+69
View File
@@ -0,0 +1,69 @@
# Hey Emacs, this is a -*- makefile -*-
#
# cjmcu.makefile
#
# https://github.com/cleanflight/cleanflight/issues/22
# http://blog.oscarliang.net/build-fpv-micro-quadcopter-smallest-quad
# hw rev2 ?
#
BOARD=cjmcu
BOARD_CFG=\"boards/$(BOARD).h\"
ARCH=stm32
$(TARGET).ARCHDIR = $(ARCH)
# not needed?
$(TARGET).OOCD_INTERFACE=flossjtag
$(TARGET).LDSCRIPT=$(SRC_ARCH)/cjmcu.ld
# -----------------------------------------------------------------------
# default flash mode is via SWD
# other possibilities: DFU-UTIL, JTAG, SWD, STLINK, SERIAL
FLASH_MODE ?= SWD
HAS_LUFTBOOT ?= 0
ifeq (,$(findstring $(HAS_LUFTBOOT),0 FALSE))
$(TARGET).CFLAGS+=-DLUFTBOOT
$(TARGET).LDFLAGS+=-Wl,-Ttext=0x8002000
endif
#
#
# some default values shared between different firmwares
#
#
#
# default LED configuration
#
RADIO_CONTROL_LED ?= none
BARO_LED ?= none
AHRS_ALIGNER_LED ?= 1
GPS_LED ?= 2
SYS_TIME_LED ?= 3
#
# default uart configuration
#
RADIO_CONTROL_SPEKTRUM_PRIMARY_PORT ?= UART3
RADIO_CONTROL_SPEKTRUM_SECONDARY_PORT ?= UART1
MODEM_PORT ?= UART1
MODEM_BAUD ?= B57600
GPS_PORT ?= UART3
GPS_BAUD ?= B38400
#
# default actuator configuration
#
# you can use different actuators by adding a configure option to your firmware section
# e.g. <configure name="ACTUATORS" value="actuators_ppm/>
# and by setting the correct "driver" attribute in servo section
# e.g. <servo driver="Ppm">
#
ACTUATORS ?= actuators_pwm
sw/airborne/arch/stm32/naze32.ld
+35
View File
@@ -0,0 +1,35 @@
/*
* Hey Emacs, this is a -*- makefile -*-
*
* Copyright (C) 2015 Felix Ruess <felix.ruess@gmail.com>
*
* This file is part of Paparazzi.
*
* Paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* Paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
/* Linker script for Abusemark Naze32 (STM32F103CBT6, 128K flash, 20K RAM). */
/* Define memory regions. */
MEMORY
{
ram (rwx) : ORIGIN = 0x20000000, LENGTH = 20K
/* Use entire memory, no flash for stored settings */
rom (rx) : ORIGIN = 0x08000000, LENGTH = 128K
}
/* Include the common ld script. */
INCLUDE libopencm3_stm32f1.ld
sw/airborne/boards/cjmcu.h
+270
View File
@@ -0,0 +1,270 @@
#ifndef CONFIG_CJMCU_H
#define CONFIG_CJMCU_H
#define BOARD_CJMCU
/* CJMCU has a 8MHz external clock and 72MHz internal. */
#define EXT_CLK 8000000
#define AHB_CLK 72000000
/*
* Onboard LEDs
*/
/* green */
#ifndef USE_LED_1
#define USE_LED_1 1
#endif
#define LED_1_GPIO GPIOC
#define LED_1_GPIO_PIN GPIO13
#define LED_1_GPIO_ON gpio_clear
#define LED_1_GPIO_OFF gpio_set
#define LED_1_AFIO_REMAP ((void)0)
/* red */
#ifndef USE_LED_2
#define USE_LED_2 1
#endif
#define LED_2_GPIO GPIOC
#define LED_2_GPIO_PIN GPIO14
#define LED_2_GPIO_ON gpio_clear
#define LED_2_GPIO_OFF gpio_set
#define LED_2_AFIO_REMAP ((void)0)
/* blue */
#ifndef USE_LED_3
#define USE_LED_3 1
#endif
#define LED_3_GPIO GPIOC
#define LED_3_GPIO_PIN GPIO15
#define LED_3_GPIO_ON gpio_clear
#define LED_3_GPIO_OFF gpio_set
#define LED_3_AFIO_REMAP ((void)0)
/* Default actuators driver */
#define DEFAULT_ACTUATORS "subsystems/actuators/actuators_pwm.h"
#define ActuatorDefaultSet(_x,_y) ActuatorPwmSet(_x,_y)
#define ActuatorsDefaultInit() ActuatorsPwmInit()
#define ActuatorsDefaultCommit() ActuatorsPwmCommit()
/*
* ADC
*/
// Internal ADC for battery on PA2 enabled by default
#ifndef USE_ADC_1
#define USE_ADC_1 1
#endif
#if USE_ADC_1
#define AD1_1_CHANNEL 2
#define ADC_1 AD1_1
#define ADC_1_GPIO_PORT GPIOA
#define ADC_1_GPIO_PIN GPIO2
#endif
/* allow to define ADC_CHANNEL_VSUPPLY in the airframe file*/
#ifndef ADC_CHANNEL_VSUPPLY
#define ADC_CHANNEL_VSUPPLY ADC_1
#endif
/* 10k/2k resistor divider, 6 * 3.3V / 4096 */
#define DefaultVoltageOfAdc(adc) (0.0049*adc)
//#define DefaultVoltageOfAdc(adc) (4)
/*
* UART pin configuration
*
* sets on which pins the UARTs are connected
*/
/* DIAG port */
#define UART1_GPIO_AF 0
#define UART1_GPIO_PORT_RX GPIO_BANK_USART1_RX
#define UART1_GPIO_RX GPIO_USART1_RX
#define UART1_GPIO_PORT_TX GPIO_BANK_USART1_TX
#define UART1_GPIO_TX GPIO_USART1_TX
/* GPS */
#define UART3_GPIO_AF 0
#define UART3_GPIO_PORT_RX GPIO_BANK_USART3_RX
#define UART3_GPIO_RX GPIO_USART3_RX
#define UART3_GPIO_PORT_TX GPIO_BANK_USART3_TX
#define UART3_GPIO_TX GPIO_USART3_TX
/* LED1 & LED2 */
/*
#define UART3_GPIO_AF AFIO_MAPR_USART3_REMAP_PARTIAL_REMAP
#define UART3_GPIO_PORT_RX GPIO_BANK_USART3_PR_RX
#define UART3_GPIO_RX GPIO_USART3_PR_RX
#define UART3_GPIO_PORT_TX GPIO_BANK_USART3_PR_TX
#define UART3_GPIO_TX GPIO_USART3_PR_TX
*/
/*
* Spektrum (none)
*/
/* PPM
*/
/*
* On Lisa/S there is no really dedicated port available. But we could use a
* bunch of different pins to do PPM Input.
*/
/*
* Default is PPM config 1, input on PA0
*/
#ifndef PPM_CONFIG
#define PPM_CONFIG 1
#endif
#if PPM_CONFIG == 1
/* input on PA0 */
#define USE_PPM_TIM2 1
#define PPM_CHANNEL TIM_IC1
#define PPM_TIMER_INPUT TIM_IC_IN_TI1
#define PPM_IRQ NVIC_TIM2_IRQ
// Capture/Compare InteruptEnable and InterruptFlag
#define PPM_CC_IE TIM_DIER_CC1IE
#define PPM_CC_IF TIM_SR_CC1IF
#define PPM_GPIO_PORT GPIOA
#define PPM_GPIO_PIN GPIO0
#define PPM_GPIO_AF 0
// Move default ADC timer
#if USE_AD_TIM2
#undef USE_AD_TIM2
#endif
#define USE_AD_TIM1 1
#elif PPM_CONFIG == 2
/* input on PA1 */
#define USE_PPM_TIM2 1
#define PPM_CHANNEL TIM_IC2
#define PPM_TIMER_INPUT TIM_IC_IN_TI2
#define PPM_IRQ NVIC_TIM2_IRQ
// Capture/Compare InteruptEnable and InterruptFlag
#define PPM_CC_IE TIM_DIER_CC2IE
#define PPM_CC_IF TIM_SR_CC2IF
#define PPM_GPIO_PORT GPIOA
#define PPM_GPIO_PIN GPIO1
#define PPM_GPIO_AF 0
// Move default ADC timer
#if USE_AD_TIM2
#undef USE_AD_TIM2
#endif
#define USE_AD_TIM1 1
#elif PPM_CONFIG == 3
/* input on PA3 */
#define USE_PPM_TIM2 1
#define PPM_CHANNEL TIM_IC4
#define PPM_TIMER_INPUT TIM_IC_IN_TI2
#define PPM_IRQ NVIC_TIM2_IRQ
// Capture/Compare InteruptEnable and InterruptFlag
#define PPM_CC_IE TIM_DIER_CC4IE
#define PPM_CC_IF TIM_SR_CC4IF
#define PPM_GPIO_PORT GPIOA
#define PPM_GPIO_PIN GPIO3
#define PPM_GPIO_AF 0
// Move default ADC timer
#if USE_AD_TIM2
#undef USE_AD_TIM2
#endif
#define USE_AD_TIM1 1
#else
#error "Unknown PPM config"
#endif // PPM_CONFIG
/*
* I2C
*
*/
#define I2C1_GPIO_PORT GPIOB
#define I2C1_GPIO_SCL GPIO6
#define I2C1_GPIO_SDA GPIO7
/* GPS TX & RX */
#define I2C2_GPIO_PORT GPIOB
#define I2C2_GPIO_SCL GPIO10
#define I2C2_GPIO_SDA GPIO11
/*
* PWM
*
*/
#define PWM_USE_TIM4 1
#define PWM_USE_TIM3 1
#define USE_PWM1 1
#define USE_PWM2 1
#define USE_PWM3 1
#define USE_PWM4 1
/* PWM_SERVO_x is the index of the servo in the actuators_pwm_values array */
#if USE_PWM1
#define PWM_SERVO_1 0
#define PWM_SERVO_1_TIMER TIM4
#define PWM_SERVO_1_GPIO GPIOB
#define PWM_SERVO_1_PIN GPIO8
#define PWM_SERVO_1_AF 0
#define PWM_SERVO_1_OC TIM_OC3
#define PWM_SERVO_1_OC_BIT (1<<2)
#else
#define PWM_SERVO_1_OC_BIT 0
#endif
#if USE_PWM2
#define PWM_SERVO_2 1
#define PWM_SERVO_2_TIMER TIM4
#define PWM_SERVO_2_GPIO GPIOB
#define PWM_SERVO_2_PIN GPIO9
#define PWM_SERVO_2_AF 0
#define PWM_SERVO_2_OC TIM_OC4
#define PWM_SERVO_2_OC_BIT (1<<3)
#else
#define PWM_SERVO_2_OC_BIT 0
#endif
#if USE_PWM3
#define PWM_SERVO_3 2
#define PWM_SERVO_3_TIMER TIM3
#define PWM_SERVO_3_GPIO GPIOB
#define PWM_SERVO_3_PIN GPIO0
#define PWM_SERVO_3_AF 0
#define PWM_SERVO_3_OC TIM_OC3
#define PWM_SERVO_3_OC_BIT (1<<2)
#else
#define PWM_SERVO_3_OC_BIT 0
#endif
#if USE_PWM4
#define PWM_SERVO_4 3
#define PWM_SERVO_4_TIMER TIM3
#define PWM_SERVO_4_GPIO GPIOB
#define PWM_SERVO_4_PIN GPIO1
#define PWM_SERVO_4_AF 0
#define PWM_SERVO_4_OC TIM_OC4
#define PWM_SERVO_4_OC_BIT (1<<3)
#else
#define PWM_SERVO_4_OC_BIT 0
#endif
/* servos 1-2 on TIM4 */
#define PWM_TIM4_CHAN_MASK (PWM_SERVO_1_OC_BIT|PWM_SERVO_2_OC_BIT)
/* servos 3-4 on TIM3 */
#define PWM_TIM3_CHAN_MASK (PWM_SERVO_3_OC_BIT|PWM_SERVO_4_OC_BIT)
#endif /* CONFIG_CJMCU_H */
sw/airborne/boards/naze32/baro_board.h
+19
View File
@@ -0,0 +1,19 @@
/*
* board specific functions for the Naze32 board
*
*/
#ifndef BOARDS_NAZE32_BARO_H
#define BOARDS_NAZE32_BARO_H
// only for printing the baro type during compilation
#ifndef BARO_BOARD
#define BARO_BOARD BARO_MS5611_I2C
#endif
extern void baro_event(void);
#define BaroEvent baro_event
#endif /* BOARDS_NAZE32_BARO_H */
sw/airborne/boards/naze32_common.h
+360
View File
@@ -0,0 +1,360 @@
#ifndef CONFIG_NAZE32_COMMON_H
#define CONFIG_NAZE32_COMMON_H
#define BOARD_NAZE32
/*
* Onboard LEDs
*/
/* L0 red status led, on PB4 */
#ifndef USE_LED_1
#define USE_LED_1 1
#endif
#define LED_1_GPIO GPIOB
#define LED_1_GPIO_PIN GPIO4
#define LED_1_GPIO_ON gpio_clear
#define LED_1_GPIO_OFF gpio_set
#define LED_1_AFIO_REMAP { \
rcc_periph_clock_enable(RCC_AFIO); \
AFIO_MAPR |= AFIO_MAPR_SWJ_CFG_JTAG_OFF_SW_ON; \
}
/* L1 green status led, on PB3 */
#ifndef USE_LED_2
#define USE_LED_2 1
#endif
#define LED_2_GPIO GPIOB
#define LED_2_GPIO_PIN GPIO3
#define LED_2_GPIO_ON gpio_clear
#define LED_2_GPIO_OFF gpio_set
#define LED_2_AFIO_REMAP ((void)0)
/*
* UART pin configuration
*
* sets on which pins the UARTs are connected
*/
/* UART1 on main port */
#define UART1_GPIO_AF 0
#define UART1_GPIO_PORT_RX GPIO_BANK_USART1_RX
#define UART1_GPIO_RX GPIO_USART1_RX
#define UART1_GPIO_PORT_TX GPIO_BANK_USART1_TX
#define UART1_GPIO_TX GPIO_USART1_TX
/* UART2 on RC I/O, tx pin 5/id "3", rx pin 6/id "4") */
#define UART2_GPIO_AF 0
#define UART2_GPIO_PORT_RX GPIO_BANK_USART2_RX
#define UART2_GPIO_RX GPIO_USART2_RX
#define UART2_GPIO_PORT_TX GPIO_BANK_USART2_TX
#define UART2_GPIO_TX GPIO_USART2_TX
/*
* Spektrum
*/
/* The line that is pulled low at power up to initiate the bind process */
/* set to ppm/servo6 input pin on RC connector for now */
#define SPEKTRUM_BIND_PIN GPIO1
#define SPEKTRUM_BIND_PIN_PORT GPIOA
#define SPEKTRUM_UART1_RCC RCC_USART1
#define SPEKTRUM_UART1_BANK GPIO_BANK_USART1_RX
#define SPEKTRUM_UART1_PIN GPIO_USART1_RX
#define SPEKTRUM_UART1_AF 0
#define SPEKTRUM_UART1_IRQ NVIC_USART1_IRQ
#define SPEKTRUM_UART1_ISR usart1_isr
#define SPEKTRUM_UART1_DEV USART1
#define SPEKTRUM_UART2_RCC RCC_USART2
#define SPEKTRUM_UART2_BANK GPIO_BANK_USART2_RX
#define SPEKTRUM_UART2_PIN GPIO_USART2_RX
#define SPEKTRUM_UART2_AF 0
#define SPEKTRUM_UART2_IRQ NVIC_USART2_IRQ
#define SPEKTRUM_UART2_ISR usart2_isr
#define SPEKTRUM_UART2_DEV USART2
/* PPM
*
* Default: PPM config 1, input on PA0: RC I/O rx_ppm pin 3/id "1"
* 6 servos: PPM config 2, input on PA7: RC I/O unused pin 8/id "6"
*/
#ifndef PPM_CONFIG
#define PPM_CONFIG 2
#else
#define PPM_CONFIG 1
#endif
#if PPM_CONFIG == 1
/* input on PA0 (ppm_in) */
#define USE_PPM_TIM2 1
#define PPM_CHANNEL TIM_IC1
#define PPM_TIMER_INPUT TIM_IC_IN_TI1
#define PPM_IRQ NVIC_TIM2_IRQ
// Capture/Compare InteruptEnable and InterruptFlag
#define PPM_CC_IE TIM_DIER_CC1IE
#define PPM_CC_IF TIM_SR_CC1IF
#define PPM_GPIO_PORT GPIOA
#define PPM_GPIO_PIN GPIO0
#define PPM_GPIO_AF 0
// Move default ADC timer
#if USE_AD_TIM2
#undef USE_AD_TIM2
#endif
#define USE_AD_TIM1 1
#if USE_SERVOS_5AND6
#error PA0 does not work with 6 servos
#endif
#elif PPM_CONFIG == 2
/* input on PA7 (unused) */
#define USE_PPM_TIM3 1
#define PPM_CHANNEL TIM_IC2
#define PPM_TIMER_INPUT TIM_IC_IN_TI2
#define PPM_IRQ NVIC_TIM3_IRQ
// Capture/Compare InteruptEnable and InterruptFlag
#define PPM_CC_IE TIM_DIER_CC2IE
#define PPM_CC_IF TIM_SR_CC2IF
#define PPM_GPIO_PORT GPIOA
#define PPM_GPIO_PIN GPIO7
#define PPM_GPIO_AF 0
// Move default ADC timer
#if USE_AD_TIM1
#undef USE_AD_TIM1
#endif
#define USE_AD_TIM2 1
#else
#error "Unknown PPM config"
#endif // PPM_CONFIG
/*
* ADC
*/
// Internal ADC for battery enabled by default
#ifndef USE_ADC_4
#define USE_ADC_4 1
#endif
#if USE_ADC_4
#define AD1_4_CHANNEL 4
#define ADC_4 AD1_4
#define ADC_4_GPIO_PORT GPIOA
#define ADC_4_GPIO_PIN GPIO4
#endif
/* allow to define ADC_CHANNEL_VSUPPLY in the airframe file*/
#ifndef ADC_CHANNEL_VSUPPLY
#define ADC_CHANNEL_VSUPPLY ADC_4
#endif
/* 10k/1k resistor divider, 11 * 3.3V / 4096 */
#define DefaultVoltageOfAdc(adc) (0.008862*adc)
/* by default activate onboard baro */
#ifndef USE_BARO_BOARD
#define USE_BARO_BOARD 1
#endif
/*
* I2C
*/
/* RC port */
#define I2C2_GPIO_PORT GPIOB
#define I2C2_GPIO_SCL GPIO10
#define I2C2_GPIO_SDA GPIO11
/*
* PWM
*
* Servo output numbering on silkscreen starts with '1'
*
* silk standard USE_SERVOS_5AND6
*
* '1' PA8 servo1
* '2' PA11 servo2
* '3' PB6 servo1 servo3
* '4' PB7 servo2 servo4
* '5' PB8 servo3 servo5
* '6' PB9 servo4 servo6
*
* PPM_in: PA0 PA7
*
*/
#if USE_SERVOS_5AND6
#define ACTUATORS_PWM_NB 6
/* servos 1-2 */
#define PWM_USE_TIM1 1
#define USE_PWM1 1
#define USE_PWM2 1
/* servos 3-6 */
#define PWM_USE_TIM4 1
#define USE_PWM3 1
#define USE_PWM4 1
#define USE_PWM5 1
#define USE_PWM6 1
#if USE_PWM1
#define PWM_SERVO_1 0
#define PWM_SERVO_1_TIMER TIM1
#define PWM_SERVO_1_GPIO GPIOA
#define PWM_SERVO_1_PIN GPIO8
#define PWM_SERVO_1_AF 0
#define PWM_SERVO_1_OC TIM_OC1
#define PWM_SERVO_1_OC_BIT (1<<0)
#else
#define PWM_SERVO_1_OC_BIT 0
#endif
#if USE_PWM2
#define PWM_SERVO_2 1
#define PWM_SERVO_2_TIMER TIM1
#define PWM_SERVO_2_GPIO GPIOA
#define PWM_SERVO_2_PIN GPIO11
#define PWM_SERVO_2_AF 0
#define PWM_SERVO_2_OC TIM_OC4
#define PWM_SERVO_2_OC_BIT (1<<3)
#else
#define PWM_SERVO_2_OC_BIT 0
#endif
#if USE_PWM3
#define PWM_SERVO_3 2
#define PWM_SERVO_3_TIMER TIM4
#define PWM_SERVO_3_GPIO GPIOB
#define PWM_SERVO_3_PIN GPIO6
#define PWM_SERVO_3_AF 0
#define PWM_SERVO_3_OC TIM_OC1
#define PWM_SERVO_3_OC_BIT (1<<0)
#else
#define PWM_SERVO_3_OC_BIT 0
#endif
#if USE_PWM4
#define PWM_SERVO_4 3
#define PWM_SERVO_4_TIMER TIM4
#define PWM_SERVO_4_GPIO GPIOB
#define PWM_SERVO_4_PIN GPIO7
#define PWM_SERVO_4_AF 0
#define PWM_SERVO_4_OC TIM_OC2
#define PWM_SERVO_4_OC_BIT (1<<1)
#else
#define PWM_SERVO_4_OC_BIT 0
#endif
#if USE_PWM5
#define PWM_SERVO_5 4
#define PWM_SERVO_5_TIMER TIM4
#define PWM_SERVO_5_GPIO GPIOB
#define PWM_SERVO_5_PIN GPIO8
#define PWM_SERVO_5_AF 0
#define PWM_SERVO_5_OC TIM_OC3
#define PWM_SERVO_5_OC_BIT (1<<2)
#else
#define PWM_SERVO_5_OC_BIT 0
#endif
#if USE_PWM6
#define PWM_SERVO_6 5
#define PWM_SERVO_6_TIMER TIM4
#define PWM_SERVO_6_GPIO GPIOB
#define PWM_SERVO_6_PIN GPIO9
#define PWM_SERVO_6_AF 0
#define PWM_SERVO_6_OC TIM_OC4
#define PWM_SERVO_6_OC_BIT (1<<3)
#else
#define PWM_SERVO_6_OC_BIT 0
#endif
/* servos 1-2 on TIM1 */
#define PWM_TIM1_CHAN_MASK (PWM_SERVO_1_OC_BIT|PWM_SERVO_2_OC_BIT)
/* servos 3-6 on TIM4 */
#define PWM_TIM4_CHAN_MASK (PWM_SERVO_3_OC_BIT|PWM_SERVO_4_OC_BIT|PWM_SERVO_5_OC_BIT|PWM_SERVO_6_OC_BIT)
#else //USE_SERVOS_5AND6
#define ACTUATORS_PWM_NB 4
/* servos 1-4 */
#define PWM_USE_TIM4 1
#define USE_PWM1 1
#define USE_PWM2 1
#define USE_PWM3 1
#define USE_PWM4 1
#if USE_PWM1
#define PWM_SERVO_1 0
#define PWM_SERVO_1_TIMER TIM4
#define PWM_SERVO_1_GPIO GPIOB
#define PWM_SERVO_1_PIN GPIO6
#define PWM_SERVO_1_AF 0
#define PWM_SERVO_1_OC TIM_OC1
#define PWM_SERVO_1_OC_BIT (1<<0)
#else
#define PWM_SERVO_1_OC_BIT 0
#endif
#if USE_PWM2
#define PWM_SERVO_2 1
#define PWM_SERVO_2_TIMER TIM4
#define PWM_SERVO_2_GPIO GPIOB
#define PWM_SERVO_2_PIN GPIO7
#define PWM_SERVO_2_AF 0
#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 2
#define PWM_SERVO_3_TIMER TIM4
#define PWM_SERVO_3_GPIO GPIOB
#define PWM_SERVO_3_PIN GPIO8
#define PWM_SERVO_3_AF 0
#define PWM_SERVO_3_OC TIM_OC3
#define PWM_SERVO_3_OC_BIT (1<<2)
#else
#define PWM_SERVO_3_OC_BIT 0
#endif
#if USE_PWM4
#define PWM_SERVO_4 3
#define PWM_SERVO_4_TIMER TIM4
#define PWM_SERVO_4_GPIO GPIOB
#define PWM_SERVO_4_PIN GPIO9
#define PWM_SERVO_4_AF 0
#define PWM_SERVO_4_OC TIM_OC4
#define PWM_SERVO_4_OC_BIT (1<<3)
#else
#define PWM_SERVO_4_OC_BIT 0
#endif
/* servos 1-4 on TIM4 */
#define PWM_TIM4_CHAN_MASK (PWM_SERVO_1_OC_BIT|PWM_SERVO_2_OC_BIT|PWM_SERVO_3_OC_BIT|PWM_SERVO_4_OC_BIT)
#endif //USE_SERVOS_5AND6
/* Default actuators driver */
#define DEFAULT_ACTUATORS "subsystems/actuators/actuators_pwm.h"
#define ActuatorDefaultSet(_x,_y) ActuatorPwmSet(_x,_y)
#define ActuatorsDefaultInit() ActuatorsPwmInit()
#define ActuatorsDefaultCommit() ActuatorsPwmCommit()
#endif /* CONFIG_NAZE32_COMMON_H */
sw/airborne/boards/naze32_rev4.h
+11
View File
@@ -0,0 +1,11 @@
#ifndef CONFIG_NAZE32_REV4_H
#define CONFIG_NAZE32_REV4_H
#include "boards/naze32_common.h"
/* Naze32 rev4 has a 8MHz external clock and 72MHz internal */
#define EXT_CLK 8000000
#define AHB_CLK 72000000
#endif /* CONFIG_NAZE32_REV4_H */
sw/airborne/boards/naze32_rev5.h
+14
View File
@@ -0,0 +1,14 @@
#ifndef CONFIG_NAZE32_REV5_H
#define CONFIG_NAZE32_REV5_H
#include "boards/naze32_common.h"
/* Naze32 rev5 has a 12MHz external clock and 72MHz internal */
#define EXT_CLK 12000000
#define AHB_CLK 72000000
/* 16Mbit flash on spi2 (Naze32 full) */
#define SPI_SELECT_SLAVE1_PORT GPIOB
#define SPI_SELECT_SLAVE1_PIN GPIO12
#endif /* CONFIG_NAZE32_REV5_H */
sw/airborne/firmwares/fixedwing/main_ap.c
+4
View File
@@ -178,6 +178,10 @@ void init_ap(void)
mcu_init();
#endif /* SINGLE_MCU */
#if defined(PPRZ_TRIG_INT_COMPR_FLASH)
pprz_trig_int_init();
#endif
/****** initialize and reset state interface ********/
stateInit();
sw/airborne/firmwares/rotorcraft/main.c
+4
View File
@@ -160,6 +160,10 @@ STATIC_INLINE void main_init(void)
{
mcu_init();
#if defined(PPRZ_TRIG_INT_COMPR_FLASH)
pprz_trig_int_init();
#endif
electrical_init();
stateInit();
sw/airborne/math/pprz_trig_int.c
+428
View File
@@ -27,6 +27,7 @@
#include "pprz_trig_int.h"
#include "pprz_algebra_int.h"
#if !defined(PPRZ_TRIG_INT_COMPR_FLASH) || defined(PPRZ_TRIG_INT_TEST)
PPRZ_TRIG_CONST int16_t pprz_trig_int[6434] = { 0,
3, 7, 11, 15, 19, 23, 27, 31, 35, 39, 43, 47, 51, 55, 59, 63,
67, 71, 75, 79, 83, 87, 91, 95, 99, 103, 107, 111, 115, 119, 123, 127,
@@ -432,7 +433,428 @@ PPRZ_TRIG_CONST int16_t pprz_trig_int[6434] = { 0,
16383, 16383, 16383, 16383, 16383, 16383, 16383, 16383, 16383, 16383, 16383, 16383, 16383, 16383, 16383, 16383,
16383
};
#endif
#if defined(PPRZ_TRIG_INT_COMPR_FLASH)
const uint8_t pprz_trig_int_compr[TRIG_INT_COMPR_LEN] = {
0xF3, 0xCF, 0xFF, 0xFC, 0xCF, 0xFF, 0xFC, 0xCF, 0xFF, 0xFC, 0xCF, 0x9E,
0xF3, 0xCF, 0x8F, 0x3C, 0xFF, 0xFC, 0x41, 0xF3, 0xCF, 0x34, 0x9F, 0x3C,
0x4F, 0x3C, 0x1F, 0x3C, 0xFF, 0x34, 0xCF, 0x34, 0xBF, 0x34, 0xAF, 0x34,
0x8F, 0x34, 0x7F, 0x34, 0x6F, 0x34, 0x6F, 0x34, 0x4F, 0x34, 0x4F, 0x34,
0x4F, 0x34, 0x3F, 0x34, 0x2F, 0x34, 0x3F, 0x34, 0x1F, 0x34, 0x1F, 0x34,
0x1F, 0x34, 0x1F, 0x34, 0x0F, 0x34, 0x0F, 0x34, 0x0F, 0x34, 0x0F, 0x34,
0xEE, 0x3E, 0xEE, 0x3E, 0xEE, 0x3E, 0xEE, 0x39, 0xEE, 0x3E, 0xEE, 0x39,
0xEE, 0x39, 0xEE, 0x39, 0xEE, 0x39, 0xEE, 0x34, 0xEE, 0x39, 0xEE, 0x34,
0xEE, 0x34, 0xEE, 0x34, 0xEE, 0x34, 0xEE, 0x34, 0xEE, 0x34, 0xEE, 0xE3,
0x4E, 0xE3, 0x3E, 0xEE, 0xE3, 0x4E, 0xE3, 0x3E, 0xEE, 0xE3, 0x3E, 0x9E,
0xE3, 0x3E, 0xEE, 0xE3, 0x39, 0xEE, 0xE3, 0x39, 0xEE, 0xE3, 0x39, 0x9E,
0xE3, 0x39, 0x9E, 0xE3, 0x39, 0x9E, 0xE3, 0x39, 0x9E, 0xE3, 0x39, 0x9E,
0xE3, 0x34, 0x9E, 0xE3, 0x39, 0x4E, 0xE3, 0x39, 0x4E, 0xE3, 0x39, 0x4E,
0xE3, 0x34, 0x4E, 0xE3, 0x39, 0x4E, 0xE3, 0x34, 0x4E, 0xE3, 0x34, 0x4E,
0xE3, 0x34, 0x4E, 0xE3, 0x34, 0x4E, 0xE3, 0x34, 0x4E, 0xE3, 0xE3, 0x34,
0x4E, 0xE3, 0xE3, 0x34, 0x4E, 0xE3, 0xE3, 0x34, 0x3E, 0x4E, 0xE3, 0xE3,
0x34, 0x3E, 0x3E, 0x4E, 0xE3, 0xE3, 0xE3, 0x34, 0x3E, 0x3E, 0x3E, 0x3E,
0x4E, 0xE3, 0xE3, 0xE3, 0xE3, 0xE3, 0xE3, 0xE3, 0xE3, 0xE3, 0xE3, 0xE3,
0xE3, 0x93, 0xE3, 0xE3, 0xE3, 0xE3, 0x93, 0xE3, 0xE3, 0xE3, 0x93, 0xE3,
0xE3, 0x93, 0xE3, 0xE3, 0x93, 0xE3, 0x93, 0xE3, 0x93, 0xE3, 0x93, 0xE3,
0x93, 0xE3, 0x93, 0xE3, 0x93, 0x93, 0xE3, 0x93, 0x93, 0xE3, 0x93, 0x93,
0xE3, 0x93, 0x93, 0x93, 0xE3, 0x93, 0x93, 0x93, 0xE3, 0x93, 0x93, 0x93,
0x93, 0x93, 0x93, 0xE3, 0x93, 0x93, 0x93, 0x93, 0x93, 0x93, 0x93, 0x93,
0x93, 0x93, 0x93, 0x93, 0x93, 0x93, 0x93, 0x93, 0x93, 0x93, 0x93, 0x43,
0x93, 0x93, 0x93, 0x93, 0x93, 0x93, 0x43, 0x93, 0x93, 0x93, 0x93, 0x43,
0x93, 0x93, 0x93, 0x43, 0x93, 0x93, 0x93, 0x43, 0x93, 0x93, 0x93, 0x43,
0x93, 0x93, 0x43, 0x93, 0x93, 0x43, 0x93, 0x43, 0x93, 0x93, 0x43, 0x93,
0x43, 0x93, 0x93, 0x43, 0x93, 0x43, 0x93, 0x43, 0x93, 0x43, 0x93, 0x43,
0x93, 0x43, 0x93, 0x43, 0x93, 0x43, 0x93, 0x43, 0x93, 0x43, 0x93, 0x43,
0x43, 0x93, 0x43, 0x93, 0x43, 0x43, 0x93, 0x43, 0x93, 0x43, 0x43, 0x93,
0x43, 0x43, 0x93, 0x43, 0x43, 0x93, 0x43, 0x43, 0x93, 0x43, 0x43, 0x93,
0x43, 0x43, 0x43, 0x93, 0x43, 0x43, 0x93, 0x43, 0x43, 0x43, 0x93, 0x43,
0x43, 0x43, 0x43, 0x93, 0x43, 0x43, 0x43, 0x43, 0x93, 0x43, 0x43, 0x43,
0x43, 0x43, 0x93, 0x43, 0x43, 0x43, 0x43, 0x43, 0x93, 0x43, 0x43, 0x43,
0x43, 0x43, 0x43, 0x43, 0x43, 0x93, 0x43, 0x43, 0x43, 0x43, 0x43, 0x43,
0x43, 0x43, 0x43, 0x43, 0x43, 0x43, 0x43, 0x43, 0x43, 0x43, 0x43, 0x43,
0x93, 0x43, 0x43, 0x43, 0x43, 0x43, 0x43, 0x43, 0x43, 0x48, 0x43, 0x43,
0x43, 0x43, 0x43, 0x43, 0x43, 0x43, 0x43, 0x43, 0x43, 0x43, 0x43, 0x43,
0x43, 0x43, 0x43, 0x48, 0x43, 0x43, 0x43, 0x43, 0x43, 0x43, 0x43, 0x43,
0x48, 0x43, 0x43, 0x43, 0x43, 0x43, 0x43, 0x48, 0x43, 0x43, 0x43, 0x43,
0x48, 0x43, 0x43, 0x43, 0x43, 0x48, 0x43, 0x43, 0x43, 0x43, 0x48, 0x43,
0x43, 0x48, 0x43, 0x43, 0x43, 0x48, 0x43, 0x43, 0x48, 0x43, 0x43, 0x43,
0x48, 0x43, 0x43, 0x48, 0x43, 0x48, 0x43, 0x43, 0x48, 0x43, 0x43, 0x48,
0x43, 0x48, 0x43, 0x48, 0x43, 0x43, 0x48, 0x43, 0x48, 0x43, 0x48, 0x43,
0x48, 0x43, 0x48, 0x43, 0x48, 0x43, 0x48, 0x48, 0x43, 0x48, 0x43, 0x48,
0x43, 0x48, 0x48, 0x43, 0x48, 0x48, 0x43, 0x48, 0x48, 0x43, 0x48, 0x48,
0x43, 0x48, 0x48, 0x48, 0x43, 0x48, 0x48, 0x48, 0x48, 0x43, 0x48, 0x48,
0x48, 0x48, 0x48, 0x48, 0x48, 0x48, 0x43, 0x48, 0x48, 0x48, 0x48, 0x48,
0x48, 0x48, 0x4D, 0x48, 0x48, 0x48, 0x48, 0x48, 0x48, 0x48, 0x4D, 0x48,
0x48, 0x48, 0x4D, 0x48, 0x48, 0x48, 0x4D, 0x48, 0x4D, 0x48, 0x48, 0x4D,
0x48, 0x4D, 0x48, 0x4D, 0x48, 0x4D, 0x4D, 0x48, 0x4D, 0x4D, 0x48, 0x4D,
0x4D, 0x4D, 0x4D, 0x4D, 0x48, 0x4D, 0x4D, 0x4D, 0x3D, 0xD4, 0xD4, 0xD4,
0xD4, 0xD4, 0x43, 0x4D, 0x4D, 0x3D, 0xD4, 0xD4, 0x43, 0x4D, 0x3D, 0xD4,
0x43, 0x4D, 0x3D, 0xD4, 0x43, 0x3D, 0xD4, 0x43, 0x3D, 0xD4, 0x43, 0x8D,
0xD4, 0x43, 0x3D, 0xD4, 0x48, 0x8D, 0xD4, 0x43, 0x8D, 0xD4, 0x48, 0x8D,
0xD4, 0x4D, 0x8D, 0xD4, 0x4D, 0xDD, 0xD4, 0x4D, 0xDD, 0xD4, 0x4D, 0xDD,
0x43, 0xDD, 0x43, 0xDD, 0x43, 0xDD, 0x48, 0xDD, 0x48, 0xDD, 0x4D, 0xDD,
0x4D, 0x0F, 0x43, 0x1F, 0x43, 0x2F, 0x43, 0x4F, 0x43, 0x7F, 0x43, 0xCF,
0x43, 0xFF, 0xDB, 0x43, 0xBF, 0x23, 0xFF, 0xDB, 0xF2, 0x3C, 0xF2, 0x36,
0xF2, 0x34, 0xF2, 0x32, 0xF2, 0x31, 0xD2, 0xDD, 0xD2, 0xDD, 0xD2, 0xDD,
0xD2, 0x8D, 0xD2, 0x3D, 0xD2, 0x3D, 0xD2, 0x3D, 0xD2, 0x2D, 0xDD, 0x23,
0x8D, 0xD2, 0x2D, 0xDD, 0xD2, 0x28, 0x8D, 0xD2, 0x28, 0x8D, 0xD2, 0x28,
0x3D, 0xD2, 0x28, 0x3D, 0xD2, 0x23, 0x8D, 0xD2, 0x23, 0x3D, 0xD2, 0x23,
0x2D, 0x3D, 0xD2, 0x23, 0x2D, 0x3D, 0xD2, 0xD2, 0x23, 0x2D, 0x3D, 0xD2,
0xD2, 0xD2, 0xD2, 0xD2, 0xD2, 0xD2, 0xD2, 0xD2, 0xD2, 0xD2, 0x82, 0xD2,
0xD2, 0x82, 0xD2, 0x82, 0xD2, 0xD2, 0x82, 0x82, 0xD2, 0x82, 0xD2, 0x82,
0x82, 0xD2, 0x82, 0x82, 0x82, 0x82, 0xD2, 0x82, 0x82, 0x82, 0x82, 0x82,
0x82, 0x82, 0x82, 0x82, 0x82, 0x82, 0x82, 0x82, 0x82, 0x82, 0x32, 0x82,
0x82, 0x82, 0x82, 0x32, 0x82, 0x82, 0x82, 0x32, 0x82, 0x82, 0x32, 0x82,
0x82, 0x32, 0x82, 0x32, 0x82, 0x82, 0x32, 0x82, 0x32, 0x82, 0x32, 0x82,
0x32, 0x82, 0x32, 0x82, 0x32, 0x82, 0x32, 0x32, 0x82, 0x32, 0x82, 0x32,
0x32, 0x82, 0x32, 0x32, 0x82, 0x32, 0x32, 0x32, 0x82, 0x32, 0x32, 0x82,
0x32, 0x32, 0x32, 0x82, 0x32, 0x32, 0x32, 0x32, 0x82, 0x32, 0x32, 0x32,
0x32, 0x32, 0x32, 0x82, 0x32, 0x32, 0x32, 0x32, 0x32, 0x32, 0x32, 0x32,
0x32, 0x82, 0x32, 0x32, 0x32, 0x32, 0x32, 0x32, 0x32, 0x32, 0x32, 0x32,
0x32, 0x32, 0x32, 0x32, 0x32, 0x32, 0x32, 0x32, 0x32, 0x37, 0x32, 0x32,
0x32, 0x32, 0x32, 0x32, 0x32, 0x32, 0x32, 0x37, 0x32, 0x32, 0x32, 0x32,
0x32, 0x37, 0x32, 0x32, 0x32, 0x32, 0x37, 0x32, 0x32, 0x32, 0x37, 0x32,
0x32, 0x32, 0x37, 0x32, 0x32, 0x37, 0x32, 0x32, 0x37, 0x32, 0x32, 0x37,
0x32, 0x37, 0x32, 0x32, 0x37, 0x32, 0x37, 0x32, 0x37, 0x32, 0x37, 0x32,
0x37, 0x32, 0x37, 0x32, 0x37, 0x37, 0x32, 0x37, 0x37, 0x32, 0x37, 0x37,
0x32, 0x37, 0x37, 0x32, 0x37, 0x37, 0x37, 0x37, 0x32, 0x37, 0x37, 0x37,
0x37, 0x37, 0x37, 0x37, 0x37, 0x37, 0x37, 0x37, 0x37, 0x37, 0x37, 0x37,
0x3C, 0x37, 0x37, 0x37, 0x3C, 0x37, 0x37, 0x3C, 0x37, 0x37, 0x3C, 0x37,
0x3C, 0x3C, 0x37, 0x3C, 0x37, 0x3C, 0x3C, 0x3C, 0x3C, 0x3C, 0x3C, 0x3C,
0x3C, 0x3C, 0x3C, 0x3C, 0x3C, 0x2C, 0xC3, 0xC3, 0x32, 0x3C, 0x2C, 0xC3,
0x32, 0x2C, 0xC3, 0x32, 0x2C, 0xC3, 0x32, 0x2C, 0xC3, 0x32, 0x7C, 0xC3,
0x37, 0x2C, 0xC3, 0x37, 0xCC, 0xC3, 0x37, 0xCC, 0xC3, 0x37, 0xCC, 0x32,
0xCC, 0xC3, 0x2C, 0xC3, 0x2C, 0xC3, 0x7C, 0xC3, 0x7C, 0xC3, 0xCC, 0xF3,
0x21, 0xF3, 0x21, 0xF3, 0x24, 0xF3, 0x27, 0xF3, 0xA1, 0xF3, 0xAF, 0x3F,
0x12, 0x2F, 0x1A, 0x7F, 0x12, 0x3F, 0x12, 0x2F, 0x12, 0x0F, 0x12, 0xCC,
0x1C, 0xCC, 0x17, 0xCC, 0x17, 0xCC, 0x12, 0xCC, 0xC1, 0x2C, 0xC1, 0x1C,
0xCC, 0xC1, 0x17, 0x7C, 0xC1, 0x17, 0x7C, 0xC1, 0x17, 0x7C, 0xC1, 0x12,
0x2C, 0xC1, 0x12, 0x2C, 0xC1, 0x12, 0x2C, 0xC1, 0x12, 0x2C, 0xC1, 0xC1,
0x12, 0x1C, 0x1C, 0x2C, 0xC1, 0xC1, 0xC1, 0xC1, 0xC1, 0xC1, 0xC1, 0x71,
0xC1, 0xC1, 0xC1, 0x71, 0xC1, 0x71, 0xC1, 0x71, 0xC1, 0x71, 0x71, 0xC1,
0x71, 0x71, 0xC1, 0x71, 0x71, 0x71, 0x71, 0x71, 0x71, 0x71, 0x71, 0x71,
0x71, 0x71, 0x71, 0x71, 0x71, 0x71, 0x71, 0x71, 0x21, 0x71, 0x71, 0x71,
0x21, 0x71, 0x71, 0x21, 0x71, 0x71, 0x21, 0x71, 0x71, 0x21, 0x71, 0x21,
0x71, 0x21, 0x71, 0x21, 0x71, 0x21, 0x71, 0x21, 0x71, 0x21, 0x21, 0x71,
0x21, 0x21, 0x71, 0x21, 0x21, 0x71, 0x21, 0x21, 0x71, 0x21, 0x21, 0x21,
0x71, 0x21, 0x21, 0x21, 0x71, 0x21, 0x21, 0x21, 0x21, 0x21, 0x71, 0x21,
0x21, 0x21, 0x21, 0x21, 0x21, 0x21, 0x21, 0x21, 0x71, 0x21, 0x21, 0x21,
0x21, 0x21, 0x21, 0x21, 0x21, 0x21, 0x21, 0x21, 0x21, 0x21, 0x21, 0x21,
0x21, 0x26, 0x21, 0x21, 0x21, 0x21, 0x21, 0x21, 0x21, 0x21, 0x26, 0x21,
0x21, 0x21, 0x21, 0x21, 0x26, 0x21, 0x21, 0x21, 0x21, 0x26, 0x21, 0x21,
0x21, 0x26, 0x21, 0x21, 0x26, 0x21, 0x21, 0x26, 0x21, 0x26, 0x21, 0x21,
0x26, 0x21, 0x26, 0x21, 0x26, 0x21, 0x26, 0x21, 0x26, 0x21, 0x26, 0x21,
0x26, 0x21, 0x26, 0x26, 0x21, 0x26, 0x26, 0x21, 0x26, 0x26, 0x26, 0x26,
0x21, 0x26, 0x26, 0x26, 0x26, 0x26, 0x26, 0x26, 0x26, 0x26, 0x26, 0x26,
0x26, 0x26, 0x26, 0x26, 0x26, 0x2B, 0x26, 0x26, 0x2B, 0x26, 0x26, 0x2B,
0x26, 0x2B, 0x26, 0x2B, 0x2B, 0x26, 0x2B, 0x2B, 0x2B, 0x2B, 0x2B, 0x2B,
0x2B, 0x2B, 0x2B, 0x2B, 0x1B, 0xB2, 0xB2, 0x21, 0x2B, 0x1B, 0xB2, 0x21,
0x1B, 0xB2, 0x21, 0x1B, 0xB2, 0x21, 0x1B, 0xB2, 0x26, 0x6B, 0xB2, 0x26,
0x6B, 0xB2, 0x26, 0xBB, 0xB2, 0x2B, 0xBB, 0xB2, 0x2B, 0xBB, 0x21, 0xBB,
0x26, 0xBB, 0x26, 0xBB, 0x2B, 0x1F, 0x21, 0x1F, 0x21, 0x5F, 0x21, 0xAF,
0x21, 0xFF, 0xF9, 0x9B, 0xF0, 0x1A, 0xF0, 0x15, 0xF0, 0x11, 0xF0, 0x10,
0xF0, 0x10, 0xB0, 0x6B, 0xB0, 0x1B, 0xB0, 0x1B, 0xB0, 0x1B, 0xB0, 0x0B,
0xBB, 0xB0, 0x06, 0xBB, 0xB0, 0x06, 0x6B, 0xB0, 0x01, 0x6B, 0xB0, 0x01,
0x1B, 0xB0, 0x01, 0x1B, 0xB0, 0x01, 0x1B, 0xB0, 0x01, 0x0B, 0x1B, 0xB0,
0xB0, 0xB0, 0x01, 0x0B, 0x0B, 0x0B, 0x0B, 0x0B, 0x0B, 0x06, 0x0B, 0x0B,
0x0B, 0x06, 0x0B, 0x06, 0x0B, 0x06, 0x0B, 0x06, 0x06, 0x0B, 0x06, 0x06,
0x06, 0x06, 0x0B, 0x06, 0x06, 0x06, 0x06, 0x06, 0x06, 0x06, 0x06, 0x06,
0x01, 0x06, 0x06, 0x06, 0x06, 0x01, 0x06, 0x06, 0x06, 0x01, 0x06, 0x06,
0x01, 0x06, 0x01, 0x06, 0x06, 0x01, 0x06, 0x01, 0x06, 0x01, 0x06, 0x01,
0x06, 0x01, 0x01, 0x06, 0x01, 0x06, 0x01, 0x01, 0x06, 0x01, 0x01, 0x06,
0x01, 0x01, 0x01, 0x06, 0x01, 0x01, 0x01, 0x06, 0x01, 0x01, 0x01, 0x01,
0x01, 0x06, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x06, 0x01, 0x01,
0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01,
0x01, 0x01, 0x01, 0x51, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01,
0x51, 0x01, 0x01, 0x01, 0x01, 0x51, 0x01, 0x01, 0x01, 0x01, 0x51, 0x01,
0x01, 0x51, 0x01, 0x01, 0x51, 0x01, 0x01, 0x51, 0x01, 0x01, 0x51, 0x01,
0x51, 0x01, 0x51, 0x01, 0x51, 0x01, 0x51, 0x01, 0x51, 0x01, 0x51, 0x01,
0x51, 0x51, 0x01, 0x51, 0x51, 0x01, 0x51, 0x51, 0x51, 0x51, 0x01, 0x51,
0x51, 0x51, 0x51, 0x51, 0x51, 0x51, 0x51, 0x51, 0x51, 0x51, 0x51, 0x51,
0x51, 0xA1, 0x51, 0x51, 0x51, 0xA1, 0x51, 0xA1, 0x51, 0xA1, 0x51, 0xA1,
0x51, 0xA1, 0xA1, 0xA1, 0xA1, 0x51, 0xA1, 0xA1, 0xA1, 0x10, 0x1A, 0x1A,
0x1A, 0x0A, 0xA1, 0xA1, 0x10, 0x1A, 0x0A, 0xA1, 0x10, 0x0A, 0xA1, 0x10,
0x0A, 0xA1, 0x15, 0x0A, 0xA1, 0x15, 0x5A, 0xA1, 0x15, 0xAA, 0xA1, 0x15,
0xAA, 0xA1, 0x0A, 0xA1, 0x1A, 0xAA, 0x15, 0xAA, 0x15, 0xAA, 0x15, 0x0F,
0x10, 0x1F, 0x10, 0x3F, 0x10, 0x8F, 0x10, 0xFF, 0xA8, 0x00
};
#endif
#if defined(PPRZ_TRIG_INT_COMPR_FLASH)
#if defined(PPRZ_TRIG_INT_COMPR_HIGHEST)
uint8_t data_buf_4[TRIG_INT_SIZE / 2];
uint16_t tree_buf_4[(1 << TREE_SIZE_4)];
void table_encode_4(int16_t val, int16_t val_prev, int16_t cnt, int16_t *tab)
{
/* build 4 bit table */
if ((cnt % 2) == 0) {
data_buf_4[cnt / 2] = val % 0x10;
} else {
data_buf_4[cnt / 2] |= (val % 0x10) << 4;
}
/* build 4 bit tree */
if (cnt == 0) {
tree_buf_4[(*tab)++] = cnt;
} else if ((val / 0x10) != (val_prev / 0x10)) {
tree_buf_4[(*tab)++] = cnt;
}
}
int16_t pprz_trig_int_4(int16_t val)
{
int8_t j;
int16_t k;
/* search tree for the 4 bit data */
k = 1 << (TREE_SIZE_4 - 1);
for (j = TREE_SIZE_4 - 2; j >= 0; j--) {
if (val < tree_buf_4[k]) {
k -= (1 << j);
} else {
k += (1 << j);
}
}
if (val < tree_buf_4[k]) {
k -= 1;
}
return (k << 4) | ((data_buf_4[val / 2] >> ((val % 2) ? 4 : 0)) & 0xF);
}
#endif
#if defined(PPRZ_TRIG_INT_COMPR_HIGH)
uint8_t data_buf_8[TRIG_INT_SIZE];
uint16_t tree_buf_8[(1 << TREE_SIZE_8)];
void table_encode_8(int16_t val, int16_t val_prev, int16_t cnt, int16_t *tab)
{
/* build 8 bit table */
data_buf_8[cnt] = val % 0x100;
/* build tree for 8 bit */
if (cnt == 0) {
tree_buf_8[(*tab)++] = cnt;
} else if ((val / 0x100) != (val_prev / 0x100)) {
tree_buf_8[(*tab)++] = cnt;
}
}
int16_t pprz_trig_int_8(int16_t val)
{
int8_t j, k;
/* search the tree for 8 bit data */
k = 1 << (TREE_SIZE_8 - 1);
for (j = TREE_SIZE_8 - 2; j >= 0; j--) {
if (val < tree_buf_8[k]) {
k -= (1 << j);
} else {
k += (1 << j);
}
}
if (val < tree_buf_8[k]) {
k -= 1;
}
return (k << 8) | data_buf_8[val];
}
#endif
#if defined(PPRZ_TRIG_INT_COMPR_LOW)
uint8_t data_buf_12[(TRIG_INT_SIZE * 3) / 2];
#if USE_REAL_TABLE_12_USE
uint16_t tree_buf_12[(1 << TREE_SIZE_12)];
#endif
void table_encode_12(int16_t val, int16_t val_prev, int16_t cnt, int16_t *tab)
{
/* build 12 bit table */
data_buf_12[cnt] = val % 0x100;
if ((cnt % 2) == 0) {
data_buf_12[TRIG_INT_SIZE + (cnt / 2)] = (val & 0xF00) >> 8 ;
} else {
data_buf_12[TRIG_INT_SIZE + (cnt / 2)] |= (val & 0xF00) >> 4;
}
#if USE_REAL_TABLE_12_USE
/* USE_REAL_TABLE_12_USE uses the original table instead of unrolled "if".
* Unrolling makes it faster but uses slightly more code.
* For the higher compressions it is not faster/creates more code.
* The option is for debugging/understanding only.
*/
/* build tree for 12 bit */
if (cnt == 0) {
tree_buf_12[(*tab)++] = cnt;
} else if ((val / 0x1000) != (val_prev / 0x1000)) {
tree_buf_12[(*tab)++] = cnt;
}
#else
// suppress unused-parameter warning
(void)val_prev;
(void)tab;
#endif
}
int16_t pprz_trig_int_12(int16_t val)
{
#if USE_REAL_TABLE_12_USE
int8_t k;
/* search tree for the 12 bit data */
k = 1 << (TREE_SIZE_12 - 1);
if (val < tree_buf_12[k]) {
k -= 1;
} else {
k += 1;
}
if (val < tree_buf_12[k]) {
k -= 1;
}
return (k << 12) | ((data_buf_12[TRIG_INT_SIZE + (val / 2)] << ((val % 2) ? 4 : 8)) & 0xF00) | (data_buf_12[val]);
#else
int16_t k;
/* search tree for the 12 bit data */
if (val < TREE_BUF_12_2) {
if (val < TREE_BUF_12_1) {
k = (0 << 12);
} else {
k = (1 << 12);
}
} else {
if (val < TREE_BUF_12_3) {
k = (2 << 12);
} else {
k = (3 << 12);
}
}
return k | ((data_buf_12[TRIG_INT_SIZE + (val >> 1)] << ((val % 2) ? 4 : 8)) & 0xF00) | (data_buf_12[val]);
#endif
}
#endif
#if defined(PPRZ_TRIG_INT_COMPR_NONE)
int16_t data_buf_16[TRIG_INT_SIZE];
inline int16_t pprz_trig_int_16(int32_t val)
{
return data_buf_16[val];
}
void table_encode_16(int16_t val, int16_t cnt)
{
data_buf_16[cnt] = val;
}
#endif
uint8_t get_nibble(uint16_t pos)
{
return (pprz_trig_int_compr[pos / 2] >> (4 * (pos % 2))) & 0xF;
}
int pprz_trig_int_init(void)
{
int16_t i, j, k, cnt, temp, val, val_prev;
#if defined(PPRZ_TRIG_INT_COMPR_HIGHEST)
int16_t tab_4 = 0;
#endif
#if defined(PPRZ_TRIG_INT_COMPR_HIGH)
int16_t tab_8 = 0;
#endif
#if defined(PPRZ_TRIG_INT_COMPR_LOW)
int16_t tab_12 = 0;
#endif
cnt = 0;
val = 0;
val_prev = 0;
/* decode run length encoding */
for (i = 0; i < TRIG_INT_RLE_LEN;) {
temp = get_nibble(i++);
if (temp < 0xF) {
/* single */
for (j = 0; j < (temp / 5) + 1; j++) {
#if defined(PPRZ_TRIG_INT_COMPR_HIGHEST)
table_encode_4(val, val_prev, cnt, &tab_4);
#endif
#if defined(PPRZ_TRIG_INT_COMPR_HIGH)
table_encode_8(val, val_prev, cnt, &tab_8);
#endif
#if defined(PPRZ_TRIG_INT_COMPR_LOW)
table_encode_12(val, val_prev, cnt, &tab_12);
#endif
#if defined(PPRZ_TRIG_INT_COMPR_NONE)
table_encode_16(val, cnt);
#endif
val_prev = val;
cnt++;
val += temp % 5;
if (cnt > TRIG_INT_SIZE) {
return -1;
}
}
} else {
/* multi */
k = get_nibble(i++);
if (i > TRIG_INT_RLE_LEN) {
return -1;
}
temp = get_nibble(i++);
if (i > TRIG_INT_RLE_LEN) {
return -1;
}
k |= (temp & 0x8) << 1;
for (j = 0; j < k + ONE_MIN; j++) {
#if defined(PPRZ_TRIG_INT_COMPR_HIGHEST)
table_encode_4(val, val_prev, cnt, &tab_4);
#endif
#if defined(PPRZ_TRIG_INT_COMPR_HIGH)
table_encode_8(val, val_prev, cnt, &tab_8);
#endif
#if defined(PPRZ_TRIG_INT_COMPR_LOW)
table_encode_12(val, val_prev, cnt, &tab_12);
#endif
#if defined(PPRZ_TRIG_INT_COMPR_NONE)
table_encode_16(val, cnt);
#endif
val_prev = val;
cnt++;
val += temp & 7;
if (cnt > TRIG_INT_SIZE) {
return -1;
}
}
}
}
if (cnt != TRIG_INT_SIZE) {
return -1;
}
return 0;
}
inline int16_t pprz_trig_int_f(int32_t angle)
{
#if defined(PPRZ_TRIG_INT_COMPR_HIGHEST)
return pprz_trig_int_4(angle);
#elif defined(PPRZ_TRIG_INT_COMPR_HIGH)
return pprz_trig_int_8(angle);
#elif defined(PPRZ_TRIG_INT_COMPR_LOW)
return pprz_trig_int_12(angle);
#elif defined(PPRZ_TRIG_INT_COMPR_NONE)
return pprz_trig_int_16(angle);
#endif
}
#endif // PPRZ_TRIG_INT_COMPR_FLASH
int32_t pprz_itrig_sin(int32_t angle)
{
@@ -443,9 +865,15 @@ int32_t pprz_itrig_sin(int32_t angle)
angle = -INT32_ANGLE_PI - angle;
}
if (angle >= 0) {
#if defined(PPRZ_TRIG_INT_COMPR_FLASH)
return pprz_trig_int_f(angle);
} else {
return -pprz_trig_int_f(-angle);
#else
return pprz_trig_int[angle];
} else {
return -pprz_trig_int[-angle];
#endif
}
}
sw/airborne/math/pprz_trig_int.h
+58
View File
@@ -40,13 +40,71 @@ extern "C" {
#define PPRZ_TRIG_CONST
#endif
#if defined(PPRZ_TRIG_INT_TEST)
#define PPRZ_TRIG_INT_COMPR_FLASH
#define PPRZ_TRIG_INT_COMPR_HIGHEST
#define PPRZ_TRIG_INT_COMPR_HIGH
#define PPRZ_TRIG_INT_COMPR_LOW
#define PPRZ_TRIG_INT_COMPR_NONE
#endif
#if defined(PPRZ_TRIG_INT_COMPR_FLASH) && !defined(PPRZ_TRIG_INT_COMPR_HIGHEST) && !defined(PPRZ_TRIG_INT_COMPR_HIGH) && !defined(PPRZ_TRIG_INT_COMPR_LOW)
#define PPRZ_TRIG_INT_COMPR_NONE
#endif
#define TRIG_INT_SIZE 6434
#define TRIG_INT_VAL_MAX 14
#define TREE_SIZE_4 (TRIG_INT_VAL_MAX - 4)
#define TREE_SIZE_8 (TRIG_INT_VAL_MAX - 8)
#define TREE_SIZE_12 (TRIG_INT_VAL_MAX - 12)
/* resulting compressed size */
#define TRIG_INT_RLE_LEN 3379
#define TRIG_INT_COMPR_LEN ((TRIG_INT_RLE_LEN + 1) / 2)
/* minimum size to use multi */
#define ONE_MIN ((3 * 3) + 1)
#define TREE_BUF_12_1 1035
#define TREE_BUF_12_2 2145
#define TREE_BUF_12_3 3474
#if !defined(PPRZ_TRIG_INT_COMPR_FLASH) || defined(PPRZ_TRIG_INT_TEST)
extern PPRZ_TRIG_CONST int16_t pprz_trig_int[];
#endif
extern int32_t pprz_itrig_sin(int32_t angle);
extern int32_t pprz_itrig_cos(int32_t angle);
extern int32_t int32_atan2(int32_t y, int32_t x);
extern int32_t int32_atan2_2(int32_t y, int32_t x);
#if defined(PPRZ_TRIG_INT_COMPR_FLASH)
uint8_t get_nibble(uint16_t pos);
int pprz_trig_int_init(void);
#if defined(PPRZ_TRIG_INT_COMPR_HIGHEST)
void table_encode_4(int16_t val, int16_t val_prev, int16_t cnt, int16_t *tab);
int16_t pprz_trig_int_4(int16_t val);
#endif
#if defined(PPRZ_TRIG_INT_COMPR_HIGH)
void table_encode_8(int16_t val, int16_t val_prev, int16_t cnt, int16_t *tab);
int16_t pprz_trig_int_8(int16_t val);
#endif
#if defined(PPRZ_TRIG_INT_COMPR_LOW)
void table_encode_12(int16_t val, int16_t val_prev, int16_t cnt, int16_t *tab);
int16_t pprz_trig_int_12(int16_t val);
#endif
#if defined(PPRZ_TRIG_INT_COMPR_NONE)
int16_t pprz_trig_int_16(int32_t val);
void table_encode_16(int16_t val, int16_t cnt);
#endif
int16_t pprz_trig_int_f(int32_t angle);
#endif // PPRZ_TRIG_INT_COMPR_FLASH
/* for backwards compatibility */
#define PPRZ_ITRIG_SIN(_s, _a) { _s = pprz_itrig_sin(_a); }
#define PPRZ_ITRIG_COS(_c, _a) { _c = pprz_itrig_cos(_a); }
sw/airborne/subsystems/ahrs/ahrs_aligner.c
+3 -2
View File
@@ -36,7 +36,7 @@
struct AhrsAligner ahrs_aligner;
#define SAMPLES_NB PERIODIC_FREQUENCY
#define SAMPLES_NB 100
static struct Int32Rates gyro_sum;
static struct Int32Vect3 accel_sum;
@@ -98,8 +98,9 @@ void ahrs_aligner_init(void)
#ifndef LOW_NOISE_THRESHOLD
#define LOW_NOISE_THRESHOLD 90000
#endif
/** Number of cycles (100 samples each) with low noise */
#ifndef LOW_NOISE_TIME
#define LOW_NOISE_TIME 1
#define LOW_NOISE_TIME 5
#endif
void ahrs_aligner_run(void)
sw/airborne/test/test_math_trig_compressed.c
+294
View File
@@ -0,0 +1,294 @@
/*
* Copyright (C) 2015 Martin Mueller <martinmm@pfump.org>
*
* 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 test_math_trig_compressed.c
*
* Run flash / RAM compressed sine look up table
*/
/*
simple 4-bit run length encoding for sine table storage in flash
reduces flash usage from 12868 bytes to 1690 bytes
PPRZ_TRIG_INT_COMPR_FLASH
0 0000: 0
1 0001: 1
2 0010: 2
3 0011: 3
4 0100: 4
5 0101: 00
6 0110: 11
7 0111: 22
8 1000: 33
9 1001: 44
10 1010: 000
11 1011: 111
12 1100: 222
13 1101: 333
14 1110: 444
15 1111 nnnn nyyy: ((n+10) * y), n=00000..11111, y=0000..0100
binary trees for sine table storage in RAM
reduces RAM usage from 12868 bytes to
PPRZ_TRIG_INT_COMPR_HIGHEST
4 bit data + 10 bit tree 5265 bytes (6434 * 4 + 1024 * 16 bits)
PPRZ_TRIG_INT_COMPR_HIGH
8 bit data + 6 bit tree 6562 bytes (6434 * 8 + 64 * 16 bits)
PPRZ_TRIG_INT_COMPR_LOW
12 bit data 9647 bytes (6434 * 12)
code size Cortex-M4 thumb2
PPRZ_TRIG_INT_COMPR_HIGHEST 104 bytes
PPRZ_TRIG_INT_COMPR_HIGH 76 bytes
PPRZ_TRIG_INT_COMPR_LOW 84 bytes
- 12 bytes
machine cycles STM32F4 (average over 10 calls)
PPRZ_TRIG_INT_COMPR_HIGHEST 197 cycles
PPRZ_TRIG_INT_COMPR_HIGH 170 cycles
PPRZ_TRIG_INT_COMPR_LOW 46 cycles
- 16 cycles
*/
#define DATALINK_C
#include BOARD_CONFIG
#include "mcu.h"
#include "mcu_periph/sys_time.h"
#include "subsystems/datalink/downlink.h"
#include "led.h"
#include "math/pprz_trig_int.h"
#include "math/pprz_algebra_int.h"
/* cycle counter only exists for STM32 architecture */
#if defined(STM32F1) || defined(STM32F4)
#include <libopencm3/cm3/dwt.h>
#else
#define dwt_read_cycle_counter() 0
#define dwt_enable_cycle_counter() 0
#endif
static inline void main_init(void);
static inline void main_periodic(void);
static inline void main_event(void);
int test_tables(void);
int32_t pprz_itrig_sin_4(int32_t angle);
int32_t pprz_itrig_sin_8(int32_t angle);
int32_t pprz_itrig_sin_12(int32_t angle);
int32_t pprz_itrig_sin_16(int32_t angle);
int test_tables(void)
{
int16_t i;
/* test all functions */
for (i = 0; i < TRIG_INT_SIZE; i++) {
if (pprz_trig_int[i] != pprz_trig_int_4(i)) return -1;
if (pprz_trig_int[i] != pprz_trig_int_8(i)) return -1;
if (pprz_trig_int[i] != pprz_trig_int_12(i)) return -1;
if (pprz_trig_int[i] != pprz_trig_int_16(i)) return -1;
}
return 0;
}
#ifdef TRIG_INT_COMPR_4
int32_t pprz_itrig_sin_4(int32_t angle)
{
INT32_ANGLE_NORMALIZE(angle);
if (angle > INT32_ANGLE_PI_2) {
angle = INT32_ANGLE_PI - angle;
} else if (angle < -INT32_ANGLE_PI_2) {
angle = -INT32_ANGLE_PI - angle;
}
if (angle >= 0) {
return pprz_trig_int_4(angle);
} else {
return -pprz_trig_int_4(-angle);
}
}
#endif
#ifdef TRIG_INT_COMPR_8
int32_t pprz_itrig_sin_8(int32_t angle)
{
INT32_ANGLE_NORMALIZE(angle);
if (angle > INT32_ANGLE_PI_2) {
angle = INT32_ANGLE_PI - angle;
} else if (angle < -INT32_ANGLE_PI_2) {
angle = -INT32_ANGLE_PI - angle;
}
if (angle >= 0) {
return pprz_trig_int_8(angle);
} else {
return -pprz_trig_int_8(-angle);
}
}
#endif
#ifdef TRIG_INT_COMPR_12
int32_t pprz_itrig_sin_12(int32_t angle)
{
INT32_ANGLE_NORMALIZE(angle);
if (angle > INT32_ANGLE_PI_2) {
angle = INT32_ANGLE_PI - angle;
} else if (angle < -INT32_ANGLE_PI_2) {
angle = -INT32_ANGLE_PI - angle;
}
if (angle >= 0) {
return pprz_trig_int_12(angle);
} else {
return -pprz_trig_int_12(-angle);
}
}
#endif
int32_t pprz_itrig_sin_16(int32_t angle)
{
INT32_ANGLE_NORMALIZE(angle);
if (angle > INT32_ANGLE_PI_2) {
angle = INT32_ANGLE_PI - angle;
} else if (angle < -INT32_ANGLE_PI_2) {
angle = -INT32_ANGLE_PI - angle;
}
if (angle >= 0) {
return pprz_trig_int_16(angle);
} else {
return -pprz_trig_int_16(-angle);
}
}
int main(void)
{
main_init();
dwt_enable_cycle_counter();
while (1) {
if (sys_time_check_and_ack_timer(0)) {
main_periodic();
}
main_event();
}
return 0;
}
static inline void main_init(void)
{
mcu_init();
sys_time_register_timer((1. / PERIODIC_FREQUENCY), NULL);
mcu_int_enable();
downlink_init();
}
static inline void main_periodic(void)
{
volatile uint32_t result;
volatile uint32_t pre_time, post_time;
uint32_t result1=0, result2=0, result3=0, result4=0;
static int16_t i = 2342;
i += TRIG_INT_SIZE / 4;
i = i % TRIG_INT_SIZE;
RunOnceEvery(10, {DOWNLINK_SEND_ALIVE(DefaultChannel, DefaultDevice, 16, MD5SUM);});
LED_PERIODIC();
pprz_trig_int_init();
if (test_tables() == 0) {
/* run 10x without a loop */
pre_time = dwt_read_cycle_counter();
result = pprz_trig_int_4(i);
result = pprz_trig_int_4(i);
result = pprz_trig_int_4(i);
result = pprz_trig_int_4(i);
result = pprz_trig_int_4(i);
result = pprz_trig_int_4(i);
result = pprz_trig_int_4(i);
result = pprz_trig_int_4(i);
result = pprz_trig_int_4(i);
result = pprz_trig_int_4(i);
post_time = dwt_read_cycle_counter();
result1 = (post_time - pre_time);
pre_time = dwt_read_cycle_counter();
result = pprz_trig_int_8(i);
result = pprz_trig_int_8(i);
result = pprz_trig_int_8(i);
result = pprz_trig_int_8(i);
result = pprz_trig_int_8(i);
result = pprz_trig_int_8(i);
result = pprz_trig_int_8(i);
result = pprz_trig_int_8(i);
result = pprz_trig_int_8(i);
result = pprz_trig_int_8(i);
post_time = dwt_read_cycle_counter();
result2 = (post_time - pre_time);
pre_time = dwt_read_cycle_counter();
result = pprz_trig_int_12(i);
result = pprz_trig_int_12(i);
result = pprz_trig_int_12(i);
result = pprz_trig_int_12(i);
result = pprz_trig_int_12(i);
result = pprz_trig_int_12(i);
result = pprz_trig_int_12(i);
result = pprz_trig_int_12(i);
result = pprz_trig_int_12(i);
result = pprz_trig_int_12(i);
post_time = dwt_read_cycle_counter();
result3 = (post_time - pre_time);
pre_time = dwt_read_cycle_counter();
result = pprz_trig_int_16(i);
result = pprz_trig_int_16(i);
result = pprz_trig_int_16(i);
result = pprz_trig_int_16(i);
result = pprz_trig_int_16(i);
result = pprz_trig_int_16(i);
result = pprz_trig_int_16(i);
result = pprz_trig_int_16(i);
result = pprz_trig_int_16(i);
result = pprz_trig_int_16(i);
post_time = dwt_read_cycle_counter();
result4 = (post_time - pre_time);
result = result;
DOWNLINK_SEND_CSC_CAN_MSG(DefaultChannel, DefaultDevice, &result1, &result2, &result3, &result4);
}
}
static inline void main_event(void)
{
mcu_event();
}