Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-21 20:04:09 +08:00
Code Issues Actions 25 Packages Projects Releases Wiki Activity

*** empty log message ***

Browse Source
This commit is contained in:
Antoine Drouin
2008-01-30 20:57:46 +00:00
parent fc98bc2dd1
commit 0994e8df5f
25 changed files with 537 additions and 145 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/tl.xml
+54 -4
View File
@@ -56,13 +56,22 @@
<define name="MM_DRDY_PINSEL_BIT" value="28"/>
<define name="MM_DRDY_PINSEL_VAL" value="2"/>
<define name="MM_DRDY_EINT" value="3"/>
<define name="MM_DRDY_VIC_IT" value="VIC_EINT3"/>
</section>
<section name="BARO">
<define name="TL_BARO_W1" value="0xABDC"/>
<define name="TL_BARO_W2" value="0x849A"/>
<define name="TL_BARO_W3" value="0X939E"/>
<define name="TL_BARO_W4" value="0xB259"/>
</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"/>
<define name="NAV_GOTO_H_PGAIN" value="-1000" unit="pprz/m"/>
<define name="NAV_GOTO_H_DGAIN" value="-500" unit="pprz/m/s"/>
</section>
@@ -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
</makefile>
conf/flight_plans/tl.xml
+8 -2
View File
@@ -1,4 +1,4 @@
<flight_plan name="booz test 1" lon0="1.27289" lat0="43.46223" ground_alt="0" alt="3" max_dist_from_home="400" security_height="1">
<flight_plan name="booz test 1" lon0="-118.860691" lat0="34.233845" ground_alt="328" alt="330" max_dist_from_home="100" security_height="1">
<waypoints>
@@ -19,7 +19,13 @@
<set value="1" var="kill_throttle"/>
<stay wp="HOME"/>
</block>
<block name="target">
<block name="on ground">
<exception cond="estimator_in_flight" deroute="flying"/>
<!-- <set value="1" var="kill_throttle"/> -->
<stay wp="TARGET"/>
</block>
<block name="flying">
<!-- <exception cond="!estimator_in_flight" deroute="on ground"/> -->
<call fun="NavSetWaypointHere(WP_TARGET)"/>
<set value="0" var="kill_throttle"/>
<stay wp="TARGET"/>
conf/messages.xml
+38 -2
View File
@@ -541,12 +541,47 @@
<field name="ami_stat" type="uint8"/>
</message>
<message name="TL_ESTIMATOR" ID="246">
<message name="TL_ESTIMATOR" ID="60">
<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="TL_RATE_LOOP_R" ID="61">
<field name="r_est" type="float" unit="rad/s"/>
<field name="r_sp" type="float" unit="rad/s"/>
<field name="r_sum_err" type="float" unit="rad"/>
</message>
<message name="TL_ATTITUDE_LOOP_R" ID="62">
<field name="psi_est" type="float" unit="rad"/>
<field name="psi_sp" type="float" unit="rad"/>
<field name="psi_sum_err" type="float" unit="rad*s"/>
<field name="r_est" type="float" unit="rad/s"/>
</message>
<message name="TL_KALM_PSI_STATE" ID="63">
<field name="psi" type="float" unit="rad"/>
<field name="bias" type="float" unit="rad/s"/>
</message>
<message name="TL_KALM_PSI_COV" ID="64">
<field name="P00" type="float" unit=""/>
<field name="P01" type="float" unit=""/>
<field name="P11" type="float" unit=""/>
</message>
<message name="TL_GYRO_RAW" ID="65">
<field name="val" type="uint16" unit=""/>
</message>
<message name="TL_DEBUG" ID="66">
<field name="body_err_x" type="float" unit=""/>
<field name="body_err_y" type="float" unit=""/>
</message>
<message name="ENOSE_STATUS" ID="250">
<field name="val1" type="uint16"/>
<field name="val2" type="uint16"/>
@@ -591,12 +626,13 @@
</message>
<!--
<message name="DC_MC_STATUS" ID="255">
<field name="time_sec" type="uint16"/>
<field name="time_ticks" type="uint16"/>
<field name="command" type="uint16"/>
</message>
-->
</class>
conf/settings/tl.xml
+11
View File
@@ -7,6 +7,17 @@
<dl_setting MAX="1" MIN="0" STEP="1" VAR="kill_throttle" shortname="kill" module="tl_control"/>
<dl_setting MAX="0" MIN="-1000" STEP="10" VAR="tl_control_rate_r_pgain" shortname="rate_pgain" module="tl_control"/>
<dl_setting MAX="20" MIN="0" STEP="0.1" VAR="tl_control_rate_r_dgain" shortname="rate_dgain" module="tl_control"/>
<dl_setting MAX="10" MIN="0" STEP="1" VAR="tl_control_rate_r_igain" shortname="rate_igain" module="tl_control"/>
<dl_setting MAX="3000" MIN="-3000" STEP="1" VAR="tl_control_trim_r" shortname="trim_r" module="tl_control"/>
<dl_setting MAX="0" MIN="-3000" STEP="1" VAR="tl_control_attitude_psi_pgain" shortname="att_gain" module="tl_control"/>
<dl_setting MAX="0" MIN="-3000" STEP="1" VAR="tl_control_attitude_psi_dgain" shortname="att_dgain" module="tl_control"/>
<dl_setting MAX="0" MIN="-10" STEP="1" VAR="tl_control_attitude_psi_igain" shortname="att_igain" module="tl_control"/>
<dl_setting MAX="0" MIN="-9600" STEP="1" VAR="tl_nav_goto_h_pgain" shortname="nav_h_pgain" module="tl_nav"/>
<dl_setting MAX="0" MIN="-9600" STEP="1" VAR="tl_nav_goto_h_dgain" shortname="nav_h_dgain" module="tl_nav"/>
<dl_setting MAX="0" MIN="1" STEP="1" VAR="telemetry_mode_Ap" shortname="telemetry_mode" module="tl_telemetry"/>
</dl_settings>
</dl_settings>
</settings>
conf/telemetry/tl.xml
+10 -6
View File
@@ -9,17 +9,21 @@
<message name="PPM" period="5"/>
<message name="RC" period="5"/>
<message name="COMMANDS" period="5"/>
<message name="ACTUATORS" period="1"/> <!-- For trimming -->
<message name="IMU_GYRO" period="0.5"/>
<message name="IMU_MAG" period="0.5"/>
<message name="ACTUATORS" period="1"/>
<message name="BAT" period="3"/>
<message name="ESTIMATOR" period="1"/>
<message name="TL_ESTIMATOR" period="0.1"/>
<message name="TL_ESTIMATOR" period="0.5"/>
<message name="NAVIGATION_REF" period="9"/>
<message name="NAVIGATION" period="1"/>
<message name="WP_MOVED" period="0.5"/>
<message name="RATE_LOOP" period="0.5"/>
<!-- <message name="DL_VALUE" period="1.5"/> -->
<message name="LOOP" period="0.5"/>
<message name="TL_DEBUG" period="0.5"/>
</mode>
<mode name="test">
<message name="IMU_GYRO" period="0.5"/>
<message name="IMU_MAG" period="0.5"/>
<message name="TL_KALM_PSI_STATE" period="0.5"/>
<message name="TL_KALM_PSI_COV" period="0.5"/>
</mode>
</process>
sw/airborne/arm7/micromag_hw.c
+12 -10
View File
@@ -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 */
sw/airborne/arm7/micromag_hw.h
+1 -1
View File
@@ -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
sw/airborne/arm7/tl_baro.c
+4 -6
View File
@@ -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) {
sw/airborne/arm7/tl_baro.h
+14 -2
View File
@@ -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);
sw/airborne/arm7/tl_imu.c
+22 -6
View File
@@ -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: {
sw/airborne/arm7/tl_imu.h
+19 -19
View File
@@ -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; \
sw/airborne/datalink.c
+4
View File
@@ -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"
sw/airborne/estimator.h
-6
View File
@@ -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 );
sw/airborne/tl_autopilot.c
+18 -16
View File
@@ -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;
}
}
sw/airborne/tl_autopilot.h
+15
View File
@@ -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 */
sw/airborne/tl_control.c
+94 -27
View File
@@ -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);
}
}
sw/airborne/tl_control.h
+8
View File
@@ -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; \
sw/airborne/tl_estimator.c
+126 -11
View File
@@ -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
}
sw/airborne/tl_estimator.h
+14 -1
View File
@@ -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;}
sw/airborne/tl_main.c
+10 -5
View File
@@ -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);
}
sw/airborne/tl_nav.c
+17 -8
View File
@@ -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) {
sw/airborne/tl_nav.h
+6 -1
View File
@@ -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
sw/airborne/tl_telemetry.h
+26 -10
View File
@@ -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
sw/include/std.h
+1 -1
View File
@@ -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; \
} \
sw/tools/gen_messages.ml
+5 -1
View File
@@ -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" *)