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

*** empty log message ***

Browse Source
This commit is contained in:
Antoine Drouin
2008-01-29 17:52:37 +00:00
parent d1f60fddb9
commit 36b3f23a1d
16 changed files with 327 additions and 87 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/booz.xml
+24
View File
@@ -33,6 +33,30 @@
</rc_commands>
<section name="MICROMAG">
<define name="MM_SS_PIN" value="20"/>
<define name="MM_SS_IODIR" value="IO0DIR"/>
<define name="MM_SS_IOSET" value="IO0SET"/>
<define name="MM_SS_IOCLR" value="IO0CLR"/>
<define name="MM_RESET_PIN" value="21"/>
<define name="MM_RESET_IODIR" value="IO1DIR"/>
<define name="MM_RESET_IOSET" value="IO1SET"/>
<define name="MM_RESET_IOCLR" value="IO1CLR"/>
<define name="MM_DRDY_PIN" value="15"/>
<define name="MM_DRDY_PINSEL" value="PINSEL0"/>
<define name="MM_DRDY_PINSEL_BIT" value="30"/>
<define name="MM_DRDY_PINSEL_VAL" value="2"/>
<define name="MM_DRDY_EINT" value="2"/>
</section>
<makefile>
include $(PAPARAZZI_SRC)/conf/autopilot/conf_booz.makefile
conf/airframes/tl.xml
+35 -5
View File
@@ -18,10 +18,10 @@
</commands>
<command_laws>
<let var="roll" value="1.0 * @ROLL"/>
<let var="pitch" value="1.0 * @PITCH"/>
<let var="yaw" value="0.1 * @YAW"/>
<let var="throttle" value="1.0 * @THROTTLE"/>
<let var="roll" value="1.0 * @ROLL"/>
<let var="pitch" value="1.0 * @PITCH"/>
<let var="yaw" value="1.0 * @YAW"/>
<let var="throttle" value="1.0 * @THROTTLE"/>
<let var="right" value="$throttle + $yaw"/><!-- add trim term for Right -->
<let var="left" value="$throttle - $yaw"/><!-- .06*thrsubtract trim term for Right -->
<set servo="AILERON" value="$roll"/>
@@ -39,6 +39,27 @@
<set command="CAM" value="@GAIN1"/>
</rc_commands>
<section name="MICROMAG">
<define name="MM_SS_PIN" value="20"/>
<define name="MM_SS_IODIR" value="IO0DIR"/>
<define name="MM_SS_IOSET" value="IO0SET"/>
<define name="MM_SS_IOCLR" value="IO0CLR"/>
<define name="MM_RESET_PIN" value="29"/>
<define name="MM_RESET_IODIR" value="IO0DIR"/>
<define name="MM_RESET_IOSET" value="IO0SET"/>
<define name="MM_RESET_IOCLR" value="IO0CLR"/>
<define name="MM_DRDY_PINSEL" value="PINSEL1"/>
<define name="MM_DRDY_PINSEL_BIT" value="28"/>
<define name="MM_DRDY_PINSEL_VAL" value="2"/>
<define name="MM_DRDY_EINT" value="3"/>
</section>
<section name="horizontal_control">
<define name="NAV_GOTO_H_PGAIN" value="RadOfDeg(10)" unit="rad/m"/>
<define name="NAV_GOTO_H_DGAIN" value="0"/>
@@ -80,10 +101,19 @@ ap.srcs += tl_autopilot.c tl_control.c tl_estimator.c
ap.CFLAGS += -DGPS -DUBX -DUSE_UART1 -DGPS_LINK=Uart1 -DUART1_BAUD=B38400 -DDOWNLINK_GPS_DEVICE=DOWNLINK_AP_DEVICE -DGPS_CONFIGURE -DUSER_GPS_CONFIGURE=\"tl_gps_configure.h\" -DGPS_BAUD=38400
ap.srcs += gps_ubx.c gps.c latlong.c
ap.srcs += tl_imu.c
ap.CFLAGS += -DADC -DUSE_ADC_5 -DADC_CHANNEL_GR=ADC_5
ap.srcs += $(SRC_ARCH)/tl_baro.c
ap.CFLAGS += -DUSE_MICROMAG
ap.srcs += micromag.c $(SRC_ARCH)/micromag_hw.c
ap.CFLAGS += -DNAV
ap.srcs += common_nav.c tl_nav.c
ap.CFLAGS += -D BARO_MS5534A_W1=0xABDC -D BARO_MS5534A_W2=0x849A -D BARO_MS5534A_W3=0X939E -D BARO_MS5534A_W4=0xB259
ap.CFLAGS += -D TL_BARO_W1=0xABDC -D TL_BARO_W2=0x849A -D TL_BARO_W3=0X939E -D TL_BARO_W4=0xB259
</makefile>
conf/messages.xml
+6
View File
@@ -541,6 +541,12 @@
<field name="ami_stat" type="uint8"/>
</message>
<message name="TL_ESTIMATOR" ID="246">
<field name="r" type="float" unit="rad/s"/>
<field name="psi" type="float" unit="rad"/>
<field name="z_baro" type="float" unit="cm"/>
</message>
<message name="ENOSE_STATUS" ID="250">
<field name="val1" type="uint16"/>
<field name="val2" type="uint16"/>
sw/airborne/arm7/baro_MS5534A.h
-8
View File
@@ -34,14 +34,6 @@
#ifdef USE_BARO_MS5534A
#if ! defined USE_SPI
#define USE_SPI
#endif
#if ! defined SPI_MASTER
#define SPI_MASTER
#endif
extern bool_t baro_MS5534A_do_reset;
extern bool_t baro_MS5534A_available;
extern uint32_t baro_MS5534A_pressure;
sw/airborne/arm7/micromag_hw.c
+23 -16
View File
@@ -1,19 +1,26 @@
/* PNI micromag3 connected on SPI1
/* PNI micromag3 connected on SPI1 */
/*
Twisted Logic
SS on P0.20
RESET on P0.29
DRDY on P0.30 ( EINT3 )
*/
/*
IMU
SS on P0.20
RESET on P1.21
DRDY on P0.15 ( EINT2 )
*/
#include "micromag.h"
volatile uint8_t micromag_cur_axe;
#ifndef DISABLE_MAGNETOMETER
static void EXTINT2_ISR(void) __attribute__((naked));
#endif /* DISABLE_MAGNETOMETER */
void micromag_hw_init( void ) {
#ifndef DISABLE_MAGNETOMETER
/* configure SS pin */
SetBit(MM_SS_IODIR, MM_SS_PIN); /* pin is output */
MmUnselect(); /* pin idles high */
@@ -30,23 +37,23 @@ void micromag_hw_init( void ) {
SetBit(EXTINT,MM_DRDY_EINT); /* clear pending EINT */
/* initialize interrupt vector */
VICIntSelect &= ~VIC_BIT( VIC_EINT2 ); /* select EINT2 as IRQ source */
VICIntEnable = VIC_BIT( VIC_EINT2 ); /* enable it */
VICVectCntl9 = VIC_ENABLE | VIC_EINT2;
VICIntSelect &= ~VIC_BIT( VIC_EINT3 ); /* select EINT2 as IRQ source */
VICIntEnable = VIC_BIT( VIC_EINT3 ); /* enable it */
VICVectCntl9 = VIC_ENABLE | VIC_EINT3;
VICVectAddr9 = (uint32_t)EXTINT2_ISR; // address of the ISR
#endif /* DISABLE_MAGNETOMETER */
}
void micromag_read( void ) {
#ifndef DISABLE_MAGNETOMETER
MmSelect();
SpiEnable();
// micromag_cur_axe = 0;
MmTriggerRead();
#endif /* DISABLE_MAGNETOMETER */
if (micromag_status == MM_IDLE ) {
MmSelect();
SpiEnable();
SpiDisableRti();
micromag_cur_axe = 0;
micromag_status = MM_BUSY;
MmTriggerRead();
}
}
#ifndef DISABLE_MAGNETOMETER
void EXTINT2_ISR(void) {
ISR_ENTRY();
/* read dummy control byte reply */
@@ -62,4 +69,4 @@ void EXTINT2_ISR(void) {
VICVectAddr = 0x00000000; /* clear this interrupt from the VIC */
ISR_EXIT();
}
#endif /* DISABLE_MAGNETOMETER */
sw/airborne/arm7/micromag_hw.h
+21 -19
View File
@@ -6,24 +6,10 @@
#include "interrupt_hw.h"
#include "spi_hw.h"
#include "airframe.h"
extern volatile uint8_t micromag_cur_axe;
#define MM_SS_PIN 20
#define MM_SS_IODIR IO0DIR
#define MM_SS_IOSET IO0SET
#define MM_SS_IOCLR IO0CLR
#define MM_RESET_PIN 21
#define MM_RESET_IODIR IO1DIR
#define MM_RESET_IOSET IO1SET
#define MM_RESET_IOCLR IO1CLR
#define MM_DRDY_PIN 15
#define MM_DRDY_PINSEL PINSEL0
#define MM_DRDY_PINSEL_BIT 30
#define MM_DRDY_PINSEL_VAL 2
#define MM_DRDY_EINT 2
#define MmSelect() SetBit(MM_SS_IOCLR,MM_SS_PIN)
#define MmUnselect() SetBit(MM_SS_IOSET,MM_SS_PIN)
@@ -33,15 +19,13 @@ extern volatile uint8_t micromag_cur_axe;
#define MmTriggerRead() { \
MmSet(); \
micromag_cur_axe++; \
if (micromag_cur_axe == MM_NB_AXIS) \
micromag_cur_axe = 0; \
MmReset(); \
uint8_t control_byte = (micromag_cur_axe+1) << 0 | 4 << 4; \
SpiSend(control_byte); \
}
#define MmOnSpiIt() { \
#if 0
#define MmOnSpiIt() { \
if (bit_is_set(SSPMIS, RTMIS)) { \
micromag_values[micromag_cur_axe] = SSPDR << 8; \
micromag_values[micromag_cur_axe] += SSPDR; \
@@ -56,6 +40,24 @@ extern volatile uint8_t micromag_cur_axe;
MmUnselect(); \
} \
}
#endif
#define MicromagOnSpiInt() { \
if (bit_is_set(SSPMIS, RTMIS)) { \
micromag_values[micromag_cur_axe] = SSPDR << 8; \
micromag_values[micromag_cur_axe] += SSPDR; \
SpiClearRti(); \
SpiDisableRti(); \
if (micromag_cur_axe == 2) { \
micromag_status = MM_DATA_AVAILABLE; \
SpiDisable(); \
MmUnselect(); \
} \
else { \
micromag_cur_axe++; \
MmTriggerRead(); \
} \
} \
}
#endif /* MICROMAG_HW_H */
sw/airborne/arm7/tl_baro.c
+93
View File
@@ -0,0 +1,93 @@
#include "tl_baro.h"
#include "airframe.h"
#define CMD_MEASUREMENT 0x0F
#define CMD_PRESSURE 0x40
#define CMD_TEMPERATURE 0x20
uint16_t tl_baro_d[TL_BARO_NB_DATA];
const uint8_t tl_baro_cmd[TL_BARO_NB_DATA] = {CMD_PRESSURE, CMD_TEMPERATURE};
uint32_t tl_baro_pressure;
uint16_t tl_baro_temp;
uint8_t tl_baro_cur_data;
static uint16_t c1, c2, c3, c4, ut1, c6;
#define MS5534A_MCLK 32768
#define PWM_PRESCALER 1
#define PWM_PERIOD ((PCLK / PWM_PRESCALER) / MS5534A_MCLK)
#define PWM_DUTY (PWM_PERIOD / 2)
void tl_baro_init(void) {
/* 32768Hz on PWM2 */
/* Configure P0.7 pin as PWM */
PINSEL0 &= ~(_BV(14));
PINSEL0 |= _BV(15);
/* No prescaler */
PWMPR = PWM_PRESCALER-1;
/* To get 32768Hz from a base frequency of 15MHz */
PWMMR0 = PWM_PERIOD;
PWMMR2 = PWM_DUTY;
PWMLER = PWMLER_LATCH0 | PWMLER_LATCH2;
PWMTCR = PWMTCR_COUNTER_ENABLE | PWMTCR_PWM_ENABLE;
PWMPCR = PWMPCR_ENA2;
/* setup slave select */
/* configure SS pin */
SetBit(TL_BARO_SS_IODIR, TL_BARO_SS_PIN); /* pin is output */
TlBaroUnselect(); /* pin idles high */
tl_baro_cur_data = TL_BARO_PRESSURE;
/* End of init, configuration (page 11) */
c1 = TL_BARO_W1 >> 1;
c2 = ((TL_BARO_W3 & 0x3f) << 6) | (TL_BARO_W4 & 0x3f);
c3 = TL_BARO_W4 >> 6;
c4 = TL_BARO_W3 >> 6;
uint16_t c5 = ((TL_BARO_W1 & 0x1) << 10) | (TL_BARO_W2 >> 6);
c6 = TL_BARO_W2 & 0x3f;
ut1 = (c5 << 3) + 20224;
}
void tl_baro_send_req(void) {
tl_baro_cur_data++;
if (tl_baro_cur_data == TL_BARO_NB_DATA) tl_baro_cur_data = TL_BARO_PRESSURE;
TlBaroSelect();
// SpiEnable();
// SpiEnableRti();
SpiClrCPHA();
SSPDR = CMD_MEASUREMENT;
SSPDR = tl_baro_cmd[tl_baro_cur_data];
}
void tl_baro_read(void) {
TlBaroSelect();
SpiEnable();
SpiEnableRti();
SpiSetCPHA();
SSPDR = 0;
SSPDR = 0;
}
void tl_baro_compute(void) {
/* Compute pressure and temp (page 10) */
int16_t dT = tl_baro_d[1] - ut1;
tl_baro_temp = 200 + (dT*(c6+50)) / (1 << 10);
int16_t off = c2*4 + ((c4-512)*dT)/(1 << 12);
uint32_t sens = c1 + (c3*dT)/(1<<10) + 24576;
uint16_t x = (sens*(tl_baro_d[0]-7168))/(1<<14) - off;
tl_baro_pressure = ((x*100)>>5) + 250*100;
}
sw/airborne/arm7/tl_baro.h
+38
View File
@@ -0,0 +1,38 @@
#ifndef TL_BARO_H
#define TL_BARO_H
#include "std.h"
#include "LPC21xx.h"
#include "spi_hw.h"
#include "sys_time.h"
#define TL_BARO_PRESSURE 0
#define TL_BARO_TEMP 1
#define TL_BARO_NB_DATA 2
extern uint8_t tl_baro_cur_data;
extern uint16_t tl_baro_d[];
extern uint32_t tl_baro_pressure;
extern uint16_t tl_baro_temp;
/* P0.28 */
#define TL_BARO_SS_IODIR IO0DIR
#define TL_BARO_SS_PIN 28
#define TL_BARO_SS_IOCLR IO0CLR
#define TL_BARO_SS_IOSET IO0SET
#define TlBaroUnselect() SetBit(TL_BARO_SS_IOCLR,TL_BARO_SS_PIN)
#define TlBaroSelect() SetBit(TL_BARO_SS_IOSET,TL_BARO_SS_PIN)
#define TlBaroOnSpiIntReading() { tl_baro_d[tl_baro_cur_data] = SSPDR<<8; tl_baro_d[tl_baro_cur_data] |= SSPDR; TlBaroUnselect(); }
#define TlBaroOnSpiIntSending() { uint8_t foo = SSPDR; foo = SSPDR; TlBaroUnselect(); SpiDisable(); SpiDisableRti(); }
extern void tl_baro_init(void);
extern void tl_baro_send_req(void);
extern void tl_baro_read(void);
extern void tl_baro_compute(void);
#endif /* TL_BARO_H */
sw/airborne/estimator.c
-18
View File
@@ -99,24 +99,6 @@ void estimator_init( void ) {
}
#ifdef IMU_3DMG
#include "inter_mcu.h"
void estimator_update_state_3DMG( void ) {
estimator_phi = from_fbw.from_fbw.euler[0];
estimator_psi = from_fbw.from_fbw.euler[1];
estimator_theta = from_fbw.from_fbw.euler[2];
}
#elif defined IMU_ANALOG && defined AHRS
#include "ahrs.h"
void estimator_update_state_ANALOG( void ) {
//ahrs.h is in NED but estimator in NWU if i remember
//perhaps this transform is not enough, i'm tired ;-)
estimator_phi = ahrs_euler[0];
estimator_theta = -ahrs_euler[1];
estimator_psi = -ahrs_euler[2];
}
#else //NO_IMU
#endif
void estimator_propagate_state( void ) {
sw/airborne/micromag.c
+2 -2
View File
@@ -1,6 +1,6 @@
#include "micromag.h"
volatile uint8_t micromag_data_available;
volatile uint8_t micromag_status;
volatile int16_t micromag_values[MM_NB_AXIS];
void micromag_init( void ) {
@@ -10,5 +10,5 @@ void micromag_init( void ) {
uint8_t i;
for (i=0; i<MM_NB_AXIS; i++)
micromag_values[i] = 0;
micromag_data_available = FALSE;
micromag_status = MM_IDLE;
}
sw/airborne/micromag.h
+14 -1
View File
@@ -8,7 +8,20 @@
extern void micromag_init( void );
extern void micromag_read( void );
extern volatile uint8_t micromag_data_available;
#define MM_IDLE 0
#define MM_BUSY 1
#define MM_SENDING_REQ_X 1
#define MM_SENDING_WAIT_X 2
#define MM_READING_X 3
#define MM_SENDING_REQ_Y 4
#define MM_SENDING_WAIT_Y 5
#define MM_READING_Y 6
#define MM_SENDING_REQ_Z 7
#define MM_SENDING_WAIT_Z 8
#define MM_READING_Z 9
#define MM_DATA_AVAILABLE 10
extern volatile uint8_t micromag_status;
extern volatile int16_t micromag_values[MM_NB_AXIS];
extern void micromag_hw_init( void );
sw/airborne/tl_control.c
+18 -10
View File
@@ -28,6 +28,7 @@ pprz_t tl_control_commands[COMMANDS_NB];
float tl_control_attitude_phi_sp;
float tl_control_attitude_theta_sp;
float tl_control_attitude_psi_sp;
/* float tl_control_attitude_phi_theta_pgain; */
/* float tl_control_attitude_phi_theta_dgain; */
/* float tl_control_attitude_psi_pgain; */
@@ -41,7 +42,7 @@ float tl_control_attitude_psi_sp;
/* setpoints for max stick throw in degres */
/* #define TL_CONTROL_ATTITUDE_PHI_THETA_MAX_SP 30. */
/* #define TL_CONTROL_ATTITUDE_PSI_MAX_SP 45. */
#define TL_CONTROL_ATTITUDE_PSI_MAX_SP 45.
void tl_control_init(void) {
@@ -72,8 +73,8 @@ void tl_control_rate_read_setpoints_from_rc(void) {
tl_control_p_sp = -rc_values[RADIO_ROLL];
tl_control_q_sp = rc_values[RADIO_PITCH];
// tl_control_r_sp = -rc_values[RADIO_YAW] * RadOfDeg(TL_CONTROL_RATE_R_MAX_SP)/MAX_PPRZ;
tl_control_r_sp = -rc_values[RADIO_YAW];
tl_control_r_sp = -rc_values[RADIO_YAW] * RadOfDeg(TL_CONTROL_RATE_R_MAX_SP)/MAX_PPRZ;
//tl_control_r_sp = -rc_values[RADIO_YAW];
tl_control_power_sp = rc_values[RADIO_THROTTLE];
}
@@ -84,21 +85,28 @@ void tl_control_rate_run(void) {
const float cmd_p = tl_control_p_sp;
const float cmd_q = tl_control_q_sp;
// const float rate_err_r = tl_estimator_uf_r - tl_control_r_sp;
// const float rate_d_err_r = rate_err_r - tl_control_rate_last_err_r;
// tl_control_rate_last_err_r = rate_err_r;
// const float cmd_r = tl_control_rate_r_pgain * ( rate_err_r + tl_control_rate_r_dgain * rate_d_err_r );
const float cmd_r = tl_control_r_sp;
const float rate_err_r = estimator_r - tl_control_r_sp;
const float rate_d_err_r = rate_err_r - tl_control_rate_last_err_r;
tl_control_rate_last_err_r = rate_err_r;
const float cmd_r = - tl_control_rate_r_pgain * ( rate_err_r + tl_control_rate_r_dgain * rate_d_err_r );
tl_control_commands[COMMAND_ROLL] = TRIM_PPRZ((int16_t)cmd_p);
tl_control_commands[COMMAND_PITCH] = TRIM_PPRZ((int16_t)cmd_q);
tl_control_commands[COMMAND_YAW] = TRIM_PPRZ((int16_t)cmd_r);
tl_control_commands[COMMAND_THROTTLE] = kill_throttle ? 0 : TRIM_PPRZ((int16_t) (tl_control_power_sp));
// tl_control_commands[COMMAND_THROTTLE] = kill_throttle ? 0 : TRIM_PPRZ((int16_t) (tl_control_power_sp));
tl_control_commands[COMMAND_THROTTLE] = TRIM_PPRZ((int16_t) (tl_control_power_sp));
}
void tl_control_attitude_read_setpoints_from_rc(void) {
#if 0
if (!estimator_in_flight)
tl_control_attitude_psi_sp = estimator_psi;
else {
tl_control_attitude_psi_sp += rc_values[RADIO_YAW] * RadOfDeg(TL_CONTROL_ATTITUDE_PSI_MAX_SP)/MAX_PPRZ;
NormRadAngle(tl_control_attitude_psi_sp);
}
#endif
}
void tl_control_attitude_run(void) {
sw/airborne/tl_estimator.c
+15
View File
@@ -1,5 +1,6 @@
#include "tl_estimator.h"
#include "gps.h"
#include "tl_imu.h"
#include "flight_plan.h"
bool_t estimator_in_flight;
@@ -16,7 +17,9 @@ float estimator_speed; /* m/s */
float estimator_climb; /* m/s */
float estimator_course; /* rad, CCW */
float estimator_r;
float estimator_psi; /* rad, CCW */
float estimator_z_baro;
void tl_estimator_init(void) {
tl_estimator_u = 0.;
@@ -37,6 +40,17 @@ void tl_estimator_use_gps(void) {
estimator_course = RadOfDeg(gps_course / 10.);
}
void tl_estimator_use_gyro(void) {
estimator_r = tl_imu_r;
}
void tl_estimator_use_mag(void) {
estimator_psi = -atan2(tl_imu_hy, tl_imu_hx);
/* FIXME */
EstimatorSetAlt(tl_imu_z_baro);
}
void tl_estimator_to_body_frame(float east, float north,
float *front, float *right) {
float c = cos(estimator_psi);
@@ -45,3 +59,4 @@ void tl_estimator_to_body_frame(float east, float north,
*front = c * north + s * east;
*right = - s * north + c * east;
}
sw/airborne/tl_estimator.h
+8 -2
View File
@@ -17,11 +17,17 @@ extern float estimator_speed; /* m/s */
extern float estimator_climb; /* m/s */
extern float estimator_course; /* rad, CCW */
extern float estimator_psi; /* rad, CCW */
extern float estimator_r;
extern float estimator_psi; /* rad, CCW */
extern float estimator_z_baro;
void tl_estimator_init(void);
void tl_estimator_use_gps(void);
void tl_estimator_use_gyro(void);
void tl_estimator_use_mag(void);
#define EstimatorSetAlt(_z) {estimator_z_baro = _z;}
void tl_estimator_to_body_frame(float east, float north, float *front, float *right);
sw/airborne/tl_main.c
+16 -4
View File
@@ -14,6 +14,7 @@
#include "tl_telemetry.h"
#include "datalink.h"
#include "tl_autopilot.h"
#include "tl_imu.h"
#include "tl_estimator.h"
#include "adc.h"
#include "tl_bat.h"
@@ -49,8 +50,12 @@ static inline void tl_main_init( void ) {
ppm_init();
radio_control_init();
tl_imu_init();
tl_estimator_init();
tl_control_init();
uart0_init_tx();
uart1_init_tx();
@@ -60,19 +65,23 @@ static inline void tl_main_init( void ) {
}
static inline void tl_main_periodic_task( void ) {
static inline void tl_main_periodic_task( void ) {
tl_bat_periodic_task();
TlImuPeriodic();
tl_estimator_use_gyro();
tl_autopilot_periodic_task();
radio_control_periodic_task();
if (rc_status != RC_OK)
tl_autopilot_mode = TL_AP_MODE_FAILSAFE;
/* 4Hz */
RunOnceEvery(15, { common_nav_periodic_task_4Hz(); tl_nav_periodic_task(); });
/* run control loops */
tl_autopilot_periodic_task();
tl_telemetry_periodic_task();
SetActuatorsFromCommands(commands);
@@ -83,5 +92,8 @@ static inline void tl_main_event_task( void ) {
RadioControlEventCheckAndHandle(tl_autopilot_on_rc_event);
GpsEventCheckAndHandle(tl_estimator_use_gps, !estimator_in_flight);
DlEventCheckAndHandle();
TlImuEventCheckAndHandle(tl_estimator_use_mag);
}
sw/airborne/tl_telemetry.h
+14 -2
View File
@@ -10,11 +10,10 @@
#include "actuators.h"
#include "tl_bat.h"
#include "tl_estimator.h"
#include "tl_imu.h"
#include "tl_nav.h"
#include "tl_control.h"
#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
#define PERIODIC_SEND_ALIVE() DOWNLINK_SEND_ALIVE()
@@ -41,7 +40,20 @@
#define PERIODIC_SEND_NAVIGATION() SEND_NAVIGATION()
#define PERIODIC_SEND_IMU_GYRO() DOWNLINK_SEND_IMU_GYRO(&tl_imu_r, &tl_imu_r, &tl_imu_r)
#define PERIODIC_SEND_IMU_MAG() DOWNLINK_SEND_IMU_MAG(&tl_imu_hx, &tl_imu_hy, &tl_imu_hz)
#define PERIODIC_SEND_TL_ESTIMATOR() DOWNLINK_SEND_TL_ESTIMATOR(&estimator_r, &estimator_psi, &estimator_z_baro)
#define PERIODIC_SEND_RATE_LOOP() DOWNLINK_SEND_BOOZ_RATE_LOOP(&estimator_r, &tl_control_r_sp, &estimator_r, &tl_control_r_sp, &estimator_r, &tl_control_r_sp)
#if 0
#define PERIODIC_SEND_TL_ESTIMATOR() { \
float d1 = tl_baro_d[0]; \
float d2 = tl_baro_d[1]; \
DOWNLINK_SEND_TL_ESTIMATOR(&estimator_z_baro, &d1, &d2); \
}
#endif
extern uint8_t telemetry_mode_Ap;