From 12ec3872a84de9bee653df9573f45e1b8ecbce07 Mon Sep 17 00:00:00 2001 From: Christophe De Wagter Date: Mon, 7 Sep 2009 14:54:03 +0000 Subject: [PATCH] AirSpeed --- conf/airframes/holiday50.xml | 10 ++++----- conf/autopilot/tiny.makefile | 5 +++++ conf/messages.xml | 11 ++++++++++ conf/settings/light.xml | 5 ++++- sw/airborne/airspeed.c | 41 ++++++++++++++++++++++++++++++++++++ sw/airborne/airspeed.h | 13 ++++++++++++ sw/airborne/ap_downlink.h | 9 ++++++++ sw/airborne/datalink.c | 2 ++ sw/airborne/estimator.c | 4 ++++ sw/airborne/estimator.h | 3 +++ sw/airborne/fw_v_ctl.c | 39 ++++++++++++++++++++++++++++++++++ sw/airborne/fw_v_ctl.h | 8 +++++++ sw/airborne/light.h | 7 ++++-- sw/airborne/main_ap.c | 12 ++++++++++- 14 files changed, 160 insertions(+), 9 deletions(-) create mode 100644 sw/airborne/airspeed.c create mode 100644 sw/airborne/airspeed.h diff --git a/conf/airframes/holiday50.xml b/conf/airframes/holiday50.xml index 24e6862826..077c7d613f 100644 --- a/conf/airframes/holiday50.xml +++ b/conf/airframes/holiday50.xml @@ -25,7 +25,7 @@ - + @@ -148,7 +148,7 @@ float v_ctl_pitch_of_vz = (v_ctl_climb_setpoint + d_err * v_ctl_auto_throttle_pitch_of_vz_dgain) * v_ctl_auto_throttle_pitch_of_vz_pgain; --> - + @@ -173,7 +173,7 @@ - + @@ -298,8 +298,8 @@ include $(PAPARAZZI_SRC)/conf/autopilot/sitl.makefile sim.CFLAGS += -DBOARD_CONFIG=\"tiny_1_1.h\" -DAGR_CLIMB -DLOITER_TRIM -DALT_KALMAN -DWIND_INFO -DSTRONG_WIND sim.srcs += nav_survey_rectangle.c nav_line.c # traffic_info.c -#sim.CFLAGS += -DUSE_AIRSPEED -#sim.srcs += airspeed.c +sim.CFLAGS += -DUSE_AIRSPEED +sim.srcs += airspeed.c sim.CFLAGS += -DCAM -DMOBILE_CAM -DPOINT_CAM -DPOINT_CAM_PITCH_ROLL # -DTEST_CAM diff --git a/conf/autopilot/tiny.makefile b/conf/autopilot/tiny.makefile index 56446f7737..3c9aae48aa 100644 --- a/conf/autopilot/tiny.makefile +++ b/conf/autopilot/tiny.makefile @@ -7,6 +7,11 @@ ap.ARCH = arm7tdmi ap.TARGET = autopilot ap.TARGETDIR = autopilot +fbw.ARCHDIR = $(ARCHI) +fbw.ARCH = arm7tdmi +fbw.TARGET = autopilot +fbw.TARGETDIR = autopilot + LPC21ISP_BAUD = 38400 LPC21ISP_XTAL = 12000 diff --git a/conf/messages.xml b/conf/messages.xml index 7b4dc139ad..dd9ab48793 100644 --- a/conf/messages.xml +++ b/conf/messages.xml @@ -222,6 +222,11 @@ + + + + + @@ -301,6 +306,12 @@ + + + + + + diff --git a/conf/settings/light.xml b/conf/settings/light.xml index b26a479a38..34be469c0e 100644 --- a/conf/settings/light.xml +++ b/conf/settings/light.xml @@ -3,7 +3,10 @@ - + + + + diff --git a/sw/airborne/airspeed.c b/sw/airborne/airspeed.c new file mode 100644 index 0000000000..bad133b3fb --- /dev/null +++ b/sw/airborne/airspeed.c @@ -0,0 +1,41 @@ +#include "airspeed.h" +#include "adc.h" +#include "airframe.h" +#include "estimator.h" + +#ifdef USE_AIRSPEED +uint16_t adc_airspeed_val; +#else +#error "You compiled the airspeed.c file but did not USE_AIRSPEED, which is needed in other *.c files" +#endif + +#ifdef ADC_CHANNEL_AIRSPEED +#ifndef SITL +static struct adc_buf buf_airspeed; +#endif +#else +#error "You compiled the airspeed.c file but did not ADC_CHANNEL_AIRSPEED, which is needed in other *.c files" +#endif + +void airspeed_init( void ) { +#ifdef ADC_CHANNEL_AIRSPEED +# ifndef ADC_CHANNEL_AIRSPEED_NB_SAMPLES +# error "You defined USE_AIRSPEED but did not assign a ADC_CHANNEL_AIRSPEED_NB_SAMPLES" +# endif +# ifndef SITL + adc_buf_channel(ADC_CHANNEL_AIRSPEED, &buf_airspeed, ADC_CHANNEL_AIRSPEED_NB_SAMPLES); +# endif +#else +# error "You defined USE_AIRSPEED but did not assign a ADC_CHANNEL_AIRSPEED" +#endif +} + +void airspeed_update( void ) { +#ifdef ADC_CHANNEL_AIRSPEED +#ifndef SITL + adc_airspeed_val = (buf_airspeed.sum / buf_airspeed.av_nb_sample) - AIRSPEED_ZERO; + float airspeed = AIRSPEED_SCALE * adc_airspeed_val; + EstimatorSetAirspeed(airspeed); +#endif +#endif +} diff --git a/sw/airborne/airspeed.h b/sw/airborne/airspeed.h new file mode 100644 index 0000000000..2cfa31ae4f --- /dev/null +++ b/sw/airborne/airspeed.h @@ -0,0 +1,13 @@ +#ifndef AIRSPEED_H +#define AIRSPEED_H + +#include + +#ifdef USE_AIRSPEED +extern uint16_t adc_airspeed_val; +#endif + +void airspeed_init( void ); +void airspeed_update( void ); + +#endif /* ADC_GENERIC_H */ diff --git a/sw/airborne/ap_downlink.h b/sw/airborne/ap_downlink.h index ae2a0278d6..63aa588536 100644 --- a/sw/airborne/ap_downlink.h +++ b/sw/airborne/ap_downlink.h @@ -173,6 +173,15 @@ #define PERIODIC_SEND_BARO_MS5534A(_chan) {} #endif +#ifdef USE_AIRSPEED +#define PERIODIC_SEND_AIRSPEED(_chan) DOWNLINK_SEND_AIRSPEED (_chan, &adc_airspeed_val,&estimator_airspeed) +#else +#define PERIODIC_SEND_AIRSPEED(_chan) {} +#endif + +#define PERIODIC_SEND_ENERGY(_chan) Downlink({ int16_t e = energy; DOWNLINK_SEND_ENERGY( &vsup, &curs, &e); }) + + #include "fw_h_ctl_a.h" #define PERIODIC_SEND_H_CTL_A(_chan) DOWNLINK_SEND_H_CTL_A(_chan, &h_ctl_roll_sum_err, &h_ctl_ref_roll_angle, &h_ctl_pitch_sum_err, &h_ctl_ref_pitch_angle) diff --git a/sw/airborne/datalink.c b/sw/airborne/datalink.c index 0ae29fbf2c..4d42579cd8 100644 --- a/sw/airborne/datalink.c +++ b/sw/airborne/datalink.c @@ -114,7 +114,9 @@ void dl_parse_msg(void) { if (msg_id == DL_WIND_INFO && DL_WIND_INFO_ac_id(dl_buffer) == AC_ID) { wind_east = DL_WIND_INFO_east(dl_buffer); wind_north = DL_WIND_INFO_north(dl_buffer); +#ifndef USE_AIRSPEED estimator_airspeed = DL_WIND_INFO_airspeed(dl_buffer); +#endif #ifdef WIND_INFO_RET DOWNLINK_SEND_WIND_INFO_RET(DefaultChannel, &wind_east, &wind_north, &estimator_airspeed); #endif diff --git a/sw/airborne/estimator.c b/sw/airborne/estimator.c index 6099c18305..cf8b7c8537 100644 --- a/sw/airborne/estimator.c +++ b/sw/airborne/estimator.c @@ -95,6 +95,10 @@ void estimator_init( void ) { EstimatorSetRate(0., 0.); +#ifdef USE_AIRSPEED + EstimatorSetAirspeed( 0. ); +#endif + estimator_flight_time = 0; estimator_airspeed = NOMINAL_AIRSPEED; diff --git a/sw/airborne/estimator.h b/sw/airborne/estimator.h index f4bb17e109..eedb1d552f 100644 --- a/sw/airborne/estimator.h +++ b/sw/airborne/estimator.h @@ -110,6 +110,9 @@ extern void alt_kalman( float ); #endif +#ifdef USE_AIRSPEED +#define EstimatorSetAirspeed(airspeed) { estimator_airspeed = airspeed; } +#endif #define EstimatorSetAtt(phi, psi, theta) { estimator_phi = phi; estimator_psi = psi; estimator_theta = theta; } #define EstimatorSetPhiPsi(phi, psi) { estimator_phi = phi; estimator_psi = psi; } diff --git a/sw/airborne/fw_v_ctl.c b/sw/airborne/fw_v_ctl.c index 018dbefb03..05f0f6eb69 100644 --- a/sw/airborne/fw_v_ctl.c +++ b/sw/airborne/fw_v_ctl.c @@ -78,6 +78,19 @@ inline static void v_ctl_climb_auto_throttle_loop( void ); inline static void v_ctl_climb_auto_pitch_loop( void ); #endif +#ifdef USE_AIRSPEED +float v_ctl_auto_airspeed_setpoint; +float v_ctl_auto_airspeed_pitch_pgain; +float v_ctl_auto_airspeed_throttle_pgain; +float v_ctl_auto_airspeed_throttle_igain; + +float v_ctl_auto_airspeed_throttle_sum_err; +#define V_CTL_AUTO_AIRSPEED_THROTTLE_MAX_SUM_ERR 100 + +inline void v_ctl_airspeed_loop( void ); +#endif + + void v_ctl_init( void ) { /* mode */ v_ctl_mode = V_CTL_MODE_MANUAL; @@ -114,6 +127,15 @@ void v_ctl_init( void ) { v_ctl_auto_pitch_sum_err = 0.; #endif +#ifdef USE_AIRSPEED + v_ctl_auto_airspeed_setpoint = V_CTL_AUTO_AIRSPEED_SETPOINT; + v_ctl_auto_airspeed_pitch_pgain = V_CTL_AUTO_AIRSPEED_PITCH_PGAIN; + v_ctl_auto_airspeed_throttle_pgain = V_CTL_AUTO_AIRSPEED_THROTTLE_PGAIN; + v_ctl_auto_airspeed_throttle_igain = V_CTL_AUTO_AIRSPEED_THROTTLE_IGAIN; + + v_ctl_auto_airspeed_throttle_sum_err = 0.; +#endif + v_ctl_throttle_setpoint = 0; } @@ -176,6 +198,18 @@ inline static void v_ctl_climb_auto_throttle_loop(void) { /* pitch pre-command */ float v_ctl_pitch_of_vz = (v_ctl_climb_setpoint + d_err * v_ctl_auto_throttle_pitch_of_vz_dgain) * v_ctl_auto_throttle_pitch_of_vz_pgain; +#ifdef USE_AIRSPEED + float err_airspeed = (v_ctl_auto_airspeed_setpoint - estimator_airspeed); + + v_ctl_auto_airspeed_throttle_sum_err += err_airspeed; + BoundAbs(v_ctl_auto_airspeed_throttle_sum_err, V_CTL_AUTO_AIRSPEED_THROTTLE_MAX_SUM_ERR); + + float v_ctl_auto_airspeed_pitch_of_airspeed = (err_airspeed) * v_ctl_auto_airspeed_pitch_pgain; + float v_ctl_auto_airspeed_throttle_of_airspeed = (err_airspeed + v_ctl_auto_airspeed_throttle_sum_err * v_ctl_auto_airspeed_throttle_igain) * v_ctl_auto_airspeed_throttle_pgain; + + controlled_throttle += v_ctl_auto_airspeed_throttle_of_airspeed; +#endif + #if defined AGR_CLIMB switch (v_ctl_auto_throttle_submode) { case V_CTL_AUTO_THROTTLE_AGRESSIVE: @@ -216,6 +250,11 @@ inline static void v_ctl_climb_auto_throttle_loop(void) { break; } /* switch submode */ #endif + +#ifdef USE_AIRSPEED + nav_pitch += v_ctl_auto_airspeed_pitch_of_airspeed; +#endif + v_ctl_throttle_setpoint = TRIM_UPPRZ(f_throttle * MAX_PPRZ); } diff --git a/sw/airborne/fw_v_ctl.h b/sw/airborne/fw_v_ctl.h index 3be13ce424..231fa48cfb 100644 --- a/sw/airborne/fw_v_ctl.h +++ b/sw/airborne/fw_v_ctl.h @@ -87,6 +87,14 @@ extern void v_ctl_init( void ); extern void v_ctl_altitude_loop( void ); extern void v_ctl_climb_loop ( void ); +#ifdef USE_AIRSPEED +/* "airspeed" inner loop parameters */ +extern float v_ctl_auto_airspeed_setpoint; +extern float v_ctl_auto_airspeed_pitch_pgain; +extern float v_ctl_auto_airspeed_throttle_pgain; +extern float v_ctl_auto_airspeed_throttle_igain; +#endif + /** Computes throttle_slewed from throttle_setpoint */ extern void v_ctl_throttle_slew( void ); diff --git a/sw/airborne/light.h b/sw/airborne/light.h index 510f7caa96..07c7f88406 100644 --- a/sw/airborne/light.h +++ b/sw/airborne/light.h @@ -36,10 +36,13 @@ extern uint8_t light_duration_2; #define LightPeriodicTask2(_1Hz) {} #endif /* LIGHT_LED_1 */ +#ifndef LIGHT_DURATION_INITIAL +#define LIGHT_DURATION_INITIAL 0 +#endif #define LightInit() { \ - light_duration_1 = 0; \ - light_duration_2 = 0; \ + light_duration_1 = LIGHT_DURATION_INITIAL; \ + light_duration_2 = LIGHT_DURATION_INITIAL; \ } diff --git a/sw/airborne/main_ap.c b/sw/airborne/main_ap.c index 3dc9f39460..f996be9611 100644 --- a/sw/airborne/main_ap.c +++ b/sw/airborne/main_ap.c @@ -69,6 +69,10 @@ #include "adc_generic.h" #endif +#ifdef USE_AIRSPEED +#include "airspeed.h" +#endif + #ifdef TELEMETER #include "srf08.h" #endif @@ -362,7 +366,7 @@ static void navigation_task( void ) { SEND_NAVIGATION(DefaultChannel); #endif - SEND_CAM(); + SEND_CAM(DefaultChannel); /* The nav task computes only nav_altitude. However, we are interested by desired_altitude (= nav_alt+alt_shift) in any case. @@ -580,6 +584,9 @@ void periodic_task_ap( void ) { #if defined GYRO gyro_update(); #endif +#if defined USE_AIRSPEED + airspeed_update(); +#endif #ifdef INFRARED ir_update(); estimator_update_state_infrared(); @@ -629,6 +636,9 @@ void init_ap( void ) { #ifdef GYRO gyro_init(); #endif +#ifdef USE_AIRSPEED + airspeed_init(); +#endif #ifdef GPS gps_init(); #endif