mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-23 13:24:03 +08:00
*** empty log message ***
This commit is contained in:
Antoine Drouin
@@ -26,14 +26,14 @@
|
||||
</rc_commands>
|
||||
|
||||
<command_laws>
|
||||
<let var="roll" value="0.3 * @ROLL"/>
|
||||
<let var="pitch" value="0.3 * @PITCH"/>
|
||||
<let var="yaw" value="-0.4 * @YAW"/>
|
||||
<let var="roll" value="trim_roll + 0.3 * @ROLL"/>
|
||||
<let var="pitch" value="trim_pitch + 0.3 * @PITCH"/>
|
||||
<let var="yaw" value="trim_yaw + -0.4 * @YAW"/>
|
||||
<let var="throttle" value="@THROTTLE"/>
|
||||
<set servo="MOTOR_FRONT" value="$throttle < 500 ? 0 : $throttle + $pitch - $yaw"/>
|
||||
<set servo="MOTOR_BACK" value="$throttle < 500 ? 0 : $throttle - $pitch - $yaw"/>
|
||||
<set servo="MOTOR_RIGHT" value="$throttle < 500 ? 0 : $throttle + $roll + $yaw"/>
|
||||
<set servo="MOTOR_LEFT" value="$throttle < 500 ? 0 : $throttle - $roll + $yaw"/>
|
||||
<set servo="MOTOR_FRONT" value="rc_values[RADIO_THROTTLE] < 500 ? 0 : $throttle + $pitch - $yaw"/>
|
||||
<set servo="MOTOR_BACK" value="rc_values[RADIO_THROTTLE] < 500 ? 0 : $throttle - $pitch - $yaw"/>
|
||||
<set servo="MOTOR_RIGHT" value="rc_values[RADIO_THROTTLE] < 500 ? 0 : $throttle + $roll + $yaw"/>
|
||||
<set servo="MOTOR_LEFT" value="rc_values[RADIO_THROTTLE] < 500 ? 0 : $throttle - $roll + $yaw"/>
|
||||
</command_laws>
|
||||
|
||||
<section name="adc" prefix="ADC_CHANNEL_">
|
||||
|
||||
@@ -0,0 +1,222 @@
|
||||
<airframe name="Microjet5">
|
||||
<!-- tiny test airframe -->
|
||||
|
||||
<!-- commands section -->
|
||||
<servos>
|
||||
<servo name="GAZ" no="0" min="1000" neutral="1000" max="2000"/>
|
||||
<servo name="AILEVON_LEFT" no="1" max="1150" neutral="1530" min="1950"/>
|
||||
<servo name="AILEVON_RIGHT" no="2" max="1900" neutral="1450" min="1050"/>
|
||||
</servos>
|
||||
|
||||
<commands>
|
||||
<axis name="THROTTLE" failsafe_value="0"/>
|
||||
<axis name="ROLL" failsafe_value="0"/>
|
||||
<axis name="PITCH" failsafe_value="0"/>
|
||||
</commands>
|
||||
|
||||
<rc_commands>
|
||||
<set command="THROTTLE" value="@THROTTLE"/>
|
||||
<set command="ROLL" value="@ROLL"/>
|
||||
<set command="PITCH" value="@PITCH"/>
|
||||
</rc_commands>
|
||||
|
||||
<section name="MIXER">
|
||||
<define name="AILEVON_AILERON_RATE" value="0.3"/>
|
||||
<define name="AILEVON_ELEVATOR_RATE" value="0.7"/>
|
||||
</section>
|
||||
|
||||
<command_laws>
|
||||
<let var="aileron" value="@ROLL * AILEVON_AILERON_RATE"/>
|
||||
<let var="elevator" value="@PITCH * AILEVON_ELEVATOR_RATE"/>
|
||||
<set servo="GAZ" value="@THROTTLE"/>
|
||||
<set servo="AILEVON_LEFT" value="$elevator + $aileron"/>
|
||||
<set servo="AILEVON_RIGHT" value="$elevator - $aileron"/>
|
||||
</command_laws>
|
||||
|
||||
<section name="AUTO1" prefix="AUTO1_">
|
||||
<define name="MAX_ROLL" value="0.6"/>
|
||||
<define name="MAX_PITCH" value="0.6"/>
|
||||
</section>
|
||||
|
||||
<section name="adc" prefix="ADC_CHANNEL_">
|
||||
<define name="IR1" value="ADC_0"/>
|
||||
<define name="IR2" value="ADC_1"/>
|
||||
<define name="IR_TOP" value="ADC_6"/>
|
||||
<define name="IR_NB_SAMPLES" value="16"/>
|
||||
<define name="VSUPPLY" value="ADC_3"/>
|
||||
<define name="GYRO_ROLL" value="ADC_4"/>
|
||||
<define name="GYRO_TEMP" value="ADC_5"/>
|
||||
<define name="GYRO_NB_SAMPLES" value="16"/>
|
||||
|
||||
</section>
|
||||
|
||||
<section name="INFRARED" prefix="IR_">
|
||||
<define name="ROLL_NEUTRAL_DEFAULT" value="0" unit="deg"/>
|
||||
<define name="PITCH_NEUTRAL_DEFAULT" value="0" unit="deg"/>
|
||||
<define name="DEFAULT_CONTRAST" value="200"/>
|
||||
<define name="RAD_OF_IR_CONTRAST" value="0.75"/>
|
||||
<linear name="RollOfIrs" arity="2" coeff1="-0.5" coeff2="0.5"/>
|
||||
<linear name="PitchOfIrs" arity="2" coeff1="-0.5" coeff2="-0.5"/>
|
||||
<linear name="TopOfIr" arity="1" coeff1="-1"/>
|
||||
<define name="RAD_OF_IR_MAX_VALUE" value="0.0045"/>
|
||||
<define name="RAD_OF_IR_MIN_VALUE" value="0.00075"/>
|
||||
<define name="ADC_ROLL_NEUTRAL" value="0"/>
|
||||
<define name="ADC_PITCH_NEUTRAL" value="-512"/>
|
||||
<define name="ADC_TOP_NEUTRAL" value="512"/>
|
||||
</section>
|
||||
|
||||
<section name="GYRO" prefix="GYRO_">
|
||||
<define name="ADC_ROLL_COEF" value="1"/>
|
||||
<define name="ADC_ROLL_NEUTRAL" value="500"/>
|
||||
<define name="ADC_TEMP_NEUTRAL" value="476"/>
|
||||
<define name="ADC_TEMP_SLOPE" value="0"/>
|
||||
</section>
|
||||
|
||||
<section name="BAT">
|
||||
<define name="MILLIAMP_PER_PERCENT" value="0.86"/>
|
||||
<define name="VOLTAGE_ADC_A" value="0.0177531"/>
|
||||
<define name="VOLTAGE_ADC_B" value="0.173626"/>
|
||||
<define name="VoltageOfAdc(adc)" value="(VOLTAGE_ADC_A * adc + VOLTAGE_ADC_B)"/>
|
||||
<define name="LOW_BATTERY" value="9.3" unit="V"/>
|
||||
</section>
|
||||
|
||||
<section name="MISC">
|
||||
<define name="NOMINAL_AIRSPEED" value="12." unit="m/s"/>
|
||||
<define name="CARROT" value="5." unit="s"/>
|
||||
<define name="KILL_MODE_DISTANCE" value="(1.5*MAX_DIST_FROM_HOME)"/>
|
||||
<define name="CONTROL_RATE" value="60" unit="Hz"/>
|
||||
</section>
|
||||
|
||||
<section name="PID">
|
||||
<define name="ROLL_PGAIN" value="5000."/>
|
||||
<define name="PITCH_OF_ROLL" value="0.0"/>
|
||||
<define name="PITCH_PGAIN" value="8000."/>
|
||||
<define name="MAX_ROLL" value="0.35"/>
|
||||
<define name="MAX_PITCH" value="0.35"/>
|
||||
<define name="MIN_PITCH" value="-0.35"/>
|
||||
<define name="AILERON_OF_GAZ" value="0.0"/>
|
||||
</section>
|
||||
|
||||
<section name="ALT" prefix="CLIMB_">
|
||||
<define name="PITCH_PGAIN" value="-0.1"/>
|
||||
<define name="PITCH_IGAIN" value="0.025"/>
|
||||
<define name="PGAIN" value="-0.03"/>
|
||||
<define name="IGAIN" value="0.1"/>
|
||||
<define name="MAX" value="1."/>
|
||||
<define name="LEVEL_GAZ" value="0.50"/>
|
||||
<define name="PITCH_OF_VZ_PGAIN" value="0.05"/>
|
||||
<define name="GAZ_OF_CLIMB" value="0.2" unit="%/(m/s)"/>
|
||||
</section>
|
||||
|
||||
<section name="NAV">
|
||||
<define name="COURSE_PGAIN" value="-0.2"/>
|
||||
<define name="ALTITUDE_PGAIN" value="-0.025"/>
|
||||
<define name="NAV_PITCH" value="0."/>
|
||||
</section>
|
||||
|
||||
<section name="SIMU">
|
||||
<define name="ROLL_RESPONSE_FACTOR" value="2."/>
|
||||
<define name="YAW_RESPONSE_FACTOR" value="1.35"/>
|
||||
<define name="WEIGHT" value="1.3"/>
|
||||
</section>
|
||||
|
||||
<section name="AGGRESSIVE" prefix="AGR_">
|
||||
<define name="CLIMB_GAZ" value="0.75"/><!-- Gaz for Aggressive Climb -->
|
||||
<define name="DESCENT_GAZ" value="0.1"/><!-- Gaz for Aggressive Decent -->
|
||||
<define name="CLIMB_PITCH" value="0.3"/><!-- Pitch for Aggressive Climb -->
|
||||
<define name="DESCENT_PITCH" value="-0.35"/><!-- Pitch for Aggressive Decent -->
|
||||
<define name="BLEND_START" value="30"/><!-- 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_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="GYRO_GAINS">
|
||||
<define name="GYRO_MAX_RATE" value="200."/>
|
||||
<define name="ROLLRATESUM_NB_SAMPLES" value="60"/>
|
||||
<define name="ATTITUDE_PGAIN" value="120.0"/>
|
||||
<define name="ROLL_RATE_PGAIN" value="85.0"/>
|
||||
<define name="ROLL_RATE_IGAIN" value="0.0"/>
|
||||
<define name="ROLL_RATE_DGAIN" value="0.0"/>
|
||||
<define name="ROLL_ATTITUDE_PGAIN" value="120.0"/><!-- new gyro ,was 600-->
|
||||
</section>
|
||||
|
||||
<section name="FAILSAFE" prefix="FAILSAFE_">
|
||||
<define name="DELAY_WITHOUT_GPS" value="1" unit="s"/>
|
||||
<define name="DEFAULT_GAZ" value="0.0" unit="%"/>
|
||||
<define name="DEFAULT_ROLL" value="0.3" unit="rad"/>
|
||||
<define name="DEFAULT_PITCH" value="0.5" unit="rad"/>
|
||||
<define name="HOME_RADIUS" value="100" unit="m"/>
|
||||
</section>
|
||||
|
||||
<section name="DATALINK" prefix="DATALINK_">
|
||||
<define name="DEVICE_TYPE" value="XBEE"/>
|
||||
<define name="DEVICE_ADDRESS" value="...."/>
|
||||
</section>
|
||||
|
||||
|
||||
<makefile>
|
||||
include $(PAPARAZZI_SRC)/conf/autopilot/tiny.makefile
|
||||
|
||||
FLASH_MODE=IAP
|
||||
|
||||
ap.CFLAGS += -DFBW -DAP -DCONFIG=\"tiny.h\" -DLED -DTIME_LED=1
|
||||
ap.srcs = sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c main_fbw.c main_ap.c main.c
|
||||
|
||||
ap.CFLAGS += -DLED_3_BANK=0 -DLED_3_PIN=17 -DLED_4_BANK=0 -DLED_4_PIN=18 -DLED_5_BANK=0 -DLED_5_PIN=19 -DLED_6_BANK=0 -DLED_6_PIN=20
|
||||
|
||||
|
||||
ap.srcs += commands.c
|
||||
|
||||
ap.CFLAGS += -DACTUATORS=\"servos_4015_hw.h\" -DSERVOS_4015
|
||||
ap.srcs += actuators.c $(SRC_ARCH)/servos_4015_hw.c
|
||||
|
||||
ap.CFLAGS += -DRADIO_CONTROL -DRADIO_CONTROL_TYPE=RC_FUTABA
|
||||
ap.srcs += radio_control.c $(SRC_ARCH)/ppm_hw.c
|
||||
|
||||
ap.CFLAGS += -DDOWNLINK -DUSE_UART0 -DDOWNLINK_TRANSPORT=XBeeTransport -DDOWNLINK_FBW_DEVICE=Uart0 -DDOWNLINK_AP_DEVICE=Uart0 -DXBEE_UART=Uart0 -DDATALINK=XBEE -DUART0_BAUD=B9600
|
||||
ap.srcs += downlink.c $(SRC_ARCH)/uart_hw.c datalink.c xbee.c
|
||||
|
||||
#ap.CFLAGS += -DDOWNLINK -DUSE_UART0 -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_FBW_DEVICE=Uart0 -DDOWNLINK_AP_DEVICE=Uart0 -DPPRZ_UART=Uart0 -DDATALINK=PPRZ -DUART0_BAUD=B9600
|
||||
#ap.srcs += downlink.c $(SRC_ARCH)/uart_hw.c datalink.c pprz_transport.c
|
||||
|
||||
|
||||
ap.CFLAGS += -DINTER_MCU
|
||||
ap.srcs += inter_mcu.c
|
||||
|
||||
ap.CFLAGS += -DADC -DUSE_AD0 -DUSE_AD0_2 -DUSE_AD0_3 -DUSE_AD1 -DUSE_AD1_5 -DUSE_AD1_7 -DUSE_AD1_3 -DUSE_AD1_5
|
||||
#ap.CFLAGS += -DADC -DUSE_AD0 -DUSE_AD0_1 -DUSE_AD0_2 -DUSE_AD0_3 -DUSE_AD1 -DUSE_AD1_5 -DUSE_AD1_7 -DUSE_AD1_3 -DUSE_AD1_5
|
||||
ap.srcs += $(SRC_ARCH)/adc_hw.c
|
||||
|
||||
ap.CFLAGS += -DGPS -DUBX -DUSE_UART1 -DGPS_LINK=Uart1 -DUART1_BAUD=B19200
|
||||
|
||||
ap.srcs += gps_ubx.c gps.c
|
||||
|
||||
ap.CFLAGS += -DINFRARED
|
||||
ap.srcs += infrared.c estimator.c
|
||||
|
||||
ap.CFLAGS += -DNAV
|
||||
ap.srcs += nav.c pid.c
|
||||
|
||||
|
||||
ap.CFLAGS += -DGYRO -DSPARK_FUN -DATTITUDE_RATE_MODE
|
||||
ap.srcs += gyro.c
|
||||
|
||||
# Harware In The Loop
|
||||
|
||||
#ap.CFLAGS += -DHITL
|
||||
|
||||
|
||||
# Config for SITL simulation
|
||||
include $(PAPARAZZI_SRC)/conf/autopilot/sitl.makefile
|
||||
|
||||
test.CFLAGS += -DFBW -DCONFIG=\"tiny.h\" -DLED -DTIME_LED=1 -DACTUATORS=\"servos_4015_hw.h\" -DSERVOS_4015 -DUSE_UART0 -DUART0_BAUD=B9600 -DDATALINK=PPRZ -DPPRZ_UART=Uart0
|
||||
test.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c pprz_transport.c setup_actuators.c $(SRC_ARCH)/uart_hw.c $(SRC_ARCH)/servos_4015_hw.c main.c
|
||||
|
||||
#tunel.CFLAGS += -DFBW -DCONFIG=\"tiny.h\" -DLED -DTIME_LED=1
|
||||
tunnel.srcs += $(SRC_ARCH)/uart_tunnel.c
|
||||
|
||||
|
||||
</makefile>
|
||||
</airframe>
|
||||
@@ -21,20 +21,20 @@
|
||||
<dl_setting VAR="pitch_pgain" MAX="15000" STEP="250" MIN="1000"/>
|
||||
</dl_settings>
|
||||
|
||||
<dl_settings name="rate_loop">
|
||||
<dl_setting VAR="roll_rate_pgain" MIN="0." STEP="1.0" MAX="2000."/>
|
||||
<dl_setting VAR="attitude_pgain" MAX="100" STEP="1" MIN="10"/>
|
||||
</dl_settings>
|
||||
|
||||
<dl_settings name="nav">
|
||||
<dl_setting VAR="climb_level_gaz" MAX="0.7" STEP="0.05" MIN="0.3"/>
|
||||
<dl_setting VAR="climb_pgain" MAX="-0.01" STEP="0.01" MIN="-0.5"/>
|
||||
<dl_setting VAR="course_pgain" MAX="-0.1" STEP="0.05" MIN="-2"/>
|
||||
</dl_settings>
|
||||
</dl_settings>
|
||||
|
||||
|
||||
<!--
|
||||
<dl_setting VAR="roll_rate_pgain" MIN="0." STEP="1.0" MAX="2000."/>
|
||||
<dl_setting VAR="attitude_pgain" MAX="100" STEP="1" MIN="10"/>
|
||||
-->
|
||||
</dl_settings>
|
||||
|
||||
|
||||
<waypoints>
|
||||
<waypoint NAME="HOME" X="0" Y="0"/>
|
||||
<waypoint NAME="1" X="200" Y="0"/>
|
||||
@@ -43,9 +43,11 @@
|
||||
<waypoint alt="450." name="S2" x="136.75000196" y="308.874998301"/>
|
||||
</waypoints>
|
||||
|
||||
<!--
|
||||
<include name="fs" procedure="failsafe_modes.xml">
|
||||
<arg name="kill_pitch" value="-0.5"/>
|
||||
</include>
|
||||
-->
|
||||
|
||||
<blocks>
|
||||
<block NAME="wait GPS">
|
||||
|
||||
@@ -4,23 +4,38 @@
|
||||
|
||||
<dl_settings>
|
||||
|
||||
<dl_setting VAR="roll_dot_pid.p_gain" MAX="6000" STEP="100" MIN="2000"/>
|
||||
<dl_setting VAR="roll_dot_pid.i_gain" MAX="1000" STEP="10" MIN="0"/>
|
||||
<dl_setting VAR="roll_dot_pid.d_gain" MAX="10" STEP="0.5" MIN="0"/>
|
||||
<dl_settings name="trim">
|
||||
<dl_setting var="trim_roll" min="-1000" step="10" max="1000"/>
|
||||
<dl_setting var="trim_pitch" min="-1000" step="10" max="1000"/>
|
||||
<dl_setting var="trim_yaw" min="-1000" step="10" max="1000"/>
|
||||
</dl_settings>
|
||||
|
||||
<dl_setting VAR="pitch_dot_pid.p_gain" MAX="-2000" STEP="100" MIN="-6000"/>
|
||||
<dl_setting VAR="pitch_dot_pid.i_gain" MAX="1000" STEP="10" MIN="0"/>
|
||||
<dl_settings name="rate">
|
||||
|
||||
<dl_setting VAR="roll_dot_pid.p_gain" MAX="6000" STEP="100" MIN="2000"/>
|
||||
<dl_setting VAR="roll_dot_pid.i_gain" MAX="750" STEP="0.05" MIN="0"/>
|
||||
<dl_setting VAR="roll_dot_pid.d_gain" MAX="10" STEP="0.5" MIN="0"/>
|
||||
<dl_setting VAR="roll_dot_pid.sum_err" MAX="1" STEP="1" MIN="0"/>
|
||||
|
||||
<dl_setting VAR="pitch_dot_pid.p_gain" MAX="-2000" STEP="100" MIN="-16000"/>
|
||||
<dl_setting VAR="pitch_dot_pid.i_gain" MAX="5" STEP="0.05" MIN="0"/>
|
||||
<dl_setting VAR="pitch_dot_pid.d_gain" MAX="10" STEP="0.5" MIN="0"/>
|
||||
<dl_setting VAR="pitch_dot_pid.sum_err" MAX="1" STEP="1" MIN="0"/>
|
||||
|
||||
<dl_setting VAR="yaw_dot_pid.p_gain" MAX="-2000" STEP="100" MIN="-6000"/>
|
||||
<dl_setting VAR="yaw_dot_pid.i_gain" MAX="10" STEP="0.5" MIN="0"/>
|
||||
<dl_setting VAR="yaw_dot_pid.i_gain" MAX="5" STEP="0.05" MIN="0"/>
|
||||
<dl_setting VAR="yaw_dot_pid.d_gain" MAX="10" STEP="0.5" MIN="0"/>
|
||||
<dl_setting VAR="yaw_dot_pid.sum_err" MAX="1" STEP="1" MIN="0"/>
|
||||
|
||||
</dl_settings>
|
||||
|
||||
<dl_settings name="att">
|
||||
|
||||
<dl_setting VAR="roll_pid.p_gain" MAX="-0.1" STEP="0.1" MIN="-5."/>
|
||||
<dl_setting VAR="roll_pid.i_gain" MAX="10" STEP="0.1" MIN="0"/>
|
||||
<dl_setting VAR="roll_pid.d_gain" MAX="10" STEP="0.1" MIN="0"/>
|
||||
|
||||
<dl_setting VAR="pitch_pid.p_gain" MAX="-.1" STEP="0.1" MIN="-5."/>
|
||||
<dl_setting VAR="pitch_pid.p_gain" MAX="-.1" STEP="0.1" MIN="-15."/>
|
||||
<dl_setting VAR="pitch_pid.i_gain" MAX="10" STEP="0.1" MIN="0"/>
|
||||
<dl_setting VAR="pitch_pid.d_gain" MAX="10" STEP="0.1" MIN="0"/>
|
||||
|
||||
@@ -28,6 +43,14 @@
|
||||
<dl_setting VAR="yaw_pid.i_gain" MAX="10" STEP="0.1" MIN="0"/>
|
||||
<dl_setting VAR="yaw_pid.d_gain" MAX="10" STEP="0.1" MIN="0"/>
|
||||
|
||||
<dl_setting VAR="yaw_pid.set_point" MAX="3" STEP="0.1" MIN="-3"/>
|
||||
<dl_setting VAR="pitch_pid.set_point" MAX="0.3" STEP="0.05" MIN="-0.3"/>
|
||||
<dl_setting VAR="roll_pid.set_point" MAX="0.3" STEP="0.05" MIN="-0.3"/>
|
||||
|
||||
</dl_settings>
|
||||
|
||||
<dl_settings name="alt">
|
||||
|
||||
<dl_setting VAR="ctl_grz_z_dot_pgain" MAX="-500" STEP="100" MIN="-6000"/>
|
||||
<dl_setting VAR="ctl_grz_z_dot_igain" MAX="5" STEP="0.05" MIN="0"/>
|
||||
<dl_setting VAR="ctl_grz_z_dot_dgain" MAX="10" STEP="0.5" MIN="0"/>
|
||||
@@ -38,10 +61,23 @@
|
||||
<dl_setting VAR="ctl_grz_z_igain" MAX="10" STEP="0.5" MIN="0"/>
|
||||
<dl_setting VAR="ctl_grz_z_dgain" MAX="10" STEP="0.5" MIN="0"/>
|
||||
|
||||
</dl_settings>
|
||||
|
||||
<dl_settings name="speed">
|
||||
|
||||
<dl_setting VAR="ve_pid.p_gain" MAX="-0.01" STEP="0.01" MIN="-0.5"/>
|
||||
<dl_setting VAR="ve_pid.i_gain" MAX="10" STEP="0.1" MIN="0"/>
|
||||
<dl_setting VAR="ve_pid.d_gain" MAX="10" STEP="0.1" MIN="0"/>
|
||||
|
||||
<dl_setting VAR="estimator_hspeed_mod" MAX="2" STEP="0.1" MIN="0"/>
|
||||
<dl_setting VAR="estimator_hspeed_dir" MAX="3" STEP="0.1" MIN="-3"/>
|
||||
|
||||
<dl_setting VAR="ve_pid.sum_err" MAX="1" STEP="1" MIN="0"/>
|
||||
<dl_setting VAR="vn_pid.sum_err" MAX="1" STEP="1" MIN="0"/>
|
||||
<dl_setting VAR="sum_err_reset" MAX="1" STEP="1" MIN="0"/>
|
||||
|
||||
</dl_settings>
|
||||
|
||||
</dl_settings>
|
||||
|
||||
|
||||
|
||||
@@ -3,14 +3,14 @@
|
||||
<telemetry>
|
||||
<process name="Ap">
|
||||
<mode name="default">
|
||||
<message name="PPRZ_MODE" period="5"/>
|
||||
<message name="DL_VALUE" period="1"/>
|
||||
<message name="RANGEFINDER" period="2"/>
|
||||
<message name="GRZ_MEASURE" period="0.5"/>
|
||||
<message name="PPRZ_MODE" period="5"/>
|
||||
<message name="RANGEFINDER" period="0.2"/>
|
||||
<message name="GRZ_RATE_LOOP" period="0.5"/>
|
||||
<message name="GRZ_ATTITUDE_LOOP" period="0.5"/>
|
||||
<message name="SPEED_LOOP" period="0.5"/>
|
||||
<message name="ATTITUDE" period="0.5"/>
|
||||
<message name="GRZ_ATTITUDE_LOOP" period="0.5"/>
|
||||
<message name="GRZ_MEASURE" period="0.5"/>
|
||||
</mode>
|
||||
<mode name="debug">
|
||||
<message name="PPRZ_MODE" period="5"/>
|
||||
|
||||
+28
-6
@@ -1,4 +1,5 @@
|
||||
|
||||
#include "std.h"
|
||||
#include "ahrs_new.h"
|
||||
|
||||
#include <math.h>
|
||||
@@ -118,7 +119,7 @@ real_t Qdot[4];
|
||||
*/
|
||||
//#define Q_gyro 0.0075
|
||||
/* I measured about 0.009 rad/s noise */
|
||||
#define Q_gyro 8e-05
|
||||
#define Q_gyro 8e-03
|
||||
|
||||
|
||||
/*
|
||||
@@ -131,6 +132,7 @@ real_t Qdot[4];
|
||||
#define R_pitch 1.3 * 1.3
|
||||
#define R_roll 1.3 * 1.3
|
||||
#define R_yaw 2.5 * 2.5
|
||||
// #define R_yaw 1. * 1.
|
||||
//#define R_pitch 0.1 * 0.1
|
||||
//#define R_roll 0.1 * 0.1
|
||||
//#define R_yaw 0.5 * 0.5
|
||||
@@ -535,13 +537,17 @@ run_kalman(
|
||||
bias_p += K[4] * error;
|
||||
bias_q += K[5] * error;
|
||||
bias_r += K[6] * error;
|
||||
|
||||
/* Bound(bias_p, -0.1, 0.1); */
|
||||
/* Bound(bias_q, -0.1, 0.1); */
|
||||
/* Bound(bias_r, -0.1, 0.1); */
|
||||
#endif
|
||||
|
||||
/*
|
||||
* We would normally normalize our quaternion here, but
|
||||
* instead we will allow our caller to do it
|
||||
*/
|
||||
//norm_quat();
|
||||
// norm_quat();
|
||||
}
|
||||
|
||||
|
||||
@@ -660,11 +666,27 @@ ahrs_pitch_update(
|
||||
*/
|
||||
real_t ahrs_heading_of_mag( const int16_t* mag ) {
|
||||
const real_t ctheta = cos( ahrs_euler[1] );
|
||||
const real_t mn = ctheta * mag[0]
|
||||
- (dcm12 * mag[1] + dcm22 * mag[2]) * dcm02 / ctheta;
|
||||
|
||||
/* const real_t mn = ctheta * mag[0] */
|
||||
/* - (dcm12 * mag[1] + dcm22 * mag[2]) * dcm02 / ctheta; */
|
||||
|
||||
const real_t me =
|
||||
(dcm22 * mag[1] - dcm12 * mag[2]) / ctheta;
|
||||
/* const real_t me = */
|
||||
/* (dcm22 * mag[1] - dcm12 * mag[2]) / ctheta; */
|
||||
|
||||
/***/
|
||||
const real_t stheta = sin( ahrs_euler[AXIS_Y] );
|
||||
const real_t cphi = cos( ahrs_euler[AXIS_X] );
|
||||
const real_t sphi = sin( ahrs_euler[AXIS_X] );
|
||||
|
||||
const real_t mn =
|
||||
ctheta* mag[0]+
|
||||
sphi*stheta* mag[1]+
|
||||
cphi*stheta* mag[2];
|
||||
const real_t me =
|
||||
0* mag[0]+
|
||||
cphi* mag[1]+
|
||||
-sphi* mag[2];
|
||||
/***/
|
||||
|
||||
const real_t heading = -atan2( me, mn );
|
||||
|
||||
|
||||
+85
-50
@@ -37,24 +37,29 @@
|
||||
while ( n < lower) { n += (uper - lower);} \
|
||||
}
|
||||
|
||||
|
||||
float trim_roll = -450;
|
||||
float trim_yaw = -250;
|
||||
float trim_pitch = 0;
|
||||
|
||||
float max_roll_dot_sp = (1.5/MAX_PPRZ);
|
||||
float max_pitch_dot_sp = (1.5/MAX_PPRZ);
|
||||
float max_yaw_dot_sp = (1.5/MAX_PPRZ);
|
||||
|
||||
float max_roll_sp = (0.8/MAX_PPRZ);
|
||||
float max_pitch_sp = (0.8/MAX_PPRZ);
|
||||
float max_roll_sp = (0.4/MAX_PPRZ);
|
||||
float max_pitch_sp = (0.4/MAX_PPRZ);
|
||||
/* yaw is incremented acording to stick position */
|
||||
#define UPDATE_SP_DT 0.016384
|
||||
float max_yaw_sp = (0.8/MAX_PPRZ*UPDATE_SP_DT);
|
||||
|
||||
float max_v_sp = (3./MAX_PPRZ);
|
||||
float max_v_sp = (6./MAX_PPRZ);
|
||||
|
||||
static float throttle_setpoint = 0.;
|
||||
|
||||
float max_z_dot_sp = (10./MAX_PPRZ);
|
||||
float ctl_grz_z_dot_pgain = -1200;
|
||||
float max_z_dot_sp = (5./MAX_PPRZ);
|
||||
float ctl_grz_z_dot_pgain = -2000;
|
||||
float ctl_grz_z_dot_igain = 3.;
|
||||
float ctl_grz_z_dot_dgain = 5.;
|
||||
float ctl_grz_z_dot_dgain = 8.;
|
||||
float ctl_grz_z_dot = 0;
|
||||
float ctl_grz_z_dot_setpoint = 0.;
|
||||
float ctl_grz_z_dot_sum_err = 0.;
|
||||
@@ -88,25 +93,24 @@ struct pid ve_pid;
|
||||
|
||||
void ctl_grz_init( void ) {
|
||||
|
||||
roll_dot_pid.p_gain = 4000.;
|
||||
roll_dot_pid.i_gain = 0.;
|
||||
roll_dot_pid.d_gain = 2.5;
|
||||
roll_dot_pid.p_gain = 4700.;
|
||||
roll_dot_pid.i_gain = 1.;
|
||||
roll_dot_pid.d_gain = 1.875;
|
||||
|
||||
pitch_dot_pid.p_gain = -4000.;
|
||||
pitch_dot_pid.i_gain = 0.;
|
||||
pitch_dot_pid.d_gain = 2.5;
|
||||
pitch_dot_pid.p_gain = -4700.;
|
||||
pitch_dot_pid.i_gain = 1.;
|
||||
pitch_dot_pid.d_gain = 1.875;
|
||||
|
||||
yaw_dot_pid.p_gain = -4500.;
|
||||
yaw_dot_pid.i_gain = 0.;
|
||||
yaw_dot_pid.d_gain = 0.;
|
||||
yaw_dot_pid.p_gain = -4700.;
|
||||
yaw_dot_pid.i_gain = 1.;
|
||||
yaw_dot_pid.d_gain = 2.;
|
||||
|
||||
|
||||
roll_pid.p_gain = -2.5;
|
||||
roll_pid.p_gain = -5;
|
||||
roll_pid.i_gain = 0.;
|
||||
roll_pid.d_gain = 1.;
|
||||
roll_pid.saturation = 3.;
|
||||
|
||||
pitch_pid.p_gain = -2.5;
|
||||
pitch_pid.p_gain = -5;
|
||||
pitch_pid.i_gain = 0.;
|
||||
pitch_pid.d_gain = 1.;
|
||||
pitch_pid.saturation = 3.;
|
||||
@@ -115,15 +119,16 @@ void ctl_grz_init( void ) {
|
||||
yaw_pid.i_gain = 0.;
|
||||
yaw_pid.d_gain = 0.;
|
||||
yaw_pid.saturation = 2.;
|
||||
yaw_pid.set_point = 0.; // RadOfDeg(-20);
|
||||
|
||||
vn_pid.p_gain = -0.1;
|
||||
vn_pid.i_gain = 0.;
|
||||
vn_pid.d_gain = 0.;
|
||||
vn_pid.saturation = .2;
|
||||
|
||||
ve_pid.p_gain = -0.1;
|
||||
ve_pid.i_gain = 0.;
|
||||
ve_pid.d_gain = 0.;
|
||||
ve_pid.p_gain = -0.2;
|
||||
ve_pid.i_gain = 1.;
|
||||
ve_pid.d_gain = 1.;
|
||||
ve_pid.saturation = .2;
|
||||
}
|
||||
|
||||
@@ -149,25 +154,25 @@ void ctl_grz_set_setpoints_auto1( void ) {
|
||||
}
|
||||
#endif
|
||||
// ctl_grz_z_dot_setpoint = (rc_values[RADIO_THROTTLE] - MAX_PPRZ/2) * max_z_dot_sp;
|
||||
// throttle_setpoint = rc_values[RADIO_THROTTLE];
|
||||
ctl_grz_z_setpoint = rc_values[RADIO_THROTTLE] * max_z_sp - 0.4;
|
||||
throttle_setpoint = rc_values[RADIO_THROTTLE];
|
||||
// ctl_grz_z_setpoint = rc_values[RADIO_THROTTLE] * max_z_sp - 0.5;
|
||||
}
|
||||
|
||||
void ctl_grz_set_setpoints_auto2( void ) {
|
||||
/* roll_pid.set_point = - rc_values[RADIO_ROLL] * max_roll_sp; */
|
||||
/* pitch_pid.set_point = rc_values[RADIO_PITCH] * max_pitch_sp; */
|
||||
roll_pid.set_point = - rc_values[RADIO_ROLL] * max_roll_sp;
|
||||
pitch_pid.set_point = rc_values[RADIO_PITCH] * max_pitch_sp;
|
||||
|
||||
float vright = rc_values[RADIO_ROLL] * max_v_sp;
|
||||
float vfront = rc_values[RADIO_PITCH] * max_v_sp;
|
||||
/* float vright = - rc_values[RADIO_ROLL] * max_v_sp; */
|
||||
/* float vfront = - rc_values[RADIO_PITCH] * max_v_sp; */
|
||||
|
||||
float alpha = M_PI/2. - yaw_pid.measure;
|
||||
float cos_alpha = cos(alpha);
|
||||
float sin_alpha = sin(alpha);
|
||||
ve_pid.set_point = cos_alpha*vright + sin_alpha*vfront;
|
||||
vn_pid.set_point = sin_alpha*vright - cos_alpha*vfront;
|
||||
|
||||
yaw_dot_pid.set_point = - rc_values[RADIO_YAW] * max_yaw_dot_sp;
|
||||
ctl_grz_z_setpoint = rc_values[RADIO_THROTTLE] * max_z_sp - 0.4;
|
||||
/* float alpha = M_PI/2. - yaw_pid.measure; */
|
||||
/* float cos_alpha = cos(alpha); */
|
||||
/* float sin_alpha = sin(alpha); */
|
||||
/* ve_pid.set_point = sin_alpha*vright + cos_alpha*vfront; */
|
||||
/* vn_pid.set_point = - cos_alpha*vright + sin_alpha*vfront; */
|
||||
|
||||
ctl_grz_z_dot_setpoint = (rc_values[RADIO_THROTTLE] - MAX_PPRZ/2) * max_z_dot_sp;
|
||||
// ctl_grz_z_setpoint = rc_values[RADIO_THROTTLE] * max_z_sp - 0.5;
|
||||
// throttle_setpoint = rc_values[RADIO_THROTTLE];
|
||||
}
|
||||
|
||||
@@ -189,17 +194,34 @@ void ctl_grz_rate_run ( void ) {
|
||||
float err = roll_dot_pid.measure - roll_dot_pid.set_point;
|
||||
float d_err = err - roll_dot_pid.last_err;
|
||||
roll_dot_pid.last_err = err;
|
||||
commands[COMMAND_ROLL] = TRIM_PPRZ((int16_t)((err + d_err * roll_dot_pid.d_gain) * roll_dot_pid.p_gain));
|
||||
if (flying) {
|
||||
roll_dot_pid.sum_err += err/100;
|
||||
Bound(roll_dot_pid.sum_err, -0.2, 0.2);
|
||||
} else
|
||||
roll_dot_pid.sum_err = 0.;
|
||||
|
||||
commands[COMMAND_ROLL] = TRIM_PPRZ((int16_t)((err + d_err * roll_dot_pid.d_gain + roll_dot_pid.sum_err * roll_dot_pid.i_gain) * roll_dot_pid.p_gain));
|
||||
|
||||
err = pitch_dot_pid.measure - pitch_dot_pid.set_point;
|
||||
d_err = err - pitch_dot_pid.last_err;
|
||||
pitch_dot_pid.last_err = err;
|
||||
commands[COMMAND_PITCH] = TRIM_PPRZ((int16_t)((err + d_err * pitch_dot_pid.d_gain) * pitch_dot_pid.p_gain));
|
||||
if (flying) {
|
||||
pitch_dot_pid.sum_err += err/100;
|
||||
Bound(pitch_dot_pid.sum_err, -0.2, 0.2);
|
||||
} else
|
||||
pitch_dot_pid.sum_err = 0;
|
||||
|
||||
commands[COMMAND_PITCH] = TRIM_PPRZ((int16_t)((err + d_err * pitch_dot_pid.d_gain + pitch_dot_pid.sum_err * pitch_dot_pid.i_gain) * pitch_dot_pid.p_gain));
|
||||
|
||||
err = yaw_dot_pid.measure - yaw_dot_pid.set_point;
|
||||
d_err = err - yaw_dot_pid.last_err;
|
||||
yaw_dot_pid.last_err = err;
|
||||
commands[COMMAND_YAW] = TRIM_PPRZ((int16_t)((err + d_err * yaw_dot_pid.d_gain) * yaw_dot_pid.p_gain));
|
||||
if (flying) {
|
||||
yaw_dot_pid.sum_err += err/100;
|
||||
Bound(yaw_dot_pid.sum_err, -0.2, 0.2);
|
||||
} else
|
||||
yaw_dot_pid.sum_err = 0;
|
||||
commands[COMMAND_YAW] = TRIM_PPRZ((int16_t)((err + d_err * yaw_dot_pid.d_gain + yaw_dot_pid.sum_err * yaw_dot_pid.i_gain) * yaw_dot_pid.p_gain));
|
||||
|
||||
commands[COMMAND_THROTTLE] = throttle_setpoint;
|
||||
}
|
||||
@@ -207,7 +229,6 @@ void ctl_grz_rate_run ( void ) {
|
||||
|
||||
|
||||
void ctl_grz_attitude_run ( void ) {
|
||||
|
||||
float err = roll_pid.measure - roll_pid.set_point;
|
||||
float d_err = err - roll_pid.last_err;
|
||||
roll_pid.last_err = err;
|
||||
@@ -243,7 +264,7 @@ void ctl_grz_alt_run( void ) {
|
||||
ctl_grz_z_dot_setpoint = Chop(((err + d_err * ctl_grz_z_dgain + ctl_grz_z_sum_err * ctl_grz_z_igain) * ctl_grz_z_pgain), MIN_Z_DOT, MAX_Z_DOT);
|
||||
}
|
||||
|
||||
void ctl_grz_speed_run( void ) {
|
||||
void ctl_grz_z_dot_run( void ) {
|
||||
float err = ctl_grz_z_dot - ctl_grz_z_dot_setpoint;
|
||||
static float z_dot_last_err;
|
||||
float d_err = err - z_dot_last_err;
|
||||
@@ -252,7 +273,8 @@ void ctl_grz_speed_run( void ) {
|
||||
if (flying) {
|
||||
ctl_grz_z_dot_sum_err += err/600;
|
||||
Bound(ctl_grz_z_dot_sum_err, -1., 1.);
|
||||
}
|
||||
} else
|
||||
ctl_grz_z_dot_sum_err = 0;
|
||||
|
||||
throttle_setpoint = TRIM_PPRZ(ctl_grz_throttle_level + (int16_t)((err + d_err * ctl_grz_z_dot_dgain + ctl_grz_z_dot_sum_err * ctl_grz_z_dot_igain) * ctl_grz_z_dot_pgain));
|
||||
}
|
||||
@@ -266,20 +288,33 @@ void ctl_grz_horiz_speed_run ( void ) {
|
||||
float err = ve_pid.measure - ve_pid.set_point;
|
||||
float d_err = err - ve_pid.last_err;
|
||||
ve_pid.last_err = err;
|
||||
east_angle_set_point = (err + d_err * ve_pid.d_gain) * ve_pid.p_gain;
|
||||
Bound(east_angle_set_point, ve_pid.saturation, -ve_pid.saturation);
|
||||
ve_pid.sum_err += err/100;
|
||||
east_angle_set_point = (err + d_err * ve_pid.d_gain + ve_pid.sum_err * ve_pid.i_gain) * ve_pid.p_gain;
|
||||
Bound(east_angle_set_point, -ve_pid.saturation, ve_pid.saturation);
|
||||
Bound(ve_pid.sum_err, -3, 3);
|
||||
|
||||
err = ve_pid.measure - vn_pid.set_point;
|
||||
err = vn_pid.measure - vn_pid.set_point;
|
||||
d_err = err - vn_pid.last_err;
|
||||
vn_pid.last_err = err;
|
||||
north_angle_set_point = (err + d_err * ve_pid.d_gain) * ve_pid.p_gain;
|
||||
Bound(north_angle_set_point, vn_pid.saturation, -vn_pid.saturation);
|
||||
vn_pid.sum_err += err/100;
|
||||
north_angle_set_point = - ((err + d_err * ve_pid.d_gain + vn_pid.sum_err * ve_pid.i_gain) * ve_pid.p_gain);
|
||||
Bound(north_angle_set_point, -vn_pid.saturation, vn_pid.saturation);
|
||||
Bound(vn_pid.sum_err, -3, 3);
|
||||
|
||||
float alpha = - (M_PI/2. - yaw_pid.measure);
|
||||
float alpha = (M_PI/2. - yaw_pid.measure);
|
||||
float cos_alpha = cos(alpha);
|
||||
float sin_alpha = sin(alpha);
|
||||
|
||||
roll_pid.set_point = cos_alpha*east_angle_set_point + sin_alpha*north_angle_set_point;
|
||||
pitch_pid.set_point = sin_alpha*east_angle_set_point - cos_alpha*north_angle_set_point;
|
||||
|
||||
roll_pid.set_point = sin_alpha*east_angle_set_point + cos_alpha*north_angle_set_point;
|
||||
pitch_pid.set_point = - cos_alpha*east_angle_set_point + sin_alpha*north_angle_set_point;
|
||||
}
|
||||
|
||||
void ctl_grz_reset( void ) {
|
||||
ctl_grz_z_dot_sum_err = 0;
|
||||
ve_pid.sum_err = 0;
|
||||
vn_pid.sum_err = 0;
|
||||
roll_dot_pid.sum_err = 0;
|
||||
pitch_dot_pid.sum_err = 0;
|
||||
yaw_dot_pid.sum_err = 0;
|
||||
}
|
||||
|
||||
|
||||
@@ -74,7 +74,7 @@ extern void ctl_grz_set_measures( void );
|
||||
extern void ctl_grz_rate_run ( void );
|
||||
extern void ctl_grz_attitude_run ( void );
|
||||
|
||||
extern void ctl_grz_speed_run ( void );
|
||||
extern void ctl_grz_z_dot_run ( void );
|
||||
extern void ctl_grz_alt_run ( void );
|
||||
extern void ctl_grz_horiz_speed_run ( void );
|
||||
|
||||
@@ -85,5 +85,9 @@ extern struct pid ve_pid;
|
||||
|
||||
extern bool_t flying;
|
||||
|
||||
void ctl_grz_reset( void );
|
||||
|
||||
extern float trim_roll, trim_yaw, trim_pitch;
|
||||
|
||||
|
||||
#endif // CONTROL_GRZ_H
|
||||
|
||||
@@ -52,7 +52,7 @@ extern struct adc_buf buf_bat;
|
||||
|
||||
#define IMU_GYRO_X_GAIN -0.000220177991888642784
|
||||
#define IMU_GYRO_Y_GAIN -0.000214915108532129801
|
||||
#define IMU_GYRO_Z_GAIN 0.0002
|
||||
#define IMU_GYRO_Z_GAIN 0.0002104
|
||||
|
||||
#define ImuUpdateGyros() { \
|
||||
\
|
||||
|
||||
+17
-8
@@ -37,6 +37,10 @@ uint16_t rangemeter; /* cm */
|
||||
#define SWITCH_PEDIOD_MIN 30 /* 60Hz */
|
||||
|
||||
|
||||
|
||||
bool_t sum_err_reset;
|
||||
|
||||
|
||||
/** 60Hz */
|
||||
void periodic_task_ap( void ) {
|
||||
|
||||
@@ -46,8 +50,13 @@ void periodic_task_ap( void ) {
|
||||
_10Hz++;
|
||||
if (_10Hz>=6) _10Hz = 0;
|
||||
|
||||
if (!_10Hz)
|
||||
if (!_10Hz) {
|
||||
PeriodicSendAp();
|
||||
if (sum_err_reset) {
|
||||
sum_err_reset = FALSE;
|
||||
ctl_grz_reset();
|
||||
}
|
||||
}
|
||||
|
||||
static uint16_t last_rangemeters[NB_RANGES];
|
||||
static uint8_t last_idx;
|
||||
@@ -55,13 +64,13 @@ void periodic_task_ap( void ) {
|
||||
// ClearBit(ADCSRA, ADIE);
|
||||
rangemeter = RangeCmOfAdc(rangemeter_adc_buf.sum/rangemeter_adc_buf.av_nb_sample);
|
||||
// SetBit(ADCSRA, ADIE);
|
||||
ctl_grz_z = rangemeter / 100.; /* cm -> m */
|
||||
last_idx++;
|
||||
if (last_idx >= NB_RANGES) last_idx = 0;
|
||||
float last_avg = (float)sum_rangemeters / NB_RANGES;
|
||||
sum_rangemeters += rangemeter - last_rangemeters[last_idx];
|
||||
last_rangemeters[last_idx] = rangemeter;
|
||||
float avg = (float)sum_rangemeters / NB_RANGES;
|
||||
ctl_grz_z = avg / 100.; /* cm -> m */
|
||||
ctl_grz_z_dot = ((avg - last_avg) * 61. / 100.);
|
||||
|
||||
|
||||
@@ -90,19 +99,19 @@ void periodic_task_ap( void ) {
|
||||
|
||||
if (pprz_mode == PPRZ_MODE_AUTO2) {
|
||||
ctl_grz_set_setpoints_auto2();
|
||||
}
|
||||
else if (pprz_mode == PPRZ_MODE_AUTO1) {
|
||||
ctl_grz_alt_run();
|
||||
} else if (pprz_mode == PPRZ_MODE_AUTO1) {
|
||||
ctl_grz_set_setpoints_auto1();
|
||||
}
|
||||
|
||||
if (pprz_mode >= PPRZ_MODE_AUTO2) {
|
||||
if (!_10Hz)
|
||||
ctl_grz_horiz_speed_run();
|
||||
ctl_grz_z_dot_run();
|
||||
/* if (!_10Hz) */
|
||||
/* ctl_grz_horiz_speed_run(); */
|
||||
}
|
||||
|
||||
if (pprz_mode >= PPRZ_MODE_AUTO1) {
|
||||
ctl_grz_speed_run();
|
||||
// ctl_grz_alt_run();
|
||||
// ctl_grz_z_dot_run();
|
||||
ctl_grz_attitude_run();
|
||||
}
|
||||
|
||||
|
||||
@@ -79,7 +79,7 @@ static inline void main_event_task( void ) {
|
||||
micromag_data_available = FALSE;
|
||||
spi_cur_slave = SPI_SLAVE_NONE;
|
||||
ImuUpdateMag();
|
||||
// DOWNLINK_SEND_AHRS_MAG(&imu_mag[AXIS_X], &imu_mag[AXIS_Y], &imu_mag[AXIS_Z]);
|
||||
DOWNLINK_SEND_AHRS_MAG(&imu_mag[AXIS_X], &imu_mag[AXIS_Y], &imu_mag[AXIS_Z]);
|
||||
spi_cur_slave = SPI_SLAVE_MAX;
|
||||
max1167_read();
|
||||
}
|
||||
@@ -146,7 +146,8 @@ static inline void ahrs_task( void ) {
|
||||
ahrs_state_update();
|
||||
ahrs_compass_update(ahrs_heading_of_mag(imu_mag));
|
||||
DOWNLINK_SEND_AHRS(&q0, &q1, &q2, &q3, &bias_p, &bias_q, &bias_r);
|
||||
//DOWNLINK_SEND_AHRS2(&ahrs_euler[AXIS_X], &ahrs_euler[AXIS_Y], &ahrs_euler[AXIS_Z]);
|
||||
// DOWNLINK_SEND_AHRS2(&ahrs_euler[AXIS_X], &ahrs_euler[AXIS_Y], &ahrs_euler[AXIS_Z]);
|
||||
// DOWNLINK_SEND_AHRS2(&imu_gyro[AXIS_X], &imu_gyro[AXIS_Y], &imu_gyro[AXIS_Z]);
|
||||
break;
|
||||
}
|
||||
ahrs_step++;
|
||||
|
||||
Reference in New Issue
Block a user