diff --git a/conf/firmwares/rotorcraft.makefile b/conf/firmwares/rotorcraft.makefile index f2b4c6229a..b2fcae195d 100644 --- a/conf/firmwares/rotorcraft.makefile +++ b/conf/firmwares/rotorcraft.makefile @@ -166,8 +166,10 @@ ap.CFLAGS += -DUSE_ADC ap.srcs += $(SRC_ARCH)/mcu_periph/adc_arch.c ap.srcs += subsystems/electrical.c # baro has variable offset amplifier on booz board +ifeq ($(BOARD), booz) ap.CFLAGS += -DUSE_DAC ap.srcs += $(SRC_ARCH)/mcu_periph/dac_arch.c +endif else ifeq ($(ARCH), stm32) ap.CFLAGS += -DUSE_ADC ap.CFLAGS += -DUSE_AD1 -DUSE_AD1_1 -DUSE_AD1_2 -DUSE_AD1_3 -DUSE_AD1_4 @@ -206,19 +208,16 @@ ap.CFLAGS += -DUSE_NAVIGATION ap.srcs += $(SRC_FIRMWARE)/guidance/guidance_h.c ap.srcs += $(SRC_FIRMWARE)/guidance/guidance_v.c -ap.srcs += $(SRC_SUBSYSTEMS)/ins.c - # # INS choice # -# include subsystems/rotorcraft/ins_hff.makefile +# include subsystems/rotorcraft/ins.makefile # or -# nothing +# include subsystems/rotorcraft/ins_extended.makefile +# +# extra: +# include subsystems/rotorcraft/ins_hff.makefile # - -# vertical filter float version -ap.srcs += $(SRC_SUBSYSTEMS)/ins/vf_float.c -ap.CFLAGS += -DUSE_VFF -DDT_VFILTER='(1./$(PERIODIC_FREQUENCY).)' ap.srcs += $(SRC_FIRMWARE)/navigation.c ap.srcs += subsystems/navigation/common_flight_plan.c diff --git a/conf/firmwares/subsystems/rotorcraft/ins.makefile b/conf/firmwares/subsystems/rotorcraft/ins.makefile new file mode 100644 index 0000000000..7fb608ffef --- /dev/null +++ b/conf/firmwares/subsystems/rotorcraft/ins.makefile @@ -0,0 +1,10 @@ +# +# simple INS with float vertical filter +# + +ap.srcs += $(SRC_SUBSYSTEMS)/ins.c + +# vertical filter float version +ap.srcs += $(SRC_SUBSYSTEMS)/ins/vf_float.c +ap.CFLAGS += -DUSE_VFF -DDT_VFILTER='(1./$(PERIODIC_FREQUENCY).)' + diff --git a/conf/firmwares/subsystems/rotorcraft/ins_extended.makefile b/conf/firmwares/subsystems/rotorcraft/ins_extended.makefile new file mode 100644 index 0000000000..f1cd021b44 --- /dev/null +++ b/conf/firmwares/subsystems/rotorcraft/ins_extended.makefile @@ -0,0 +1,10 @@ +# +# extended INS with vertical filter using sonar in a better way (flap ground) +# + +ap.srcs += $(SRC_SUBSYSTEMS)/ins_extended.c + +# vertical filter float version +ap.srcs += $(SRC_SUBSYSTEMS)/ins/vf_extended_float.c +ap.CFLAGS += -DUSE_VFF_EXTENDED -DDT_VFILTER='(1./$(PERIODIC_FREQUENCY).)' + diff --git a/conf/messages.xml b/conf/messages.xml index d4785486c4..1abf8e1d07 100644 --- a/conf/messages.xml +++ b/conf/messages.xml @@ -1334,6 +1334,16 @@ + + + + + + + + + + diff --git a/conf/radios/FlyElectricRx31.xml b/conf/radios/FlyElectricRx31.xml new file mode 100644 index 0000000000..aa8edeee20 --- /dev/null +++ b/conf/radios/FlyElectricRx31.xml @@ -0,0 +1,11 @@ + + + + + + + + + + + diff --git a/conf/settings/control/stabilization_att_int.xml b/conf/settings/control/stabilization_att_int.xml index 3a79a16d48..6588b5470a 100644 --- a/conf/settings/control/stabilization_att_int.xml +++ b/conf/settings/control/stabilization_att_int.xml @@ -4,18 +4,21 @@ - + - + - + + - + + + diff --git a/conf/settings/estimation/ins_sonar.xml b/conf/settings/estimation/ins_sonar.xml new file mode 100644 index 0000000000..0c016fa593 --- /dev/null +++ b/conf/settings/estimation/ins_sonar.xml @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/sw/airborne/boards/navgo/baro_board.h b/sw/airborne/boards/navgo/baro_board.h index 250476b148..72065a498a 100644 --- a/sw/airborne/boards/navgo/baro_board.h +++ b/sw/airborne/boards/navgo/baro_board.h @@ -1,5 +1,5 @@ /* - * Copyright (C) 2010 ENAC + * Copyright (C) 2011-2012 Gautier Hattenberger * * This file is part of paparazzi. * @@ -33,12 +33,10 @@ #include "std.h" #include "peripherals/mcp355x.h" -#define BARO_FILTER_GAIN 5 - #define BaroEvent(_b_abs_handler, _b_diff_handler) { \ mcp355x_event(); \ if (mcp355x_data_available) { \ - baro.absolute = (baro.absolute + BARO_FILTER_GAIN*mcp355x_data) / (BARO_FILTER_GAIN+1); \ + baro.absolute = mcp355x_data; \ _b_abs_handler(); \ mcp355x_data_available = FALSE; \ } \ diff --git a/sw/airborne/boards/navgo/imu_navgo.c b/sw/airborne/boards/navgo/imu_navgo.c index e03736c172..2cc655b905 100644 --- a/sw/airborne/boards/navgo/imu_navgo.c +++ b/sw/airborne/boards/navgo/imu_navgo.c @@ -56,6 +56,9 @@ volatile bool_t gyr_valid; volatile bool_t acc_valid; volatile bool_t mag_valid; +#include "filters/median_filter.h" +struct MedianFilter3Int median_gyro, median_accel, median_mag; + void imu_impl_init(void) { ///////////////////////////////////////////////////////////////////// @@ -69,6 +72,11 @@ void imu_impl_init(void) ///////////////////////////////////////////////////////////////////// // HMC58XX hmc58xx_init(); + + // Init median filters + InitMedianFilterRatesInt(median_gyro); + InitMedianFilterVect3Int(median_accel); + InitMedianFilterVect3Int(median_mag); } void imu_periodic( void ) @@ -96,7 +104,6 @@ void imu_navgo_downlink_raw( void ) DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel, DefaultDevice,&imu.mag_unscaled.x,&imu.mag_unscaled.y,&imu.mag_unscaled.z); } - void imu_navgo_event( void ) { @@ -104,6 +111,7 @@ void imu_navgo_event( void ) itg3200_event(); if (itg3200_data_available) { RATES_ASSIGN(imu.gyro_unscaled, -itg3200_data.q, itg3200_data.p, itg3200_data.r); + UpdateMedianFilterRatesInt(median_gyro, imu.gyro_unscaled); itg3200_data_available = FALSE; gyr_valid = TRUE; } @@ -112,6 +120,7 @@ void imu_navgo_event( void ) adxl345_event(); if (adxl345_data_available) { VECT3_ASSIGN(imu.accel_unscaled, adxl345_data.y, -adxl345_data.x, adxl345_data.z); + UpdateMedianFilterVect3Int(median_accel, imu.accel_unscaled); adxl345_data_available = FALSE; acc_valid = TRUE; } @@ -120,6 +129,7 @@ void imu_navgo_event( void ) hmc58xx_event(); if (hmc58xx_data_available) { VECT3_ASSIGN(imu.mag_unscaled, hmc58xx_data.x, hmc58xx_data.y, hmc58xx_data.z); + UpdateMedianFilterVect3Int(median_mag, imu.mag_unscaled); hmc58xx_data_available = FALSE; mag_valid = TRUE; } diff --git a/sw/airborne/boards/navgo_1.0.h b/sw/airborne/boards/navgo_1.0.h index bdade6fc54..c3d4c28d72 100644 --- a/sw/airborne/boards/navgo_1.0.h +++ b/sw/airborne/boards/navgo_1.0.h @@ -54,6 +54,15 @@ /* ADC */ +/* not compatible with PWM0 */ +#define ADC_0 AdcBank1(6) +#ifdef USE_ADC_0 +#ifndef USE_AD1 +#define USE_AD1 +#endif +#define USE_AD1_6 +#endif + /* battery */ /* allow to define ADC_CHANNEL_VSUPPLY in the airframe file*/ #ifndef ADC_CHANNEL_VSUPPLY @@ -79,17 +88,18 @@ #define SPI1_DRDY_EINT 0 #define SPI1_DRDY_VIC_IT VIC_EINT0 -/* PWM0 (internal PWM5) */ -/* P0.21 */ -#define PWM0_PINSEL PINSEL1 -#define PWM0_PINSEL_VAL 0x01 -#define PWM0_PINSEL_BIT 10 - -/* PWM1 (internal PWM2 */ +/* PWM0 (internal PWM2) */ /* P0.7 */ -#define PWM1_PINSEL PINSEL0 -#define PWM1_PINSEL_VAL 0x02 -#define PWM1_PINSEL_BIT 14 +#define PWM0_PINSEL PINSEL0 +#define PWM0_PINSEL_VAL 0x02 +#define PWM0_PINSEL_BIT 14 + +/* PWM1 (internal PWM5) */ +/* not compatible with ADC_0 */ +/* P0.21 */ +#define PWM1_PINSEL PINSEL1 +#define PWM1_PINSEL_VAL 0x01 +#define PWM1_PINSEL_BIT 10 #define BOARD_HAS_BARO 1 diff --git a/sw/airborne/filters/median_filter.h b/sw/airborne/filters/median_filter.h new file mode 100644 index 0000000000..68f5920f1e --- /dev/null +++ b/sw/airborne/filters/median_filter.h @@ -0,0 +1,131 @@ +/* + * Copyright (c) 2012 Ted Carancho. (AeroQuad) + * (c) 2012 Gautier Hattenberger + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + * + */ + +#ifndef MEDIAN_H +#define MEDIAN_H + +#define MEDIAN_DATASIZE 5 + +#include "std.h" +#include "math/pprz_algebra_int.h" + +struct MedianFilterInt { + int32_t data[MEDIAN_DATASIZE], sortData[MEDIAN_DATASIZE]; + int8_t dataIndex; +}; + +inline void init_median_filter(struct MedianFilterInt * filter); +inline int32_t update_median_filter(struct MedianFilterInt * filter, int32_t new_data); +inline int32_t get_median_filter(struct MedianFilterInt * filter); + +inline void init_median_filter(struct MedianFilterInt * filter) { + int i; + for (i = 0; i < MEDIAN_DATASIZE; i++) { + filter->data[i] = 0; + filter->sortData[i] = 0; + } + filter->dataIndex = 0; +} + +inline int32_t update_median_filter(struct MedianFilterInt * filter, int32_t new_data) { + int temp, i, j; // used to sort array + + // Insert new data into raw data array round robin style + filter->data[filter->dataIndex] = new_data; + if (filter->dataIndex < (MEDIAN_DATASIZE-1)) { + filter->dataIndex++; + } + else { + filter->dataIndex = 0; + } + + // Copy raw data to sort data array + memcpy(filter->sortData, filter->data, sizeof(filter->data)); + + // Insertion Sort + for(i = 1; i <= (MEDIAN_DATASIZE-1); i++) { + temp = filter->sortData[i]; + j = i-1; + while(temp < filter->sortData[j] && j>=0) { + filter->sortData[j+1] = filter->sortData[j]; + j = j-1; + } + filter->sortData[j+1] = temp; + } + return filter->sortData[(MEDIAN_DATASIZE)>>1]; // return data value in middle of sorted array +} + +inline int32_t get_median_filter(struct MedianFilterInt * filter) { + return filter->sortData[(MEDIAN_DATASIZE)>>1]; +} + +struct MedianFilter3Int { + struct MedianFilterInt mf[3]; +}; + +#define InitMedianFilterVect3Int(_f) { \ + for (int i = 0; i < 3; i++) { \ + init_median_filter(&(_f.mf[i])); \ + } \ +} + +#define InitMedianFilterEulerInt(_f) InitMedianFilterVect3Int(_f) +#define InitMedianFilterRatesInt(_f) InitMedianFilterVect3Int(_f) + +#define UpdateMedianFilterVect3Int(_f, _v) { \ + (_v).x = update_median_filter(&(_f.mf[0]), (_v).x); \ + (_v).y = update_median_filter(&(_f.mf[1]), (_v).y); \ + (_v).z = update_median_filter(&(_f.mf[2]), (_v).z); \ +} + +#define UpdateMedianFilterEulerInt(_f, _v) { \ + (_v).phi = update_median_filter(&(_f.mf[0]), (_v).phi); \ + (_v).theta = update_median_filter(&(_f.mf[1]), (_v).theta); \ + (_v).psi = update_median_filter(&(_f.mf[2]), (_v).psi); \ +} + +#define UpdateMedianFilterRatesInt(_f, _v) { \ + (_v).p = update_median_filter(&(_f.mf[0]), (_v).p); \ + (_v).q = update_median_filter(&(_f.mf[1]), (_v).q); \ + (_v).r = update_median_filter(&(_f.mf[2]), (_v).r); \ +} + +#define GetMedianFilterVect3Int(_f, _v) { \ + (_v).x = get_median_filter(&(_f.mf[0])); \ + (_v).y = get_median_filter(&(_f.mf[1])); \ + (_v).z = get_median_filter(&(_f.mf[2])); \ +} + +#define GetMedianFilterEulerInt(_f, _v) { \ + (_v).phi = get_median_filter(&(_f.mf[0])); \ + (_v).theta = get_median_filter(&(_f.mf[1])); \ + (_v).psi = get_median_filter(&(_f.mf[2])); \ +} + +#define GetMedianFilterRatesInt(_f, _v) { \ + (_v).p = get_median_filter(&(_f.mf[0])); \ + (_v).q = get_median_filter(&(_f.mf[1])); \ + (_v).r = get_median_filter(&(_f.mf[2])); \ +} + +#endif diff --git a/sw/airborne/firmwares/rotorcraft/autopilot.c b/sw/airborne/firmwares/rotorcraft/autopilot.c index 978121cdc7..478683ba41 100644 --- a/sw/airborne/firmwares/rotorcraft/autopilot.c +++ b/sw/airborne/firmwares/rotorcraft/autopilot.c @@ -22,6 +22,7 @@ #include "firmwares/rotorcraft/autopilot.h" #include "subsystems/radio_control.h" +#include "subsystems/gps.h" #include "firmwares/rotorcraft/commands.h" #include "firmwares/rotorcraft/navigation.h" #include "firmwares/rotorcraft/guidance.h" @@ -248,7 +249,9 @@ void autopilot_on_rc_frame(void) { else { uint8_t new_autopilot_mode = 0; AP_MODE_OF_PPRZ(radio_control.values[RADIO_MODE], new_autopilot_mode); - autopilot_set_mode(new_autopilot_mode); + /* don't enter NAV mode if GPS is lost (this also prevents mode oscillations) */ + if (!(new_autopilot_mode == AP_MODE_NAV && GpsIsLost())) + autopilot_set_mode(new_autopilot_mode); } /* if not in FAILSAFE mode check motor and in_flight status, read RC */ diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_adpt.h b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_adpt.h index 879c6cc50e..625f588d01 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_adpt.h +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_adpt.h @@ -84,7 +84,7 @@ int32_t gv_adapt_Xmeas; /* Initial State and Covariance */ -#define GV_ADAPT_X0_F 0.0015 +#define GV_ADAPT_X0_F 0.003 #define GV_ADAPT_X0 BFP_OF_REAL(GV_ADAPT_X0_F, GV_ADAPT_X_FRAC) #define GV_ADAPT_P0_F 0.1 #define GV_ADAPT_P0 BFP_OF_REAL(GV_ADAPT_P0_F, GV_ADAPT_P_FRAC) diff --git a/sw/airborne/firmwares/rotorcraft/main.c b/sw/airborne/firmwares/rotorcraft/main.c index ed193506ff..7376bd7462 100644 --- a/sw/airborne/firmwares/rotorcraft/main.c +++ b/sw/airborne/firmwares/rotorcraft/main.c @@ -185,8 +185,10 @@ STATIC_INLINE void failsafe_check( void ) { } #if USE_GPS - if (radio_control.status != RC_OK && - autopilot_mode == AP_MODE_NAV && + if (autopilot_mode == AP_MODE_NAV && +#if NO_GPS_LOST_WITH_RC_VALID + radio_control.status != RC_OK && +#endif GpsIsLost()) { autopilot_set_mode(AP_MODE_FAILSAFE); diff --git a/sw/airborne/firmwares/rotorcraft/navigation.c b/sw/airborne/firmwares/rotorcraft/navigation.c index 52535bda4b..aa6227c4e9 100644 --- a/sw/airborne/firmwares/rotorcraft/navigation.c +++ b/sw/airborne/firmwares/rotorcraft/navigation.c @@ -53,6 +53,9 @@ uint8_t nav_segment_start, nav_segment_end; uint8_t nav_circle_centre; int32_t nav_circle_radius, nav_circle_qdr, nav_circle_radians; +int32_t nav_leg_progress; +int32_t nav_leg_length; + int32_t nav_roll, nav_pitch; int32_t nav_heading, nav_course; float nav_radius; @@ -99,6 +102,8 @@ void nav_init(void) { nav_radius = DEFAULT_CIRCLE_RADIUS; nav_throttle = 0; nav_climb = 0; + nav_leg_progress = 0; + nav_leg_length = 1; } @@ -186,17 +191,16 @@ void nav_route(uint8_t wp_start, uint8_t wp_end) { // go back to metric precision or values are too large INT32_VECT2_RSHIFT(wp_diff,wp_diff,INT32_POS_FRAC); INT32_VECT2_RSHIFT(pos_diff,pos_diff,INT32_POS_FRAC); - int32_t leg_length; int32_t leg_length2 = Max((wp_diff.x * wp_diff.x + wp_diff.y * wp_diff.y),1); - INT32_SQRT(leg_length,leg_length2); - int32_t nav_leg_progress = (pos_diff.x * wp_diff.x + pos_diff.y * wp_diff.y) / leg_length; + INT32_SQRT(nav_leg_length,leg_length2); + nav_leg_progress = (pos_diff.x * wp_diff.x + pos_diff.y * wp_diff.y) / nav_leg_length; int32_t progress = Max((CARROT_DIST >> INT32_POS_FRAC), 0); nav_leg_progress += progress; - int32_t prog_2 = leg_length;// + progress / 2; + int32_t prog_2 = nav_leg_length;// + progress / 2; Bound(nav_leg_progress, 0, prog_2); struct Int32Vect2 progress_pos; VECT2_SMUL(progress_pos, wp_diff, nav_leg_progress); - VECT2_SDIV(progress_pos, progress_pos, leg_length); + VECT2_SDIV(progress_pos, progress_pos, nav_leg_length); INT32_VECT2_LSHIFT(progress_pos,progress_pos,INT32_POS_FRAC); VECT2_SUM(navigation_target,waypoints[wp_start],progress_pos); //printf("target %d %d | p %d %d | s %d %d | l %d %d %d\n", @@ -244,6 +248,7 @@ static inline void nav_set_altitude( void ) { } } + /** Reset the geographic reference to the current GPS fix */ unit_t nav_reset_reference( void ) { ins_ltp_initialised = FALSE; diff --git a/sw/airborne/firmwares/rotorcraft/navigation.h b/sw/airborne/firmwares/rotorcraft/navigation.h index a1a68e391e..d71cff7586 100644 --- a/sw/airborne/firmwares/rotorcraft/navigation.h +++ b/sw/airborne/firmwares/rotorcraft/navigation.h @@ -58,6 +58,9 @@ extern int32_t nav_roll, nav_pitch; ///< with #INT32_ANGLE_FRAC extern int32_t nav_heading, nav_course; ///< with #INT32_ANGLE_FRAC extern float nav_radius; +extern int32_t nav_leg_progress; +extern int32_t nav_leg_length; + extern uint8_t vertical_mode; extern uint32_t nav_throttle; ///< direct throttle from 0:MAX_PPRZ, used in VERTICAL_MODE_MANUAL extern int32_t nav_climb, nav_altitude, nav_flight_altitude; @@ -124,12 +127,18 @@ extern void nav_route(uint8_t wp_start, uint8_t wp_end); nav_route(_start, _end); \ } +/** Nav glide routine */ +#define NavGlide(_last_wp, _wp) { \ + int32_t start_alt = waypoints[_last_wp].z; \ + int32_t diff_alt = waypoints[_wp].z - start_alt; \ + int32_t alt = start_alt + ((diff_alt * nav_leg_progress) / nav_leg_length); \ + NavVerticalAltitudeMode(POS_FLOAT_OF_BFP(alt),0); \ +} + bool_t nav_approaching_from(uint8_t wp_idx, uint8_t from_idx); #define NavApproaching(wp, time) nav_approaching_from(wp, 0) #define NavApproachingFrom(wp, from, time) nav_approaching_from(wp, from) -#define NavGlide(_last_wp, _wp) {} - /** Set the climb control to auto-throttle with the specified pitch pre-command */ #define NavVerticalAutoThrottleMode(_pitch) { \ diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c index a0d42477d3..bc845d87ab 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c @@ -52,6 +52,11 @@ void stabilization_attitude_init(void) { stabilization_attitude_ref_init(); + VECT3_ASSIGN(stabilization_gains.a, + STABILIZATION_ATTITUDE_PHI_AGAIN, + STABILIZATION_ATTITUDE_THETA_AGAIN, + STABILIZATION_ATTITUDE_PSI_AGAIN); + VECT3_ASSIGN(stabilization_gains.p, STABILIZATION_ATTITUDE_PHI_PGAIN, STABILIZATION_ATTITUDE_THETA_PGAIN, @@ -107,11 +112,14 @@ void stabilization_attitude_run(bool_t in_flight) { /* compute feedforward command */ stabilization_att_ff_cmd[COMMAND_ROLL] = - OFFSET_AND_ROUND(stabilization_gains.dd.x * stab_att_ref_accel.p, 5); + OFFSET_AND_ROUND(stabilization_gains.dd.x * stab_att_ref_accel.p, 5) + + OFFSET_AND_ROUND(stabilization_gains.a.x * stab_att_ref_euler.phi, 6); stabilization_att_ff_cmd[COMMAND_PITCH] = - OFFSET_AND_ROUND(stabilization_gains.dd.y * stab_att_ref_accel.q, 5); + OFFSET_AND_ROUND(stabilization_gains.dd.y * stab_att_ref_accel.q, 5) + + OFFSET_AND_ROUND(stabilization_gains.a.y * stab_att_ref_euler.theta, 6); stabilization_att_ff_cmd[COMMAND_YAW] = - OFFSET_AND_ROUND(stabilization_gains.dd.z * stab_att_ref_accel.r, 5); + OFFSET_AND_ROUND(stabilization_gains.dd.z * stab_att_ref_accel.r, 5) + + OFFSET_AND_ROUND(stabilization_gains.a.z * stab_att_ref_euler.psi, 6); /* compute feedback command */ /* attitude error */ diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_int.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_int.h index 02b761183a..2051caec5d 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_int.h +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_int.h @@ -29,6 +29,7 @@ #include "generated/airframe.h" struct Int32AttitudeGains { + struct Int32Vect3 a; struct Int32Vect3 p; struct Int32Vect3 d; struct Int32Vect3 dd; diff --git a/sw/airborne/firmwares/rotorcraft/telemetry.h b/sw/airborne/firmwares/rotorcraft/telemetry.h index ace0a8d03d..25511638e4 100644 --- a/sw/airborne/firmwares/rotorcraft/telemetry.h +++ b/sw/airborne/firmwares/rotorcraft/telemetry.h @@ -495,6 +495,21 @@ #define PERIODIC_SEND_VFF(_trans, _dev) {} #endif +#if USE_VFF_EXTENDED +#include "subsystems/ins/vf_extended_float.h" +#define PERIODIC_SEND_VFF_EXTENDED(_trans, _dev) { \ + DOWNLINK_SEND_VFF_EXTENDED(_trans, _dev, \ + &vff_z_meas, \ + &vff_z_meas_baro, \ + &vff_z, \ + &vff_zdot, \ + &vff_bias, \ + &vff_offset); \ + } +#else +#define PERIODIC_SEND_VFF_EXTENDED(_trans, _dev) {} +#endif + #if USE_HFF #include "subsystems/ins/hf_float.h" #define PERIODIC_SEND_HFF(_trans, _dev) { \ diff --git a/sw/airborne/modules/cam_control/booz_cam.c b/sw/airborne/modules/cam_control/booz_cam.c index 2082a831d3..14904ca88d 100644 --- a/sw/airborne/modules/cam_control/booz_cam.c +++ b/sw/airborne/modules/cam_control/booz_cam.c @@ -33,9 +33,9 @@ uint8_t booz_cam_mode; // Tilt definition -#ifdef BOOZ_CAM_TILT_NEUTRAL -int16_t booz_cam_tilt_pwm; int16_t booz_cam_tilt; +int16_t booz_cam_tilt_pwm; +#ifdef BOOZ_CAM_TILT_NEUTRAL #ifndef BOOZ_CAM_TILT_MIN #define BOOZ_CAM_TILT_MIN BOOZ_CAM_TILT_NEUTRAL #endif @@ -46,8 +46,8 @@ int16_t booz_cam_tilt; #endif // Pan definition -#ifdef BOOZ_CAM_PAN_NEUTRAL int16_t booz_cam_pan; +#ifdef BOOZ_CAM_PAN_NEUTRAL #ifndef BOOZ_CAM_PAN_MIN #define BOOZ_CAM_PAN_MIN BOOZ_CAM_PAN_NEUTRAL #endif @@ -78,9 +78,14 @@ void booz_cam_init(void) { booz_cam_tilt_pwm = BOOZ_CAM_TILT_NEUTRAL; BOOZ_CAM_SetPwm(booz_cam_tilt_pwm); booz_cam_tilt = 0; +#else + booz_cam_tilt_pwm = 1500; + booz_cam_tilt = 0; #endif #ifdef BOOZ_CAM_USE_PAN booz_cam_pan = BOOZ_CAM_PAN_NEUTRAL; +#else + booz_cam_pan = 0; #endif } diff --git a/sw/airborne/modules/cam_control/booz_cam.h b/sw/airborne/modules/cam_control/booz_cam.h index 1afa847cb0..2a1fbfb0d8 100644 --- a/sw/airborne/modules/cam_control/booz_cam.h +++ b/sw/airborne/modules/cam_control/booz_cam.h @@ -47,13 +47,9 @@ extern uint8_t booz_cam_mode; -#ifdef BOOZ_CAM_TILT_NEUTRAL -extern int16_t booz_cam_tilt_pwm; extern int16_t booz_cam_tilt; -#endif -#ifdef BOOZ_CAM_PAN_NEUTRAL extern int16_t booz_cam_pan; -#endif +extern int16_t booz_cam_tilt_pwm; extern void booz_cam_init(void); extern void booz_cam_periodic(void); diff --git a/sw/airborne/subsystems/gps.h b/sw/airborne/subsystems/gps.h index 46e445d128..3f722936fc 100644 --- a/sw/airborne/subsystems/gps.h +++ b/sw/airborne/subsystems/gps.h @@ -107,7 +107,17 @@ extern void gps_impl_init(void); #ifndef GPS_TIMEOUT #define GPS_TIMEOUT 5 #endif -#define GpsIsLost() (sys_time.nb_sec - gps.last_fix_time > GPS_TIMEOUT) + +#include "mcu_periph/sys_time.h" +inline bool_t GpsIsLost(void); + +inline bool_t GpsIsLost(void) { + if (sys_time.nb_sec - gps.last_fix_time > GPS_TIMEOUT) { + gps.fix = GPS_FIX_NONE; + return TRUE; + } + return FALSE; +} //TODO diff --git a/sw/airborne/subsystems/ins.h b/sw/airborne/subsystems/ins.h index 83bb470096..a3f876031d 100644 --- a/sw/airborne/subsystems/ins.h +++ b/sw/airborne/subsystems/ins.h @@ -33,7 +33,7 @@ extern struct NedCoor_i ins_gps_pos_cm_ned; extern struct NedCoor_i ins_gps_speed_cm_s_ned; /* barometer */ -#if USE_VFF +#if USE_VFF || USE_VFF_EXTENDED extern int32_t ins_baro_alt; extern int32_t ins_qfe; extern bool_t ins_baro_initialised; diff --git a/sw/airborne/subsystems/ins/vf_extended_float.c b/sw/airborne/subsystems/ins/vf_extended_float.c new file mode 100644 index 0000000000..08f53a96ff --- /dev/null +++ b/sw/airborne/subsystems/ins/vf_extended_float.c @@ -0,0 +1,314 @@ +/* + * Copyright (C) 2008-2009 Antoine Drouin + * Copyright (C) 2012 Gautier Hattenberger + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +#include "subsystems/ins/vf_extended_float.h" + +#define DEBUG_VFF_EXTENDED 1 + +#if DEBUG_VFF_EXTENDED +#include "mcu_periph/uart.h" +#include "messages.h" +#include "subsystems/datalink/downlink.h" +#endif + +/* + +X = [ z zdot accel_bias baro_offset ] + +temps : + propagate 86us + update 46us + +*/ +/* initial covariance diagonal */ +#define INIT_PXX 1. +/* process noise */ +#define ACCEL_NOISE 0.5 +#define Qzz ACCEL_NOISE/512./512./2. +#define Qzdotzdot ACCEL_NOISE/512. +#define Qbiasbias 1e-7 +#define Qoffoff 1e-4 +#define R_BARO 1. +#define R_ALT 0.1 +#define R_OFFSET 1. + +float vff_z; +float vff_zdot; +float vff_bias; +float vff_offset; +float vff_zdotdot; + +float vff_P[VFF_STATE_SIZE][VFF_STATE_SIZE]; + +float vff_z_meas; +float vff_z_meas_baro; + +void vff_init(float init_z, float init_zdot, float init_accel_bias, float init_baro_offset) { + vff_z = init_z; + vff_zdot = init_zdot; + vff_bias = init_accel_bias; + vff_offset = init_baro_offset; + int i, j; + for (i=0; i + * Copyright (C) 2012 Gautier Hattenberger + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +#ifndef VF_EXTENDED_FLOAT_H +#define VF_EXTENDED_FLOAT_H + +#define VFF_STATE_SIZE 4 + +extern float vff_z; +extern float vff_zdot; +extern float vff_bias; +extern float vff_offset; +extern float vff_P[VFF_STATE_SIZE][VFF_STATE_SIZE]; +extern float vff_zdotdot; + +extern float vff_z_meas; +extern float vff_z_meas_baro; + +extern void vff_init(float z, float zdot, float accel_bias, float baro_offset); +extern void vff_propagate(float accel); +extern void vff_update_baro(float z_meas); +extern void vff_update_alt(float z_meas); +extern void vff_update_offset(float offset); +extern void vff_update_baro_conf(float z_meas, float conf); +extern void vff_update_alt_conf(float z_meas, float conf); +//extern void vff_update_vz_conf(float vz_meas, float conf); +extern void vff_realign(float z_meas); + +#endif /* VF_EXTENDED_FLOAT_H */ diff --git a/sw/airborne/subsystems/ins_extended.c b/sw/airborne/subsystems/ins_extended.c new file mode 100644 index 0000000000..f1d6e3976f --- /dev/null +++ b/sw/airborne/subsystems/ins_extended.c @@ -0,0 +1,321 @@ +/* + * Copyright (C) 2008-2010 The Paparazzi Team + * Copyright (C) 2012 Gautier Hattenberger + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +#include "subsystems/ins.h" + +#include "subsystems/imu.h" +#include "subsystems/sensors/baro.h" +#include "subsystems/gps.h" + +#include "generated/airframe.h" +#include "math/pprz_algebra_int.h" +#include "math/pprz_algebra_float.h" + +#include "subsystems/ahrs.h" + +#if USE_VFF_EXTENDED +#include "subsystems/ins/vf_extended_float.h" +#endif + +#if USE_HFF +#include "subsystems/ins/hf_float.h" +#endif + +#ifdef SITL +#include "nps_fdm.h" +#include +#endif + +#ifdef INS_SONAR_THROTTLE_THRESHOLD +#include "firmwares/rotorcraft/stabilization.h" +#endif + +#include "math/pprz_geodetic_int.h" + +#include "generated/flight_plan.h" + +/* gps transformed to LTP-NED */ +struct LtpDef_i ins_ltp_def; + bool_t ins_ltp_initialised; +struct NedCoor_i ins_gps_pos_cm_ned; +struct NedCoor_i ins_gps_speed_cm_s_ned; +#if USE_HFF +/* horizontal gps transformed to NED in meters as float */ +struct FloatVect2 ins_gps_pos_m_ned; +struct FloatVect2 ins_gps_speed_m_s_ned; +#endif +bool_t ins_hf_realign; + +/* barometer */ +#if USE_VFF_EXTENDED +int32_t ins_qfe; +bool_t ins_baro_initialised; +int32_t ins_baro_alt; +#include "filters/median_filter.h" +struct MedianFilterInt baro_median; +#if USE_SONAR +bool_t ins_update_on_agl; +int32_t ins_sonar_offset; +struct MedianFilterInt sonar_median; +#ifndef INS_SONAR_OFFSET +#define INS_SONAR_OFFSET 0 +#endif +#define VFF_R_SONAR_0 0.1 +#define VFF_R_SONAR_OF_M 0.2 +#endif +#endif +bool_t ins_vf_realign; + +/* output */ +struct NedCoor_i ins_ltp_pos; +struct NedCoor_i ins_ltp_speed; +struct NedCoor_i ins_ltp_accel; +struct EnuCoor_i ins_enu_pos; +struct EnuCoor_i ins_enu_speed; +struct EnuCoor_i ins_enu_accel; + + +void ins_init() { +#if USE_INS_NAV_INIT + ins_ltp_initialised = TRUE; + + /** FIXME: should use the same code than MOVE_WP in firmwares/rotorcraft/datalink.c */ + struct LlaCoor_i llh_nav0; /* Height above the ellipsoid */ + llh_nav0.lat = INT32_RAD_OF_DEG(NAV_LAT0); + llh_nav0.lon = INT32_RAD_OF_DEG(NAV_LON0); + /* NAV_ALT0 = ground alt above msl, NAV_MSL0 = geoid-height (msl) over ellipsoid */ + llh_nav0.alt = NAV_ALT0 + NAV_MSL0; + + struct EcefCoor_i ecef_nav0; + ecef_of_lla_i(&ecef_nav0, &llh_nav0); + + ltp_def_from_ecef_i(&ins_ltp_def, &ecef_nav0); + ins_ltp_def.hmsl = NAV_ALT0; +#else + ins_ltp_initialised = FALSE; +#endif +#if USE_VFF_EXTENDED + ins_baro_initialised = FALSE; + init_median_filter(&baro_median); +#if USE_SONAR + ins_update_on_agl = FALSE; + init_median_filter(&sonar_median); + ins_sonar_offset = INS_SONAR_OFFSET; +#endif + vff_init(0., 0., 0., 0.); +#endif + ins_vf_realign = FALSE; + ins_hf_realign = FALSE; +#if USE_HFF + b2_hff_init(0., 0., 0., 0.); +#endif + INT32_VECT3_ZERO(ins_ltp_pos); + INT32_VECT3_ZERO(ins_ltp_speed); + INT32_VECT3_ZERO(ins_ltp_accel); + INT32_VECT3_ZERO(ins_enu_pos); + INT32_VECT3_ZERO(ins_enu_speed); + INT32_VECT3_ZERO(ins_enu_accel); +} + +void ins_periodic( void ) { +} + +#if USE_HFF +void ins_realign_h(struct FloatVect2 pos, struct FloatVect2 speed) { + b2_hff_realign(pos, speed); +} +#else +void ins_realign_h(struct FloatVect2 pos __attribute__ ((unused)), struct FloatVect2 speed __attribute__ ((unused))) {} +#endif /* USE_HFF */ + + +void ins_realign_v(float z) { +#if USE_VFF_EXTENDED + vff_realign(z); +#endif +} + +void ins_propagate() { + /* untilt accels */ + struct Int32Vect3 accel_meas_body; + INT32_RMAT_TRANSP_VMULT(accel_meas_body, imu.body_to_imu_rmat, imu.accel); + struct Int32Vect3 accel_meas_ltp; + INT32_RMAT_TRANSP_VMULT(accel_meas_ltp, ahrs.ltp_to_body_rmat, accel_meas_body); + +#if USE_VFF_EXTENDED + float z_accel_meas_float = ACCEL_FLOAT_OF_BFP(accel_meas_ltp.z); + if (baro.status == BS_RUNNING && ins_baro_initialised) { + vff_propagate(z_accel_meas_float); + ins_ltp_accel.z = ACCEL_BFP_OF_REAL(vff_zdotdot); + ins_ltp_speed.z = SPEED_BFP_OF_REAL(vff_zdot); + ins_ltp_pos.z = POS_BFP_OF_REAL(vff_z); + } + else { // feed accel from the sensors + // subtract -9.81m/s2 (acceleration measured due to gravity, but vehivle not accelerating in ltp) + ins_ltp_accel.z = accel_meas_ltp.z + ACCEL_BFP_OF_REAL(9.81); + } +#if !USE_SONAR + vff_update_offset(0.); +#endif +#else + ins_ltp_accel.z = accel_meas_ltp.z + ACCEL_BFP_OF_REAL(9.81); +#endif /* USE_VFF_EXTENDED */ + +#if USE_HFF + /* propagate horizontal filter */ + b2_hff_propagate(); +#else + ins_ltp_accel.x = accel_meas_ltp.x; + ins_ltp_accel.y = accel_meas_ltp.y; +#endif /* USE_HFF */ + + INT32_VECT3_ENU_OF_NED(ins_enu_pos, ins_ltp_pos); + INT32_VECT3_ENU_OF_NED(ins_enu_speed, ins_ltp_speed); + INT32_VECT3_ENU_OF_NED(ins_enu_accel, ins_ltp_accel); +} + +void ins_update_baro() { +#if USE_VFF_EXTENDED + int32_t baro_pressure = update_median_filter(&baro_median, baro.absolute); + if (baro.status == BS_RUNNING) { + if (!ins_baro_initialised) { + ins_qfe = baro_pressure; + ins_baro_initialised = TRUE; + } + if (ins_vf_realign) { + ins_vf_realign = FALSE; + ins_qfe = baro_pressure; +#if USE_SONAR + // FIXME should use an averaged value + //ins_sonar_offset = sonar_meas; +#endif + vff_realign(0.); + ins_ltp_accel.z = ACCEL_BFP_OF_REAL(vff_zdotdot); + ins_ltp_speed.z = SPEED_BFP_OF_REAL(vff_zdot); + ins_ltp_pos.z = POS_BFP_OF_REAL(vff_z); + ins_enu_pos.z = -ins_ltp_pos.z; + ins_enu_speed.z = -ins_ltp_speed.z; + ins_enu_accel.z = -ins_ltp_accel.z; + } + else { /* not realigning, so normal update with baro measurement */ + ins_baro_alt = ((baro_pressure - ins_qfe) * INS_BARO_SENS_NUM)/INS_BARO_SENS_DEN; + float alt_float = POS_FLOAT_OF_BFP(ins_baro_alt); + vff_update_baro(alt_float); + } + } +#endif +} + + +void ins_update_gps(void) { +#if USE_GPS + if (gps.fix == GPS_FIX_3D) { + if (!ins_ltp_initialised) { + ltp_def_from_ecef_i(&ins_ltp_def, &gps.ecef_pos); + ins_ltp_def.lla.alt = gps.lla_pos.alt; + ins_ltp_def.hmsl = gps.hmsl; + ins_ltp_initialised = TRUE; + } + ned_of_ecef_point_i(&ins_gps_pos_cm_ned, &ins_ltp_def, &gps.ecef_pos); + ned_of_ecef_vect_i(&ins_gps_speed_cm_s_ned, &ins_ltp_def, &gps.ecef_vel); +#if USE_HFF + VECT2_ASSIGN(ins_gps_pos_m_ned, ins_gps_pos_cm_ned.x, ins_gps_pos_cm_ned.y); + VECT2_SDIV(ins_gps_pos_m_ned, ins_gps_pos_m_ned, 100.); + VECT2_ASSIGN(ins_gps_speed_m_s_ned, ins_gps_speed_cm_s_ned.x, ins_gps_speed_cm_s_ned.y); + VECT2_SDIV(ins_gps_speed_m_s_ned, ins_gps_speed_m_s_ned, 100.); + if (ins_hf_realign) { + ins_hf_realign = FALSE; +#ifdef SITL + struct FloatVect2 true_pos, true_speed; + VECT2_COPY(true_pos, fdm.ltpprz_pos); + VECT2_COPY(true_speed, fdm.ltpprz_ecef_vel); + b2_hff_realign(true_pos, true_speed); +#else + const struct FloatVect2 zero = {0.0, 0.0}; + b2_hff_realign(ins_gps_pos_m_ned, zero); +#endif + } + b2_hff_update_gps(); +#if !USE_VFF_EXTENDED /* vff not used */ + ins_ltp_pos.z = (ins_gps_pos_cm_ned.z * INT32_POS_OF_CM_NUM) / INT32_POS_OF_CM_DEN; + ins_ltp_speed.z = (ins_gps_speed_cm_s_ned.z * INT32_SPEED_OF_CM_S_NUM) INT32_SPEED_OF_CM_S_DEN; +#endif /* vff not used */ +#endif /* hff used */ + + +#if !USE_HFF /* hff not used */ +#if !USE_VFF_EXTENDED /* neither hf nor vf used */ + INT32_VECT3_SCALE_3(ins_ltp_pos, ins_gps_pos_cm_ned, INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN); + INT32_VECT3_SCALE_3(ins_ltp_speed, ins_gps_speed_cm_s_ned, INT32_SPEED_OF_CM_S_NUM, INT32_SPEED_OF_CM_S_DEN); +#else /* only vff used */ + INT32_VECT2_SCALE_2(ins_ltp_pos, ins_gps_pos_cm_ned, INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN); + INT32_VECT2_SCALE_2(ins_ltp_speed, ins_gps_speed_cm_s_ned, INT32_SPEED_OF_CM_S_NUM, INT32_SPEED_OF_CM_S_DEN); +#endif + +#if USE_GPS_LAG_HACK + VECT2_COPY(d_pos, ins_ltp_speed); + INT32_VECT2_RSHIFT(d_pos, d_pos, 11); + VECT2_ADD(ins_ltp_pos, d_pos); +#endif +#endif /* hff not used */ + + INT32_VECT3_ENU_OF_NED(ins_enu_pos, ins_ltp_pos); + INT32_VECT3_ENU_OF_NED(ins_enu_speed, ins_ltp_speed); + INT32_VECT3_ENU_OF_NED(ins_enu_accel, ins_ltp_accel); + } +#endif /* USE_GPS */ +} + +void ins_update_sonar() { +#if USE_SONAR && USE_VFF_EXTENDED + static float last_offset = 0.; + //static int32_t sonar_filtered = 0; + int32_t sonar_filtered = update_median_filter(&sonar_median, sonar_meas); + //sonar_filtered = (sm + 2*sonar_filtered) / 3; + float sonar = (sonar_filtered - ins_sonar_offset) * INS_SONAR_SENS; + /* update filter assuming a flat ground */ + if (sonar < INS_SONAR_MAX_RANGE +#ifdef INS_SONAR_THROTTLE_THRESHOLD + && stabilization_cmd[COMMAND_THRUST] < INS_SONAR_THROTTLE_THRESHOLD +#endif +#ifdef INS_SONAR_STAB_THRESHOLD + && stabilization_cmd[COMMAND_ROLL] < INS_SONAR_STAB_THRESHOLD + && stabilization_cmd[COMMAND_ROLL] > -INS_SONAR_STAB_THRESHOLD + && stabilization_cmd[COMMAND_PITCH] < INS_SONAR_STAB_THRESHOLD + && stabilization_cmd[COMMAND_PITCH] > -INS_SONAR_STAB_THRESHOLD + && stabilization_cmd[COMMAND_YAW] < INS_SONAR_STAB_THRESHOLD + && stabilization_cmd[COMMAND_YAW] > -INS_SONAR_STAB_THRESHOLD +#endif +#ifdef INS_SONAR_BARO_THRESHOLD + && ins_baro_alt > -POS_BFP_OF_REAL(INS_SONAR_BARO_THRESHOLD) /* z down */ +#endif + && ins_update_on_agl + && baro.status == BS_RUNNING) { + vff_update_alt_conf(-sonar, VFF_R_SONAR_0 + VFF_R_SONAR_OF_M * fabs(sonar)); + last_offset = vff_offset; + } + else { + /* update offset with last value to avoid divergence */ + vff_update_offset(last_offset); + } +#endif +}