diff --git a/conf/airframes/tl.xml b/conf/airframes/tl.xml
index fb9837a7da..176d5c7402 100644
--- a/conf/airframes/tl.xml
+++ b/conf/airframes/tl.xml
@@ -56,13 +56,22 @@
+
+
+
+
@@ -107,13 +116,54 @@ 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 TL_BARO_W1=0xABDC -D TL_BARO_W2=0x849A -D TL_BARO_W3=0X939E -D TL_BARO_W4=0xB259
+#ap.CFLAGS += -D TL_BARO_W1=0xABDC -D TL_BARO_W2=0x849A -D TL_BARO_W3=0X939E -D TL_BARO_W4=0xB259
+
+
+#
+#
+#
+
+tmm.ARCHDIR = $(ARCHI)
+tmm.ARCH = arm7tdmi
+tmm.TARGET = tmm
+tmm.TARGETDIR = tmm
+
+tmm.CFLAGS += -DCONFIG=\"tiny_1_1.h\" -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC(16.666e-3)'
+tmm.srcs = tl_test_mm_main.c sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c
+
+tmm.CFLAGS += -DLED -DLED -DTIME_LED=1
+tmm.CFLAGS += -DDOWNLINK -DUSE_UART0 -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart0 -DPPRZ_UART=Uart0 -DUART0_BAUD=B9600
+tmm.srcs += downlink.c $(SRC_ARCH)/uart_hw.c pprz_transport.c
+
+tmm.srcs += micromag.c $(SRC_ARCH)/micromag_hw.c
+
+#
+#
+#
+
+timu.ARCHDIR = $(ARCHI)
+timu.ARCH = arm7tdmi
+timu.TARGET = timu
+timu.TARGETDIR = timu
+
+timu.CFLAGS += -DCONFIG=\"tiny_1_1.h\" -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC(16.666e-3)'
+timu.srcs = tl_test_imu_main.c sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c
+
+timu.CFLAGS += -DLED -DLED -DTIME_LED=1
+timu.CFLAGS += -DDOWNLINK -DUSE_UART0 -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart0 -DPPRZ_UART=Uart0 -DUART0_BAUD=B9600
+timu.srcs += downlink.c $(SRC_ARCH)/uart_hw.c pprz_transport.c
+
+timu.srcs += $(SRC_ARCH)/tl_imu.c
+timu.CFLAGS += -DADC -DUSE_ADC_5 -DADC_CHANNEL_GR=ADC_5
+timu.srcs += $(SRC_ARCH)/adc_hw.c
+timu.srcs += $(SRC_ARCH)/tl_baro.c
+timu.srcs += micromag.c $(SRC_ARCH)/micromag_hw.c
+
diff --git a/conf/flight_plans/tl.xml b/conf/flight_plans/tl.xml
index 75bd1817aa..7117068881 100644
--- a/conf/flight_plans/tl.xml
+++ b/conf/flight_plans/tl.xml
@@ -1,4 +1,4 @@
-
+
@@ -19,7 +19,13 @@
-
+
+
+
+
+
+
+
diff --git a/conf/messages.xml b/conf/messages.xml
index 4bb9a24dea..77c7e5b3c4 100644
--- a/conf/messages.xml
+++ b/conf/messages.xml
@@ -541,12 +541,47 @@
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
@@ -591,12 +626,13 @@
+
diff --git a/conf/settings/tl.xml b/conf/settings/tl.xml
index 8a161178de..7abcdfc6ae 100644
--- a/conf/settings/tl.xml
+++ b/conf/settings/tl.xml
@@ -7,6 +7,17 @@
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/telemetry/tl.xml b/conf/telemetry/tl.xml
index e3a1aacb59..5000c220bc 100644
--- a/conf/telemetry/tl.xml
+++ b/conf/telemetry/tl.xml
@@ -9,17 +9,21 @@
-
-
-
+
-
+
-
-
+
+
+
+
+
+
+
+
diff --git a/sw/airborne/arm7/micromag_hw.c b/sw/airborne/arm7/micromag_hw.c
index bf5c8e7676..d76df700de 100644
--- a/sw/airborne/arm7/micromag_hw.c
+++ b/sw/airborne/arm7/micromag_hw.c
@@ -15,12 +15,12 @@
#include "micromag.h"
-
volatile uint8_t micromag_cur_axe;
-static void EXTINT2_ISR(void) __attribute__((naked));
+static void EXTINT_ISR(void) __attribute__((naked));
void micromag_hw_init( void ) {
+
/* configure SS pin */
SetBit(MM_SS_IODIR, MM_SS_PIN); /* pin is output */
MmUnselect(); /* pin idles high */
@@ -30,31 +30,32 @@ void micromag_hw_init( void ) {
MmReset(); /* pin idles low */
/* configure DRDY pin */
- /* connected pin to EINT2 */
+ /* connected pin to EXINT */
MM_DRDY_PINSEL |= MM_DRDY_PINSEL_VAL << MM_DRDY_PINSEL_BIT;
SetBit(EXTMODE, MM_DRDY_EINT); /* EINT is edge trigered */
SetBit(EXTPOLAR,MM_DRDY_EINT); /* EINT is trigered on rising edge */
SetBit(EXTINT,MM_DRDY_EINT); /* clear pending EINT */
/* initialize interrupt vector */
- 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
+ VICIntSelect &= ~VIC_BIT( MM_DRDY_VIC_IT ); /* select EINT as IRQ source */
+ VICIntEnable = VIC_BIT( MM_DRDY_VIC_IT ); /* enable it */
+ VICVectCntl9 = VIC_ENABLE | MM_DRDY_VIC_IT;
+ VICVectAddr9 = (uint32_t)EXTINT_ISR; // address of the ISR
+
}
void micromag_read( void ) {
if (micromag_status == MM_IDLE ) {
MmSelect();
- SpiEnable();
SpiDisableRti();
+ SpiEnable();
micromag_cur_axe = 0;
micromag_status = MM_BUSY;
MmTriggerRead();
}
}
-void EXTINT2_ISR(void) {
+void EXTINT_ISR(void) {
ISR_ENTRY();
/* read dummy control byte reply */
uint8_t foo __attribute__ ((unused)) = SSPDR;
@@ -62,8 +63,9 @@ void EXTINT2_ISR(void) {
SSPDR = 0;
SSPDR = 0;
/* enable timeout interrupt */
+ SpiClearRti();
SpiEnableRti();
- /* clear EINT2 */
+ /* clear EINT */
SetBit(EXTINT,MM_DRDY_EINT); /* clear EINT2 */
VICVectAddr = 0x00000000; /* clear this interrupt from the VIC */
diff --git a/sw/airborne/arm7/micromag_hw.h b/sw/airborne/arm7/micromag_hw.h
index 2aa76589d6..83734ff5ac 100644
--- a/sw/airborne/arm7/micromag_hw.h
+++ b/sw/airborne/arm7/micromag_hw.h
@@ -21,7 +21,7 @@ extern volatile uint8_t micromag_cur_axe;
MmSet(); \
MmReset(); \
uint8_t control_byte = (micromag_cur_axe+1) << 0 | 4 << 4; \
- SpiSend(control_byte); \
+ SSPDR = control_byte; /* SpiSend(control_byte); */ \
}
#if 0
diff --git a/sw/airborne/arm7/tl_baro.c b/sw/airborne/arm7/tl_baro.c
index fdb67a6aa5..9a28d9dab7 100644
--- a/sw/airborne/arm7/tl_baro.c
+++ b/sw/airborne/arm7/tl_baro.c
@@ -42,7 +42,7 @@ void tl_baro_init(void) {
/* setup slave select */
/* configure SS pin */
SetBit(TL_BARO_SS_IODIR, TL_BARO_SS_PIN); /* pin is output */
- TlBaroUnselect(); /* pin idles high */
+ TlBaroUnselect(); /* pin low */
tl_baro_cur_data = TL_BARO_PRESSURE;
@@ -64,22 +64,20 @@ 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();
+ SpiEnable();
+ SpiEnableRti();
SSPDR = CMD_MEASUREMENT;
SSPDR = tl_baro_cmd[tl_baro_cur_data];
-
}
void tl_baro_read(void) {
TlBaroSelect();
+ SpiSetCPHA();
SpiEnable();
SpiEnableRti();
- SpiSetCPHA();
SSPDR = 0;
SSPDR = 0;
-
}
void tl_baro_compute(void) {
diff --git a/sw/airborne/arm7/tl_baro.h b/sw/airborne/arm7/tl_baro.h
index 03bc20dfed..6a9e2acd36 100644
--- a/sw/airborne/arm7/tl_baro.h
+++ b/sw/airborne/arm7/tl_baro.h
@@ -26,9 +26,21 @@ extern uint16_t tl_baro_temp;
#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 TlBaroOnSpiIntReading() { \
+ tl_baro_d[tl_baro_cur_data] = SSPDR<<8; \
+ tl_baro_d[tl_baro_cur_data] |= SSPDR; \
+ TlBaroUnselect(); \
+ SpiDisable(); \
+ SpiDisableRti(); \
+ }
-#define TlBaroOnSpiIntSending() { uint8_t foo = SSPDR; foo = SSPDR; TlBaroUnselect(); SpiDisable(); SpiDisableRti(); }
+#define TlBaroOnSpiIntSending() { \
+ uint8_t foo __attribute__ ((unused)) = SSPDR; \
+ uint8_t foo1 __attribute__ ((unused)) = SSPDR; \
+ TlBaroUnselect(); \
+ SpiDisable(); \
+ SpiDisableRti(); \
+ }
extern void tl_baro_init(void);
diff --git a/sw/airborne/arm7/tl_imu.c b/sw/airborne/arm7/tl_imu.c
index 377aeab8b2..c2182e25bb 100644
--- a/sw/airborne/arm7/tl_imu.c
+++ b/sw/airborne/arm7/tl_imu.c
@@ -2,8 +2,12 @@
#include "LPC21xx.h"
#include "interrupt_hw.h"
+#include "adc.h"
-uint8_t tl_imu_status;
+#include "downlink.h"
+#include "messages.h"
+
+volatile uint8_t tl_imu_status;
struct adc_buf buf_gr;
@@ -11,7 +15,7 @@ float tl_imu_r;
float tl_imu_hx;
float tl_imu_hy;
float tl_imu_hz;
-float tl_imu_z_baro;
+float tl_imu_pressure;
static void SPI1_ISR(void) __attribute__((naked));
@@ -34,6 +38,7 @@ void tl_imu_init(void) {
micromag_init();
tl_baro_init();
+#if defined TL_IMU_USE_BARO | defined TL_IMU_USE_MICROMAG
/* setup pins for SSP (SCK, MISO, MOSI) */
PINSEL1 |= 2 << 2 | 2 << 4 | 2 << 6;
@@ -47,7 +52,7 @@ void tl_imu_init(void) {
VICIntEnable = VIC_BIT(VIC_SPI1); // SPI1 interrupt enabled
VICVectCntl7 = VIC_ENABLE | VIC_SPI1;
VICVectAddr7 = (uint32_t)SPI1_ISR; // address of the ISR
-
+#endif
tl_imu_r = 0.;
tl_imu_hx = 1.;
@@ -58,6 +63,15 @@ void tl_imu_init(void) {
}
+void tl_imu_periodic(void) {
+ /* read gyro */
+ tl_imu_r = GR_GAIN * ( buf_gr.sum - GR_NEUTRAL);
+#ifdef TL_IMU_USE_BARO
+ RunOnceEvery(3, {tl_baro_read(); tl_imu_status = TL_IMU_READING_BARO;});
+#elif defined TL_IMU_USE_MICROMAG
+ RunOnceEvery(3, { MmUnselect();micromag_read(); tl_imu_status = TL_IMU_READING_MICROMAG;});
+#endif
+ }
void SPI1_ISR(void) {
@@ -71,10 +85,12 @@ void SPI1_ISR(void) {
break;
case TL_IMU_SENDING_BARO_REQ: {
TlBaroOnSpiIntSending();
- //micromag_read();
- //tl_imu_status = TL_IMU_READING_MICROMAG;
+#ifdef TL_IMU_USE_MICROMAG
+ micromag_read();
+ tl_imu_status = TL_IMU_READING_MICROMAG;
+#else
tl_imu_status = TL_IMU_DATA_AVAILABLE;
-
+#endif
}
break;
case TL_IMU_READING_MICROMAG: {
diff --git a/sw/airborne/arm7/tl_imu.h b/sw/airborne/arm7/tl_imu.h
index 3a2528e96c..dbf38c158a 100644
--- a/sw/airborne/arm7/tl_imu.h
+++ b/sw/airborne/arm7/tl_imu.h
@@ -1,24 +1,28 @@
#ifndef TL_IMU_H
#define TL_IMU_H
+#define TL_IMU_USE_MICROMAG
+#define TL_IMU_USE_BARO
+
+
#include "std.h"
#include "adc.h"
#include "spi.h"
#include "micromag.h"
#include "tl_baro.h"
-#define GR_GAIN 2.105e-4
-#define GR_NEUTRAL 24880
+#define GR_GAIN 3.57e-4
+#define GR_NEUTRAL 24894
-#define HX_GAIN 1.
-#define HX_NEUTRAL 0
-#define HX_CHAN 0
-#define HY_GAIN 1.
-#define HY_NEUTRAL 0
-#define HY_CHAN 1
-#define HZ_GAIN 1.
-#define HZ_NEUTRAL 0
-#define HZ_CHAN 2
+#define HX_GAIN -1.
+#define HX_NEUTRAL 0.
+#define HX_CHAN 1
+#define HY_GAIN -1.
+#define HY_NEUTRAL 0
+#define HY_CHAN 0
+#define HZ_GAIN -1.
+#define HZ_NEUTRAL 0
+#define HZ_CHAN 2
#define TL_IMU_IDLE 0
@@ -29,21 +33,17 @@
extern void tl_imu_init(void);
+extern void tl_imu_periodic(void);
-extern uint8_t tl_imu_status;
+extern volatile uint8_t tl_imu_status;
extern float tl_imu_r;
extern float tl_imu_hx;
extern float tl_imu_hy;
extern float tl_imu_hz;
-extern float tl_imu_z_baro;
+extern float tl_imu_pressure;
extern struct adc_buf buf_gr;
-#define TlImuPeriodic() { \
- /* read gyro */ \
- tl_imu_r = GR_GAIN * ( buf_gr.sum - GR_NEUTRAL); \
- RunOnceEvery(3, {tl_baro_read(); tl_imu_status = TL_IMU_READING_BARO;}); \
- }
#define TlImuEventCheckAndHandle(user_callback) { \
if (tl_imu_status == TL_IMU_DATA_AVAILABLE) { \
@@ -51,7 +51,7 @@ extern struct adc_buf buf_gr;
tl_imu_hy = HY_GAIN * (float)( micromag_values[HY_CHAN] - HY_NEUTRAL); \
tl_imu_hz = HZ_GAIN * (float)( micromag_values[HZ_CHAN] - HZ_NEUTRAL); \
tl_baro_compute(); /* faire un truc avec les données baro */ \
- tl_imu_z_baro = tl_baro_pressure; \
+ tl_imu_pressure = tl_baro_pressure; \
user_callback(); \
micromag_status = MM_IDLE; \
tl_imu_status = TL_IMU_IDLE; \
diff --git a/sw/airborne/datalink.c b/sw/airborne/datalink.c
index c5c1b30d9c..04fe5615c0 100644
--- a/sw/airborne/datalink.c
+++ b/sw/airborne/datalink.c
@@ -34,6 +34,10 @@
#include "nav.h"
#include "datalink.h"
#include "flight_plan.h"
+<<<<<<< datalink.c
+#include "messages.h"
+=======
+>>>>>>> 1.34
#include "gps.h"
#include "uart.h"
#include "gpio.h"
diff --git a/sw/airborne/estimator.h b/sw/airborne/estimator.h
index 85dc4ded6c..b58fac423e 100644
--- a/sw/airborne/estimator.h
+++ b/sw/airborne/estimator.h
@@ -68,12 +68,6 @@ extern float estimator_airspeed; /* m/s */
void estimator_init( void );
-#ifdef IMU_3DMG
-void estimator_update_state_3DMG( void );
-#elif defined IMU_ANALOG && defined AHRS
-void estimator_update_state_ANALOG( void );
-#else //NO_IMU
-#endif
void estimator_propagate_state( void );
void estimator_update_state_gps( void );
diff --git a/sw/airborne/tl_autopilot.c b/sw/airborne/tl_autopilot.c
index 5d397dc6e8..47b2fdb761 100644
--- a/sw/airborne/tl_autopilot.c
+++ b/sw/airborne/tl_autopilot.c
@@ -3,7 +3,7 @@
#include "radio_control.h"
#include "commands.h"
#include "tl_control.h"
-// #include "tl_nav.h"
+#include "tl_nav.h"
uint8_t tl_autopilot_mode;
@@ -22,16 +22,18 @@ void tl_autopilot_periodic_task(void) {
tl_control_rate_run();
SetCommands(tl_control_commands);
break;
-/* case TL_AP_MODE_ATTITUDE: */
-/* tl_control_attitude_run(); */
-/* SetCommands(tl_control_commands); */
-/* break; */
-/* case TL_AP_MODE_NAV: */
-/* tl_nav_run(); */
-/* SetCommands(tl_control_commands); */
-/* break; */
- }
+ case TL_AP_MODE_ATTITUDE:
+ tl_control_attitude_run();
+ SetCommands(tl_control_commands);
+ break;
+ case TL_AP_MODE_NAV:
+ /* 4Hz */
+ RunOnceEvery(15, { common_nav_periodic_task_4Hz(); tl_nav_periodic_task(); });
+ tl_control_attitude_run();
+ SetCommands(tl_control_commands);
+ break;
+ }
}
@@ -46,11 +48,11 @@ void tl_autopilot_on_rc_event(void) {
case TL_AP_MODE_RATE:
tl_control_rate_read_setpoints_from_rc();
break;
-/* case TL_AP_MODE_ATTITUDE: */
-/* tl_control_attitude_read_setpoints_from_rc(); */
-/* break; */
-/* case TL_AP_MODE_NAV: */
-/* tl_nav_read_setpoints_from_rc(); */
-/* break; */
+ case TL_AP_MODE_ATTITUDE:
+ tl_control_attitude_read_setpoints_from_rc();
+ break;
+ case TL_AP_MODE_NAV:
+ tl_control_nav_read_setpoints_from_rc();
+ break;
}
}
diff --git a/sw/airborne/tl_autopilot.h b/sw/airborne/tl_autopilot.h
index d9af3ab66f..34d043a3a1 100644
--- a/sw/airborne/tl_autopilot.h
+++ b/sw/airborne/tl_autopilot.h
@@ -17,9 +17,24 @@ extern void tl_autopilot_on_rc_event(void);
#define TRESHOLD_RATE_PPRZ (MIN_PPRZ / 2)
#define TRESHOLD_ATTITUDE_PPRZ (MAX_PPRZ/2)
+
+#if 0
#define TL_AP_MODE_OF_PPRZ(mode) \
((mode) < TRESHOLD_RATE_PPRZ ? TL_AP_MODE_RATE : \
(mode) < TRESHOLD_ATTITUDE_PPRZ ? TL_AP_MODE_ATTITUDE : \
TL_AP_MODE_NAV )
+#define TL_AP_MODE_OF_PPRZ(mode) \
+ ((mode) < TRESHOLD_RATE_PPRZ ? TL_AP_MODE_RATE : \
+ TL_AP_MODE_ATTITUDE )
+
+#endif // 0
+
+#define TL_AP_MODE_OF_PPRZ(mode) \
+ ((mode) < TRESHOLD_RATE_PPRZ ? TL_AP_MODE_ATTITUDE : \
+ TL_AP_MODE_NAV )
+
+
+
+
#endif /* TL_AUTOPILOT_H */
diff --git a/sw/airborne/tl_control.c b/sw/airborne/tl_control.c
index 78d60009a4..5f49a6c87e 100644
--- a/sw/airborne/tl_control.c
+++ b/sw/airborne/tl_control.c
@@ -14,12 +14,17 @@ float tl_control_power_sp;
float tl_control_rate_r_pgain;
float tl_control_rate_r_dgain;
+float tl_control_rate_r_igain;
float tl_control_rate_last_err_r;
+float tl_control_rate_sum_err_r;
+
+
pprz_t tl_control_commands[COMMANDS_NB];
-#define TL_CONTROL_RATE_R_PGAIN -600.
+#define TL_CONTROL_RATE_R_PGAIN -120.
#define TL_CONTROL_RATE_R_DGAIN 5.
+#define TL_CONTROL_RATE_R_IGAIN 0.2
/* setpoints for max stick throw in degres per second */
#define TL_CONTROL_RATE_R_MAX_SP 100.
@@ -29,20 +34,23 @@ 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; */
-/* float tl_control_attitude_psi_dgain; */
+float tl_control_attitude_psi_pgain;
+float tl_control_attitude_psi_dgain;
+float tl_control_attitude_psi_igain;
+float tl_control_attitude_psi_sum_err;
-/* #define TL_CONTROL_ATTITUDE_PHI_THETA_PGAIN -1250. */
-/* #define TL_CONTROL_ATTITUDE_PHI_THETA_DGAIN -700. */
-/* #define TL_CONTROL_ATTITUDE_PSI_PGAIN -1050. */
-/* #define TL_CONTROL_ATTITUDE_PSI_DGAIN -850. */
+#define TL_CONTROL_ATTITUDE_PSI_PGAIN -500.
+#define TL_CONTROL_ATTITUDE_PSI_DGAIN -150.
+#define TL_CONTROL_ATTITUDE_PSI_IGAIN -0.20
+#define TL_CONTROL_ATTITUDE_PSI_TRIM 350
+
/* 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 (90. / 40.) /* called with RC messags */
+
+#define TL_CONTROL_TRIM_R 300
+int16_t tl_control_trim_r;
void tl_control_init(void) {
@@ -56,15 +64,18 @@ void tl_control_init(void) {
tl_control_rate_r_pgain = TL_CONTROL_RATE_R_PGAIN;
tl_control_rate_r_dgain = TL_CONTROL_RATE_R_DGAIN;
+ tl_control_rate_r_igain = TL_CONTROL_RATE_R_IGAIN;
-/* tl_control_attitude_phi_sp = 0.; */
-/* tl_control_attitude_theta_sp =0.; */
-/* tl_control_attitude_psi_sp =0.; */
-/* tl_control_attitude_phi_theta_pgain = TL_CONTROL_ATTITUDE_PHI_THETA_PGAIN; */
-/* tl_control_attitude_phi_theta_dgain = TL_CONTROL_ATTITUDE_PHI_THETA_DGAIN; */
-/* tl_control_attitude_psi_pgain = TL_CONTROL_ATTITUDE_PSI_PGAIN; */
-/* tl_control_attitude_psi_dgain = TL_CONTROL_ATTITUDE_PSI_DGAIN; */
+ tl_control_attitude_phi_sp = 0.;
+ tl_control_attitude_theta_sp =0.;
+ tl_control_attitude_psi_sp =0.;
+ tl_control_attitude_psi_pgain = TL_CONTROL_ATTITUDE_PSI_PGAIN;
+ tl_control_attitude_psi_dgain = TL_CONTROL_ATTITUDE_PSI_DGAIN;
+ tl_control_attitude_psi_igain = TL_CONTROL_ATTITUDE_PSI_IGAIN;
+
+ tl_control_trim_r = TL_CONTROL_TRIM_R;
+
}
@@ -85,30 +96,86 @@ 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 = 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 );
+ if (estimator_in_flight) {
+ 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;
+ tl_control_rate_sum_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_rate_r_igain * tl_control_rate_sum_err_r);
+
+ tl_control_commands[COMMAND_YAW] = TRIM_PPRZ((int16_t)cmd_r + tl_control_trim_r);
+ // tl_control_commands[COMMAND_THROTTLE] = kill_throttle ? 0 : TRIM_PPRZ((int16_t) (tl_control_power_sp));
+ }
+ else {
+ tl_control_rate_sum_err_r = 0.;
+ tl_control_commands[COMMAND_YAW] = 0;
+ }
+
+ tl_control_commands[COMMAND_THROTTLE] = TRIM_PPRZ((int16_t) (tl_control_power_sp));
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] = TRIM_PPRZ((int16_t) (tl_control_power_sp));
}
+// #define DO_STEPS
+
void tl_control_attitude_read_setpoints_from_rc(void) {
-#if 0
+ tl_control_attitude_phi_sp = -rc_values[RADIO_ROLL];
+ tl_control_attitude_theta_sp = rc_values[RADIO_PITCH];
+ tl_control_power_sp = rc_values[RADIO_THROTTLE];
+
+#ifndef DO_STEPS
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;
+ tl_control_attitude_psi_sp += -rc_values[RADIO_YAW] * RadOfDeg(TL_CONTROL_ATTITUDE_PSI_MAX_SP)/MAX_PPRZ;
NormRadAngle(tl_control_attitude_psi_sp);
}
+#else
+ if (cpu_time_sec % 8 < 4)
+ tl_control_attitude_psi_sp = RadOfDeg(-45.);
+ else
+ tl_control_attitude_psi_sp = RadOfDeg(45.);
#endif
+
}
void tl_control_attitude_run(void) {
+ const float cmd_p = tl_control_attitude_phi_sp;
+ const float cmd_q = tl_control_attitude_theta_sp;
+
+ if (estimator_in_flight) {
+ float err_psi = estimator_psi - tl_control_attitude_psi_sp;
+ NormRadAngle(err_psi);
+ tl_control_attitude_psi_sum_err += err_psi;
+
+ const float cmd_r = - (tl_control_attitude_psi_pgain * err_psi +
+ tl_control_attitude_psi_dgain * estimator_r +
+ tl_control_attitude_psi_igain * tl_control_attitude_psi_sum_err );
+
+ tl_control_commands[COMMAND_YAW] = TRIM_PPRZ((int16_t)cmd_r + TL_CONTROL_ATTITUDE_PSI_TRIM);
+ }
+ else {
+ tl_control_attitude_psi_sum_err = 0.;
+ tl_control_commands[COMMAND_YAW] = 0;
+ }
+ 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_THROTTLE] = TRIM_PPRZ((int16_t) (tl_control_power_sp));
+}
+
+
+void tl_control_nav_read_setpoints_from_rc(void) {
+ tl_control_power_sp = rc_values[RADIO_THROTTLE];
+ 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);
+ }
}
diff --git a/sw/airborne/tl_control.h b/sw/airborne/tl_control.h
index a50b569be6..e2696ba2a2 100644
--- a/sw/airborne/tl_control.h
+++ b/sw/airborne/tl_control.h
@@ -12,6 +12,8 @@ extern void tl_control_rate_run(void);
extern void tl_control_attitude_read_setpoints_from_rc(void);
extern void tl_control_attitude_run(void);
+extern void tl_control_nav_read_setpoints_from_rc(void);
+
extern bool_t kill_throttle;
extern float tl_control_p_sp;
@@ -23,6 +25,9 @@ extern float tl_control_rate_pq_pgain;
extern float tl_control_rate_pq_dgain;
extern float tl_control_rate_r_pgain;
extern float tl_control_rate_r_dgain;
+extern float tl_control_rate_r_igain;
+extern float tl_control_rate_sum_err_r;
+
extern float tl_control_attitude_phi_sp;
extern float tl_control_attitude_theta_sp;
@@ -32,7 +37,10 @@ extern float tl_control_attitude_phi_theta_pgain;
extern float tl_control_attitude_phi_theta_dgain;
extern float tl_control_attitude_psi_pgain;
extern float tl_control_attitude_psi_dgain;
+extern float tl_control_attitude_psi_igain;
+extern float tl_control_attitude_psi_sum_err;
+extern int16_t tl_control_trim_r;
#define TlControlAttitudeSetSetPoints(_phi_sp, _theta_sp, _psi_sp, _power_sp) { \
tl_control_attitude_phi_sp = _phi_sp; \
diff --git a/sw/airborne/tl_estimator.c b/sw/airborne/tl_estimator.c
index e275d790d0..00afe7e6b0 100644
--- a/sw/airborne/tl_estimator.c
+++ b/sw/airborne/tl_estimator.c
@@ -2,6 +2,7 @@
#include "gps.h"
#include "tl_imu.h"
#include "flight_plan.h"
+#include "tl_control.h"
bool_t estimator_in_flight;
uint16_t estimator_flight_time;
@@ -17,11 +18,50 @@ float estimator_speed; /* m/s */
float estimator_climb; /* m/s */
float estimator_course; /* rad, CCW */
+float estimator_speed_east;
+float estimator_speed_north;
+
float estimator_r;
float estimator_psi; /* rad, CCW */
float estimator_z_baro;
+static float estimator_cos_psi;
+static float estimator_sin_psi;
+
+
+#define DT_UPDATE (1./60.)
+static const float Q_angle = 0.00025;
+static const float Q_bias = 0.0005;
+static const float R_accel = 0.5;
+
+#define TL_PSI_KALM_UNINIT 0
+#define TL_PSI_KALM_RUNNING 1
+
+uint8_t tl_psi_kalm_status;
+float tl_psi_kalm_psi;
+float tl_psi_kalm_r;
+float tl_psi_kalm_bias;
+float tl_psi_kalm_P[2][2];
+
+
+static inline void compute_dcm(void) {
+ estimator_cos_psi = cos(estimator_psi);
+ estimator_sin_psi = sin(estimator_psi);
+}
+
+void tl_estimator_to_body_frame(float east, float north,
+ float *front, float *right) {
+ *front = estimator_cos_psi * north + estimator_sin_psi * east;
+ *right = - estimator_sin_psi * north + estimator_cos_psi * east;
+}
+
+void tl_estimator_periodic_task(void) {
+ estimator_in_flight = tl_control_power_sp > 500;
+}
+
void tl_estimator_init(void) {
+ estimator_psi = 0.;
+ compute_dcm();
tl_estimator_u = 0.;
tl_estimator_v = 0.;
}
@@ -38,25 +78,100 @@ void tl_estimator_use_gps(void) {
estimator_speed = gps_gspeed / 100.;
estimator_climb = gps_climb / 100.;
estimator_course = RadOfDeg(gps_course / 10.);
+
+ estimator_speed_east = estimator_speed * sin(estimator_course);
+ estimator_speed_north = estimator_speed * cos(estimator_course);
+
+ tl_estimator_to_body_frame(estimator_speed_east, estimator_speed_north, &tl_estimator_u, &tl_estimator_v);
}
void tl_estimator_use_gyro(void) {
estimator_r = tl_imu_r;
+ if (tl_psi_kalm_status == TL_PSI_KALM_RUNNING) {
+ tl_psi_kalm_propagate(tl_imu_r);
+ estimator_psi = tl_psi_kalm_psi;
+ estimator_r = tl_psi_kalm_r;
+ compute_dcm();
+ }
}
-void tl_estimator_use_mag(void) {
- estimator_psi = -atan2(tl_imu_hy, tl_imu_hx);
- /* FIXME */
- EstimatorSetAlt(tl_imu_z_baro);
+void tl_estimator_use_imu(void) {
+ float estimator_psi_measure = -atan2(tl_imu_hy, tl_imu_hx);
+ const float ground_pressure = 98000;
+ estimator_z_baro = (ground_pressure - (float)tl_imu_pressure)*0.084;
+
+ if (tl_psi_kalm_status == TL_PSI_KALM_UNINIT) {
+ tl_psi_kalm_start(estimator_r, estimator_psi);
+ estimator_psi = estimator_psi_measure;
+ compute_dcm();
+ } else {
+ tl_psi_kalm_update(estimator_psi_measure);
+ }
+}
+
+
+
+
+void tl_psi_kalm_init(void) {
+ tl_psi_kalm_status = TL_PSI_KALM_UNINIT;
+}
+
+void tl_psi_kalm_start( float gyro, float angle) {
+ tl_psi_kalm_psi = angle;
+ tl_psi_kalm_bias = gyro;
+ tl_psi_kalm_r = 0.;
+ tl_psi_kalm_P[0][0] = 1.;
+ tl_psi_kalm_P[0][1] = 0.;
+ tl_psi_kalm_P[1][0] = 0.;
+ tl_psi_kalm_P[0][1] = 1.;
+ tl_psi_kalm_status = TL_PSI_KALM_RUNNING;
}
-void tl_estimator_to_body_frame(float east, float north,
- float *front, float *right) {
- float c = cos(estimator_psi);
- float s = sin(estimator_psi);
-
- *front = c * north + s * east;
- *right = - s * north + c * east;
+void tl_psi_kalm_propagate( float gyro) {
+ /* r_est = r_measure - bias */
+ tl_psi_kalm_r = gyro - tl_psi_kalm_bias;
+
+ /* psi += r * dt */
+ tl_psi_kalm_psi += (tl_psi_kalm_r*DT_UPDATE);
+ NormRadAngle(tl_psi_kalm_psi);
+
+ const float P_dot00 = Q_angle - tl_psi_kalm_P[0][1] - tl_psi_kalm_P[1][0];
+ const float P_dot01 = - tl_psi_kalm_P[1][1];
+ const float P_dot10 = - tl_psi_kalm_P[1][1];
+ const float P_dot11 = Q_bias;
+ /* P += Pdot * dt */
+ tl_psi_kalm_P[0][0] += (P_dot00 * DT_UPDATE);
+ tl_psi_kalm_P[0][1] += (P_dot01 * DT_UPDATE);
+ tl_psi_kalm_P[1][0] += (P_dot10 * DT_UPDATE);
+ tl_psi_kalm_P[1][1] += (P_dot11 * DT_UPDATE);
+}
+
+//#define PROPAGATE_ONLY 1
+
+void tl_psi_kalm_update( float angle) {
+#ifndef PROPAGATE_ONLY
+ float err = angle - tl_psi_kalm_psi;
+ NormRadAngle(err);
+ const float Pct_0 = tl_psi_kalm_P[0][0];
+ const float Pct_1 = tl_psi_kalm_P[1][0];
+ /* E = C P C' + R */
+ const float E = R_accel + Pct_0;
+ /* K = P C' inv(E) */
+ const float K_0 = Pct_0 / E;
+ const float K_1 = Pct_1 / E;
+ /* P = P - K C P */
+ const float t_0 = Pct_0;
+ const float t_1 = tl_psi_kalm_P[0][1];
+
+ tl_psi_kalm_P[0][0] -= K_0 * t_0;
+ tl_psi_kalm_P[0][1] -= K_0 * t_1;
+ tl_psi_kalm_P[1][0] -= K_1 * t_0;
+ tl_psi_kalm_P[1][1] -= K_1 * t_1;
+
+ /* X += K * err */
+ tl_psi_kalm_psi += K_0 * err;
+ tl_psi_kalm_bias += K_1 * err;
+#endif
}
diff --git a/sw/airborne/tl_estimator.h b/sw/airborne/tl_estimator.h
index f1dfbbd616..9c796a98f5 100644
--- a/sw/airborne/tl_estimator.h
+++ b/sw/airborne/tl_estimator.h
@@ -17,6 +17,9 @@ extern float estimator_speed; /* m/s */
extern float estimator_climb; /* m/s */
extern float estimator_course; /* rad, CCW */
+extern float estimator_speed_east;
+extern float estimator_speed_north;
+
extern float estimator_r;
extern float estimator_psi; /* rad, CCW */
extern float estimator_z_baro;
@@ -24,7 +27,17 @@ 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);
+void tl_estimator_use_imu(void);
+void tl_estimator_periodic_task(void);
+
+extern float tl_psi_kalm_psi;
+extern float tl_psi_kalm_bias;
+extern float tl_psi_kalm_P[2][2];
+
+extern void tl_psi_kalm_init(void);
+extern void tl_psi_kalm_start( float gyro, float angle);
+extern void tl_psi_kalm_propagate( float gyro);
+extern void tl_psi_kalm_update( float angle);
#define EstimatorSetAlt(_z) {estimator_z_baro = _z;}
diff --git a/sw/airborne/tl_main.c b/sw/airborne/tl_main.c
index 3d57346386..e628281964 100644
--- a/sw/airborne/tl_main.c
+++ b/sw/airborne/tl_main.c
@@ -56,6 +56,8 @@ static inline void tl_main_init( void ) {
tl_control_init();
+ tl_nav_init();
+
uart0_init_tx();
uart1_init_tx();
@@ -69,7 +71,12 @@ static inline void tl_main_periodic_task( void ) {
tl_bat_periodic_task();
- TlImuPeriodic();
+ tl_estimator_periodic_task();
+
+ tl_imu_periodic();
+ if (telemetry_mode_Ap == TELEMETRY_MODE_Ap_test)
+ DOWNLINK_SEND_TL_GYRO_RAW(&(buf_gr.sum));
+
tl_estimator_use_gyro();
tl_autopilot_periodic_task();
@@ -79,9 +86,7 @@ static inline void tl_main_periodic_task( void ) {
if (rc_status != RC_OK)
tl_autopilot_mode = TL_AP_MODE_FAILSAFE;
- /* 4Hz */
- RunOnceEvery(15, { common_nav_periodic_task_4Hz(); tl_nav_periodic_task(); });
-
+
tl_telemetry_periodic_task();
SetActuatorsFromCommands(commands);
@@ -95,5 +100,5 @@ static inline void tl_main_event_task( void ) {
DlEventCheckAndHandle();
- TlImuEventCheckAndHandle(tl_estimator_use_mag);
+ TlImuEventCheckAndHandle(tl_estimator_use_imu);
}
diff --git a/sw/airborne/tl_nav.c b/sw/airborne/tl_nav.c
index a5d7bd9f08..2eb4c2e01b 100644
--- a/sw/airborne/tl_nav.c
+++ b/sw/airborne/tl_nav.c
@@ -7,9 +7,12 @@
float tl_nav_goto_h_pgain;
float tl_nav_goto_h_dgain;
+float tl_nav_goto_x_sp;
+float tl_nav_goto_y_sp;
+float y_unit_err_body, x_unit_err_body;
-#define TL_NAV_GOTO_MAX_PHI_COMMAND RadOfDeg(30.)
-#define TL_NAV_GOTO_MAX_THETA_COMMAND RadOfDeg(30.)
+#define TL_NAV_GOTO_MAX_PHI_COMMAND MAX_PPRZ
+#define TL_NAV_GOTO_MAX_THETA_COMMAND MAX_PPRZ
static void nav_goto_waypoint(uint8_t wp);
@@ -41,28 +44,34 @@ void nav_init_stage( void ) {
}
static void fly_to_xy(float x_sp, float y_sp) {
+
+ tl_nav_goto_x_sp = x_sp;
+ tl_nav_goto_y_sp = y_sp;
+
/* get a position error vector */
- float x_error = estimator_x - x_sp;
- float y_error = estimator_y - y_sp;
+ float x_error = estimator_x - tl_nav_goto_x_sp;
+ float y_error = estimator_y - tl_nav_goto_y_sp;
/* Normalize the error */
float norm_error = sqrt(x_error*x_error+ y_error*y_error);
+ if (norm_error < 1e-3) norm_error = 1e-3;
x_error /= norm_error;
y_error /= norm_error;
/* convert to body frame */
- float y_unit_err_body, x_unit_err_body;
tl_estimator_to_body_frame(x_error, y_error, &x_unit_err_body, &y_unit_err_body);
/* Compute command */
- float tl_nav_goto_phi_command = - tl_nav_goto_h_pgain * y_unit_err_body * norm_error + - tl_nav_goto_h_dgain * tl_estimator_v;
- float tl_nav_goto_theta_command = tl_nav_goto_h_pgain * x_unit_err_body * norm_error + tl_nav_goto_h_dgain * tl_estimator_u;
+ float tl_nav_goto_phi_command = - (tl_nav_goto_h_pgain * y_unit_err_body * norm_error +
+ tl_nav_goto_h_dgain * tl_estimator_v);
+ float tl_nav_goto_theta_command = (tl_nav_goto_h_pgain * x_unit_err_body * norm_error +
+ tl_nav_goto_h_dgain * tl_estimator_u);
Bound(tl_nav_goto_phi_command, -TL_NAV_GOTO_MAX_PHI_COMMAND, TL_NAV_GOTO_MAX_PHI_COMMAND);
Bound(tl_nav_goto_theta_command, -TL_NAV_GOTO_MAX_THETA_COMMAND, TL_NAV_GOTO_MAX_THETA_COMMAND);
tl_control_attitude_phi_sp = tl_nav_goto_phi_command;
- tl_control_attitude_theta_sp = tl_nav_goto_theta_command;
+ tl_control_attitude_theta_sp = - tl_nav_goto_theta_command;
}
static void nav_goto_waypoint(uint8_t wp) {
diff --git a/sw/airborne/tl_nav.h b/sw/airborne/tl_nav.h
index 19f97f9324..286b1dc478 100644
--- a/sw/airborne/tl_nav.h
+++ b/sw/airborne/tl_nav.h
@@ -3,10 +3,15 @@
#include "common_nav.h"
+extern float tl_nav_goto_h_pgain;
+extern float tl_nav_goto_h_dgain;
+extern float tl_nav_goto_x_sp;
+extern float tl_nav_goto_y_sp;
+extern float y_unit_err_body, x_unit_err_body;
+
void tl_nav_init(void);
/** To be called at 4Hz */
void tl_nav_periodic_task(void);
-
#endif // TL_NAV_H
diff --git a/sw/airborne/tl_telemetry.h b/sw/airborne/tl_telemetry.h
index b8439ad6ce..166e0fd02c 100644
--- a/sw/airborne/tl_telemetry.h
+++ b/sw/airborne/tl_telemetry.h
@@ -1,4 +1,5 @@
#ifndef TL_TELEMETRY_H
+#define TL_TELEMETRY_H
#include "std.h"
#include "messages.h"
@@ -13,6 +14,7 @@
#include "tl_imu.h"
#include "tl_nav.h"
#include "tl_control.h"
+#include "tl_autopilot.h"
#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
@@ -43,17 +45,32 @@
#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_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); \
+#define PERIODIC_SEND_LOOP() { \
+ switch (tl_autopilot_mode) { \
+ case TL_AP_MODE_RATE : \
+ DOWNLINK_SEND_TL_RATE_LOOP_R(&estimator_r, &tl_control_r_sp, &tl_control_rate_sum_err_r); \
+ break; \
+ case TL_AP_MODE_ATTITUDE : \
+ DOWNLINK_SEND_TL_ATTITUDE_LOOP_R(&estimator_psi, &tl_control_attitude_psi_sp, &tl_control_attitude_psi_sum_err, &estimator_r); \
+ break; \
+ case TL_AP_MODE_NAV : \
+ DOWNLINK_SEND_BOOZ_HOV_LOOP(&tl_nav_goto_x_sp, &tl_nav_goto_y_sp, &tl_estimator_u, &estimator_x, &tl_estimator_v, &estimator_y, & tl_control_attitude_phi_sp, & tl_control_attitude_theta_sp); \
+ break; \
+ } \
+ }
+
+#define PERIODIC_SEND_TL_DEBUG() DOWNLINK_SEND_TL_DEBUG(&x_unit_err_body, &y_unit_err_body)
+
+
+#define PERIODIC_SEND_TL_KALM_PSI_STATE() { \
+ DOWNLINK_SEND_TL_KALM_PSI_STATE(&tl_psi_kalm_psi, &tl_psi_kalm_bias); \
+ }
+
+#define PERIODIC_SEND_TL_KALM_PSI_COV() { \
+ DOWNLINK_SEND_TL_KALM_PSI_COV(&tl_psi_kalm_P[0][0], &tl_psi_kalm_P[0][1], &tl_psi_kalm_P[1][1]); \
}
-#endif
extern uint8_t telemetry_mode_Ap;
@@ -61,5 +78,4 @@ static inline void tl_telemetry_periodic_task(void) {
PeriodicSendAp()
}
-#define TL_TELEMETRY_H
#endif //TL_TELEMETRY_H
diff --git a/sw/include/std.h b/sw/include/std.h
index 03555b46e3..e9e511d120 100644
--- a/sw/include/std.h
+++ b/sw/include/std.h
@@ -94,7 +94,7 @@ typedef uint8_t unit_t;
#define RunOnceEvery(_prescaler, _code) { \
static uint8_t prescaler = 0; \
prescaler++; \
- if (prescaler > _prescaler) { \
+ if (prescaler >= _prescaler) { \
prescaler = 0; \
_code; \
} \
diff --git a/sw/tools/gen_messages.ml b/sw/tools/gen_messages.ml
index 14c6e6d4cd..40c3f44b7c 100644
--- a/sw/tools/gen_messages.ml
+++ b/sw/tools/gen_messages.ml
@@ -245,6 +245,8 @@ let _ =
let avr_h = stdout in
+(* Printf.fprintf avr_h "#ifndef _MESSAGES_H\n";
+ Printf.fprintf avr_h "#define _MESSAGES_H\n"; *)
Printf.fprintf avr_h "/* Automatically generated from %s */\n" filename;
Printf.fprintf avr_h "/* Please DO NOT EDIT */\n";
@@ -263,5 +265,7 @@ let _ =
end;
(** Macros for airborne datalink (receiving) *)
- List.iter (Gen_onboard.print_get_macros avr_h) messages
+ List.iter (Gen_onboard.print_get_macros avr_h) messages;
+
+(* Printf.fprintf avr_h "#endif /* _MESSAGES_H*/\n" *)