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

Merge branch 'imav' into imav_merge

Browse Source 
Conflicts:
	sw/airborne/firmwares/rotorcraft/guidance/guidance_v_adpt.h
This commit is contained in:
Gautier Hattenberger
2012-07-11 11:57:27 +02:00
parent 3ba5a83eec c77307b8ed
commit 3d41115fe9
26 changed files with 989 additions and 51 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/firmwares/rotorcraft.makefile
+7 -8
View File
@@ -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
conf/firmwares/subsystems/rotorcraft/ins.makefile
+10
View File
@@ -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).)'
conf/firmwares/subsystems/rotorcraft/ins_extended.makefile
+10
View File
@@ -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).)'
conf/messages.xml
+10
View File
@@ -1334,6 +1334,16 @@
</message>
<!-- 161 is free -->
<message name="VFF_EXTENDED" id="161">
<field name="measure1" type="float"/>
<field name="measure2" type="float"/>
<field name="z" type="float"/>
<field name="zd" type="float"/>
<field name="zdd" type="float"/>
<field name="bias" type="float"/>
<field name="offset" type="float"/>
</message>
<message name="VFF" id="162">
<field name="measure" type="float"/>
conf/radios/FlyElectricRx31.xml
+11
View File
@@ -0,0 +1,11 @@
<?xml version="1.0"?>
<!DOCTYPE radio SYSTEM "radio.dtd">
<radio name="FlyElectric Rx31" data_min="900" data_max="2050" sync_min ="5000" sync_max ="15000" pulse_type="NEGATIVE">
<channel ctl="A" function="THROTTLE" min="1200" neutral="1200" max="1900" average="0"/>
<channel ctl="D" function="ROLL" max="1200" neutral="1520" min="1900" average="0"/>
<channel ctl="C" function="PITCH" max="1200" neutral="1520" min="1900" average="0"/>
<channel ctl="B" function="YAW" max="1200" neutral="1520" min="1900" average="0"/>
<channel ctl="G" function="GEAR" min="1200" neutral="1500" max="1900" average="1"/>
<channel ctl="E" function="FLAP" min="1200" neutral="1500" max="1900" average="1"/>
<channel ctl="F" function="MODE" min="1200" neutral="1500" max="1900" average="1"/>
</radio>
conf/settings/control/stabilization_att_int.xml
+7 -4
View File
@@ -4,18 +4,21 @@
<dl_settings>
<dl_settings NAME="Att Loop">
<dl_setting var="stabilization_gains.p.x" min="1" step="1" max="4000" module="stabilization/stabilization_attitude" shortname="pgain phi" param="STABILIZATION_ATTITUDE_PHI_PGAIN"/>
<dl_setting var="stabilization_gains.p.x" min="1" step="1" max="8000" module="stabilization/stabilization_attitude" shortname="pgain phi" param="STABILIZATION_ATTITUDE_PHI_PGAIN"/>
<dl_setting var="stabilization_gains.d.x" min="1" step="1" max="4000" module="stabilization/stabilization_attitude" shortname="dgain p" param="STABILIZATION_ATTITUDE_PHI_DGAIN"/>
<dl_setting var="stabilization_gains.i.x" min="0" step="1" max="400" module="stabilization/stabilization_attitude" shortname="igain phi" handler="SetKiPhi"param="STABILIZATION_ATTITUDE_PHI_IGAIN" />
<dl_setting var="stabilization_gains.i.x" min="0" step="1" max="800" module="stabilization/stabilization_attitude" shortname="igain phi" handler="SetKiPhi"param="STABILIZATION_ATTITUDE_PHI_IGAIN" />
<dl_setting var="stabilization_gains.dd.x" min="0" step="1" max="1000" module="stabilization/stabilization_attitude" shortname="ddgain p" param="STABILIZATION_ATTITUDE_PHI_DDGAIN"/>
<dl_setting var="stabilization_gains.p.y" min="1" step="1" max="4000" module="stabilization/stabilization_attitude" shortname="pgain theta" param="STABILIZATION_ATTITUDE_THETA_PGAIN"/>
<dl_setting var="stabilization_gains.a.x" min="0" step="1" max="1000" module="stabilization/stabilization_attitude" shortname="again phi" param="STABILIZATION_ATTITUDE_PHI_AGAIN"/>
<dl_setting var="stabilization_gains.p.y" min="1" step="1" max="8000" module="stabilization/stabilization_attitude" shortname="pgain theta" param="STABILIZATION_ATTITUDE_THETA_PGAIN"/>
<dl_setting var="stabilization_gains.d.y" min="1" step="1" max="4000" module="stabilization/stabilization_attitude" shortname="dgain q" param="STABILIZATION_ATTITUDE_THETA_DGAIN"/>
<dl_setting var="stabilization_gains.i.y" min="0" step="1" max="400" module="stabilization/stabilization_attitude" shortname="igain theta" param="STABILIZATION_ATTITUDE_THETA_IGAIN"/>
<dl_setting var="stabilization_gains.i.y" min="0" step="1" max="800" module="stabilization/stabilization_attitude" shortname="igain theta" param="STABILIZATION_ATTITUDE_THETA_IGAIN"/>
<dl_setting var="stabilization_gains.dd.y" min="0" step="1" max="1000" module="stabilization/stabilization_attitude" shortname="ddgain q" param="STABILIZATION_ATTITUDE_THETA_DDGAIN"/>
<dl_setting var="stabilization_gains.a.y" min="0" step="1" max="1000" module="stabilization/stabilization_attitude" shortname="again theta" param="STABILIZATION_ATTITUDE_THETA_AGAIN"/>
<dl_setting var="stabilization_gains.p.z" min="1" step="1" max="4000" module="stabilization/stabilization_attitude" shortname="pgain psi" param="STABILIZATION_ATTITUDE_PSI_PGAIN"/>
<dl_setting var="stabilization_gains.d.z" min="1" step="1" max="4000" module="stabilization/stabilization_attitude" shortname="dgain r" param="STABILIZATION_ATTITUDE_PSI_DGAIN"/>
<dl_setting var="stabilization_gains.i.z" min="0" step="1" max="400" module="stabilization/stabilization_attitude" shortname="igain psi" param="STABILIZATION_ATTITUDE_PSI_IGAIN"/>
<dl_setting var="stabilization_gains.dd.z" min="0" step="1" max="1000" module="stabilization/stabilization_attitude" shortname="ddgain r" param="STABILIZATION_ATTITUDE_PSI_DDGAIN"/>
<dl_setting var="stabilization_gains.a.z" min="0" step="1" max="1000" module="stabilization/stabilization_attitude" shortname="again psi" param="STABILIZATION_ATTITUDE_PSI_AGAIN"/>
</dl_settings>
</dl_settings>
conf/settings/estimation/ins_sonar.xml
+9
View File
@@ -0,0 +1,9 @@
<!DOCTYPE settings SYSTEM "../settings.dtd">
<settings>
<dl_settings>
<dl_settings NAME="sonar">
<dl_setting MAX="1" MIN="0" STEP="1" VAR="ins_update_on_agl" shortname="use_sonar" values="FALSE|TRUE" module="subsystems/ins/vf_extended_float"/>
</dl_settings>
</dl_settings>
</settings>
sw/airborne/boards/navgo/baro_board.h
+2 -4
View File
@@ -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; \
} \
sw/airborne/boards/navgo/imu_navgo.c
+11 -1
View File
@@ -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;
}
sw/airborne/boards/navgo_1.0.h
+20 -10
View File
@@ -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
sw/airborne/filters/median_filter.h
+131
View File
@@ -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
sw/airborne/firmwares/rotorcraft/autopilot.c
+4 -1
View File
@@ -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 */
sw/airborne/firmwares/rotorcraft/guidance/guidance_v_adpt.h
+1 -1
View File
@@ -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)
sw/airborne/firmwares/rotorcraft/main.c
+4 -2
View File
@@ -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);
sw/airborne/firmwares/rotorcraft/navigation.c
+10 -5
View File
@@ -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;
sw/airborne/firmwares/rotorcraft/navigation.h
+11 -2
View File
@@ -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) { \
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c
+11 -3
View File
@@ -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 */
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_int.h
+1
View File
@@ -29,6 +29,7 @@
#include "generated/airframe.h"
struct Int32AttitudeGains {
struct Int32Vect3 a;
struct Int32Vect3 p;
struct Int32Vect3 d;
struct Int32Vect3 dd;
sw/airborne/firmwares/rotorcraft/telemetry.h
+15
View File
@@ -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) { \
sw/airborne/modules/cam_control/booz_cam.c
+8 -3
View File
@@ -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
}
sw/airborne/modules/cam_control/booz_cam.h
+1 -5
View File
@@ -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);
sw/airborne/subsystems/gps.h
+11 -1
View File
@@ -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
sw/airborne/subsystems/ins.h
+1 -1
View File
@@ -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;
sw/airborne/subsystems/ins/vf_extended_float.c
+314
View File
@@ -0,0 +1,314 @@
/*
* Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
* 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<VFF_STATE_SIZE; i++) {
for (j=0; j<VFF_STATE_SIZE; j++)
vff_P[i][j] = 0.;
vff_P[i][i] = INIT_PXX;
}
}
/*
F = [ 1 dt -dt^2/2 0
0 1 -dt 0
0 0 1 0
0 0 0 1 ];
B = [ dt^2/2 dt 0 0]';
Q = [ Qzz 0 0 0
0 Qzdotzdot 0 0
0 0 Qbiasbias 0
0 0 0 0 Qoffoff ];
Xk1 = F * Xk0 + B * accel;
Pk1 = F * Pk0 * F' + Q;
*/
void vff_propagate(float accel) {
/* update state */
vff_zdotdot = accel + 9.81 - vff_bias;
vff_z = vff_z + DT_VFILTER * vff_zdot;
vff_zdot = vff_zdot + DT_VFILTER * vff_zdotdot;
/* update covariance */
const float FPF00 = vff_P[0][0] + DT_VFILTER * ( vff_P[1][0] + vff_P[0][1] + DT_VFILTER * vff_P[1][1] );
const float FPF01 = vff_P[0][1] + DT_VFILTER * ( vff_P[1][1] - vff_P[0][2] - DT_VFILTER * vff_P[1][2] );
const float FPF02 = vff_P[0][2] + DT_VFILTER * ( vff_P[1][2] );
const float FPF10 = vff_P[1][0] + DT_VFILTER * (-vff_P[2][0] + vff_P[1][1] - DT_VFILTER * vff_P[2][1] );
const float FPF11 = vff_P[1][1] + DT_VFILTER * (-vff_P[2][1] - vff_P[1][2] + DT_VFILTER * vff_P[2][2] );
const float FPF12 = vff_P[1][2] + DT_VFILTER * (-vff_P[2][2] );
const float FPF20 = vff_P[2][0] + DT_VFILTER * ( vff_P[2][1] );
const float FPF21 = vff_P[2][1] + DT_VFILTER * (-vff_P[2][2] );
const float FPF22 = vff_P[2][2];
const float FPF33 = vff_P[3][3];
vff_P[0][0] = FPF00 + Qzz;
vff_P[0][1] = FPF01;
vff_P[0][2] = FPF02;
vff_P[1][0] = FPF10;
vff_P[1][1] = FPF11 + Qzdotzdot;
vff_P[1][2] = FPF12;
vff_P[2][0] = FPF20;
vff_P[2][1] = FPF21;
vff_P[2][2] = FPF22 + Qbiasbias;
vff_P[3][3] = FPF33 + Qoffoff;
#if DEBUG_VFF_EXTENDED
RunOnceEvery(10, DOWNLINK_SEND_VFF_EXTENDED(DefaultChannel, DefaultDevice, &vff_z_meas, &vff_z_meas_baro, &vff_z, &vff_zdot, &vff_zdotdot, &vff_bias, &vff_offset));
#endif
}
/*
* Update sensor "with" offset (baro)
H = [1 0 0 -1];
// state residual
y = rangemeter - H * Xm;
// covariance residual
S = H*Pm*H' + R;
// kalman gain
K = Pm*H'*inv(S);
// update state
Xp = Xm + K*y;
// update covariance
Pp = Pm - K*H*Pm;
*/
__attribute__ ((always_inline)) static inline void update_baro_conf(float z_meas, float conf) {
vff_z_meas_baro = z_meas;
const float y = z_meas - vff_z + vff_offset;
const float S = vff_P[0][0] - vff_P[0][3] - vff_P[3][0] + vff_P[3][3] + conf;
const float K0 = (vff_P[0][0] - vff_P[0][3]) * 1/S;
const float K1 = (vff_P[1][0] - vff_P[1][3]) * 1/S;
const float K2 = (vff_P[2][0] - vff_P[2][3]) * 1/S;
const float K3 = (vff_P[3][0] - vff_P[3][3]) * 1/S;
vff_z = vff_z + K0 * y;
vff_zdot = vff_zdot + K1 * y;
vff_bias = vff_bias + K2 * y;
vff_offset = vff_offset + K3 * y;
const float P0 = vff_P[0][0] - vff_P[3][0];
const float P1 = vff_P[0][1] - vff_P[3][1];
const float P2 = vff_P[0][2] - vff_P[3][2];
const float P3 = vff_P[0][3] - vff_P[3][3];
vff_P[0][0] -= K0 * P0;
vff_P[0][1] -= K0 * P1;
vff_P[0][2] -= K0 * P2;
vff_P[0][3] -= K0 * P3;
vff_P[1][0] -= K1 * P0;
vff_P[1][1] -= K1 * P1;
vff_P[1][2] -= K1 * P2;
vff_P[1][3] -= K1 * P3;
vff_P[2][0] -= K2 * P0;
vff_P[2][1] -= K2 * P1;
vff_P[2][2] -= K2 * P2;
vff_P[2][3] -= K2 * P3;
vff_P[3][0] -= K3 * P0;
vff_P[3][1] -= K3 * P1;
vff_P[3][2] -= K3 * P2;
vff_P[3][3] -= K3 * P3;
}
void vff_update_baro(float z_meas) {
update_baro_conf(z_meas, R_BARO);
}
void vff_update_baro_conf(float z_meas, float conf) {
update_baro_conf(z_meas, conf);
}
/*
* Update sensor "without" offset (gps, sonar)
H = [1 0 0 0];
// state residual
y = rangemeter - H * Xm;
// covariance residual
S = H*Pm*H' + R;
// kalman gain
K = Pm*H'*inv(S);
// update state
Xp = Xm + K*y;
// update covariance
Pp = Pm - K*H*Pm;
*/
__attribute__ ((always_inline)) static inline void update_alt_conf(float z_meas, float conf) {
vff_z_meas = z_meas;
const float y = z_meas - vff_z;
const float S = vff_P[0][0] + conf;
const float K0 = vff_P[0][0] * 1/S;
const float K1 = vff_P[1][0] * 1/S;
const float K2 = vff_P[2][0] * 1/S;
const float K3 = vff_P[3][0] * 1/S;
vff_z = vff_z + K0 * y;
vff_zdot = vff_zdot + K1 * y;
vff_bias = vff_bias + K2 * y;
vff_offset = vff_offset + K3 * y;
const float P0 = vff_P[0][0];
const float P1 = vff_P[0][1];
const float P2 = vff_P[0][2];
const float P3 = vff_P[0][3];
vff_P[0][0] -= K0 * P0;
vff_P[0][1] -= K0 * P1;
vff_P[0][2] -= K0 * P2;
vff_P[0][3] -= K0 * P3;
vff_P[1][0] -= K1 * P0;
vff_P[1][1] -= K1 * P1;
vff_P[1][2] -= K1 * P2;
vff_P[1][3] -= K1 * P3;
vff_P[2][0] -= K2 * P0;
vff_P[2][1] -= K2 * P1;
vff_P[2][2] -= K2 * P2;
vff_P[2][3] -= K2 * P3;
vff_P[3][0] -= K3 * P0;
vff_P[3][1] -= K3 * P1;
vff_P[3][2] -= K3 * P2;
vff_P[3][3] -= K3 * P3;
}
void vff_update_alt(float z_meas) {
update_alt_conf(z_meas, R_ALT);
}
void vff_update_alt_conf(float z_meas, float conf) {
update_alt_conf(z_meas, conf);
}
/*
* Update sensor offset (baro)
H = [0 0 0 1];
// state residual
y = rangemeter - H * Xm;
// covariance residual
S = H*Pm*H' + R;
// kalman gain
K = Pm*H'*inv(S);
// update state
Xp = Xm + K*y;
// update covariance
Pp = Pm - K*H*Pm;
*/
__attribute__ ((always_inline)) static inline void update_offset_conf(float offset, float conf) {
const float y = offset - vff_offset;
const float S = vff_P[3][3] + conf;
const float K0 = vff_P[0][3] * 1/S;
const float K1 = vff_P[1][3] * 1/S;
const float K2 = vff_P[2][3] * 1/S;
const float K3 = vff_P[3][3] * 1/S;
vff_z = vff_z + K0 * y;
vff_zdot = vff_zdot + K1 * y;
vff_bias = vff_bias + K2 * y;
vff_offset = vff_offset + K3 * y;
const float P0 = vff_P[3][0];
const float P1 = vff_P[3][1];
const float P2 = vff_P[3][2];
const float P3 = vff_P[3][3];
vff_P[0][0] -= K0 * P0;
vff_P[0][1] -= K0 * P1;
vff_P[0][2] -= K0 * P2;
vff_P[0][3] -= K0 * P3;
vff_P[1][0] -= K1 * P0;
vff_P[1][1] -= K1 * P1;
vff_P[1][2] -= K1 * P2;
vff_P[1][3] -= K1 * P3;
vff_P[2][0] -= K2 * P0;
vff_P[2][1] -= K2 * P1;
vff_P[2][2] -= K2 * P2;
vff_P[2][3] -= K2 * P3;
vff_P[3][0] -= K3 * P0;
vff_P[3][1] -= K3 * P1;
vff_P[3][2] -= K3 * P2;
vff_P[3][3] -= K3 * P3;
}
void vff_update_offset(float offset) {
update_offset_conf(offset, R_OFFSET);
}
void vff_realign(float z_meas) {
//vff_z = z_meas;
//vff_zdot = 0.;
//vff_offset = 0.;
vff_init(z_meas, 0., 0., 0.);
}
sw/airborne/subsystems/ins/vf_extended_float.h
+48
View File
@@ -0,0 +1,48 @@
/*
* Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
* 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 */
sw/airborne/subsystems/ins_extended.c
+321
View File
@@ -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 <stdio.h>
#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
}