rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-02 10:24:11 +01:00
Code Issues Releases Activity

INS: Miscellaneous changes for debugging handling and also some stuff for GPS

Browse Source 
signal loss.
This commit is contained in:
James Cotton 2011-09-12 11:17:17 -05:00
parent 5b836db54c
commit 00c9d62f6a
3 changed files with 84 additions and 60 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
flight/INS/ins.c
View File

@ -233,18 +233,23 @@ int main()
if(ISNAN(accel_data.filtered.x + accel_data.filtered.y + accel_data.filtered.z) || if(ISNAN(accel_data.filtered.x + accel_data.filtered.y + accel_data.filtered.z) ||
ISNAN(gyro_data.filtered.x + gyro_data.filtered.y + gyro_data.filtered.z) || ISNAN(gyro_data.filtered.x + gyro_data.filtered.y + gyro_data.filtered.z) ||
ACCEL_OOB(accel_data.filtered.x + accel_data.filtered.y + accel_data.filtered.z) || ACCEL_OOB(accel_data.filtered.x) || ACCEL_OOB(accel_data.filtered.y) || ACCEL_OOB(accel_data.filtered.z) ||
GYRO_OOB(gyro_data.filtered.x + gyro_data.filtered.y + gyro_data.filtered.z)) { GYRO_OOB(gyro_data.filtered.x) || GYRO_OOB(gyro_data.filtered.y) || GYRO_OOB(gyro_data.filtered.z)) {
// If any values are NaN or huge don't update // If any values are NaN or huge don't update
//TODO: add field to ahrs status to track number of these events //TODO: add field to ahrs status to track number of these events
continue; continue;
} }
if(total_conversion_blocks <= 3000 && !zeroed_gyros) { if(total_conversion_blocks <= 100 && !zeroed_gyros) {
zero_gyros(total_conversion_blocks == 3000); // TODO: Replace this with real init
if(total_conversion_blocks == 3000) zero_gyros(total_conversion_blocks == 100);
if(total_conversion_blocks == 100)
zeroed_gyros = true; zeroed_gyros = true;
PIOS_DELAY_WaituS(TYPICAL_PERIOD); PIOS_DELAY_WaituS(TYPICAL_PERIOD);
float zeros[3] = {0,0,0};
INSSetGyroBias(zeros);
continue; continue;
} }
/* If algorithm changed reinit. This could go in callback but wouldn't be synchronous */ /* If algorithm changed reinit. This could go in callback but wouldn't be synchronous */
@ -255,8 +260,9 @@ int main()
time_val2 = PIOS_DELAY_GetRaw(); time_val2 = PIOS_DELAY_GetRaw();
//print_ekf_binary(true); print_ekf_binary(true);
switch(ahrs_algorithm) { switch(ahrs_algorithm) {
case INSSETTINGS_ALGORITHM_SIMPLE: case INSSETTINGS_ALGORITHM_SIMPLE:
simple_update(); simple_update();
@ -322,8 +328,11 @@ static void print_ekf_binary(bool ekf)
delay = PIOS_DELAY_DiffuS(timeval); delay = PIOS_DELAY_DiffuS(timeval);
timeval = PIOS_DELAY_GetRaw(); timeval = PIOS_DELAY_GetRaw();
PIOS_DELAY_WaituS(500);
uint8_t framing[2] = { 0xff, 0x00 }; uint8_t framing[2] = { 0xff, 0x00 };
// Dump raw buffer // Dump raw buffer
PIOS_COM_SendBuffer(PIOS_COM_AUX, &framing[0], sizeof(framing)); PIOS_COM_SendBuffer(PIOS_COM_AUX, &framing[0], sizeof(framing));
PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & total_conversion_blocks, sizeof(total_conversion_blocks)); PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & total_conversion_blocks, sizeof(total_conversion_blocks));
PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & accel_data.filtered.x, 4*3); PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & accel_data.filtered.x, 4*3);
@ -615,7 +624,7 @@ void get_gps_data()
gps_data.heading = pos.Heading; gps_data.heading = pos.Heading;
gps_data.groundspeed = pos.Groundspeed; gps_data.groundspeed = pos.Groundspeed;
gps_data.quality = 1; /* currently unused */ gps_data.quality = pos.Satellites;
gps_data.updated = true; gps_data.updated = true;
const uint32_t INSGPS_GPS_MINSAT = 6; const uint32_t INSGPS_GPS_MINSAT = 6;
@ -628,7 +637,6 @@ void get_gps_data()
(home.Set == HOMELOCATION_SET_FALSE) || (home.Set == HOMELOCATION_SET_FALSE) ||
((home.ECEF[0] == 0) && (home.ECEF[1] == 0) && (home.ECEF[2] == 0))) ((home.ECEF[0] == 0) && (home.ECEF[1] == 0) && (home.ECEF[2] == 0)))
{ {
gps_data.quality = 0;
gps_data.updated = false; gps_data.updated = false;
} }
} }

110
flight/INS/insgps_helper.c
View File

@ -11,7 +11,9 @@
#define INSGPS_GPS_TIMEOUT 2 /* 2 seconds triggers reinit of position */ #define INSGPS_GPS_TIMEOUT 2 /* 2 seconds triggers reinit of position */
#define INSGPS_GPS_MINSAT 6 /* 2 seconds triggers reinit of position */ #define INSGPS_GPS_MINSAT 6 /* 2 seconds triggers reinit of position */
#define INSGPS_GPS_MINPDOP 3.5 /* minimum PDOP for postition updates */ #define INSGPS_GPS_MINPDOP 3.5 /* minimum PDOP for postition updates */
/* If GPS is more than this distance on any dimension then wait a few updates */
/* and reinitialize there */
#define INSGPS_GPS_FAR 10
#define timer_rate() 100000 #define timer_rate() 100000
#define timer_count() 1 #define timer_count() 1
@ -50,13 +52,14 @@ static uint32_t ins_last_time;
/** /**
* @brief Update the EKF when in outdoor mode. The primary difference is using the GPS values. * @brief Update the EKF when in outdoor mode. The primary difference is using the GPS values.
*/ */
uint32_t total_far_count = 0;
uint32_t relocated = 0;
void ins_outdoor_update() void ins_outdoor_update()
{ {
float gyro[3], accel[3], vel[3]; float gyro[3], accel[3], vel[3];
static uint32_t last_gps_time = 0;
float dT; float dT;
uint16_t sensors; uint16_t sensors;
static uint32_t gps_far_count = 0;
dT = PIOS_DELAY_DiffuS(ins_last_time) / 1e6; dT = PIOS_DELAY_DiffuS(ins_last_time) / 1e6;
ins_last_time = PIOS_DELAY_GetRaw(); ins_last_time = PIOS_DELAY_GetRaw();
@ -97,49 +100,42 @@ void ins_outdoor_update()
sensors = 0; sensors = 0;
/*
* Detect if greater than certain time since last gps update and if so
* reset EKF to that position since probably drifted too far for safe
* update
*/
uint32_t this_gps_time = timer_count();
float gps_delay;
if (this_gps_time < last_gps_time)
gps_delay = ((0xFFFF - last_gps_time) - this_gps_time) / timer_rate();
else
gps_delay = (this_gps_time - last_gps_time) / timer_rate();
last_gps_time = this_gps_time;
if (gps_data.updated) if (gps_data.updated)
{ {
vel[0] = gps_data.groundspeed * cos(gps_data.heading * DEG_TO_RAD); vel[0] = gps_data.groundspeed * cos(gps_data.heading * DEG_TO_RAD);
vel[1] = gps_data.groundspeed * sin(gps_data.heading * DEG_TO_RAD); vel[1] = gps_data.groundspeed * sin(gps_data.heading * DEG_TO_RAD);
vel[2] = 0; vel[2] = 0;
if(gps_delay > INSGPS_GPS_TIMEOUT) if (abs(gps_data.NED[0] - Nav.Pos[0]) > INSGPS_GPS_FAR ||
INSPosVelReset(gps_data.NED,vel); // position stale, reset abs(gps_data.NED[1] - Nav.Pos[1]) > INSGPS_GPS_FAR ||
else { abs(gps_data.NED[2] - Nav.Pos[2]) > INSGPS_GPS_FAR ||
abs(vel[0] - Nav.Vel[0]) > INSGPS_GPS_FAR ||
abs(vel[1] - Nav.Vel[1]) > INSGPS_GPS_FAR) {
gps_far_count++;
total_far_count++;
gps_data.updated = false;
if(gps_far_count > 30) {
INSPosVelReset(gps_data.NED,vel);
relocated++;
}
}
else {
sensors |= HORIZ_SENSORS | POS_SENSORS; sensors |= HORIZ_SENSORS | POS_SENSORS;
/*
* When using gps need to make sure that barometer is brought into NED frame
* we should try and see if the altitude from the home location is good enough
* to use for the offset but for now starting with this conservative filter
*/
if(fabs(gps_data.NED[2] + (altitude_data.altitude - baro_offset)) > 10) {
baro_offset = gps_data.NED[2] + altitude_data.altitude;
} else {
/* IIR filter with 100 second or so tau to keep them crudely in the same frame */
baro_offset = baro_offset * 0.999 + (gps_data.NED[2] + altitude_data.altitude) * 0.001;
}
gps_data.updated = false;
} }
/*
* When using gps need to make sure that barometer is brought into NED frame
* we should try and see if the altitude from the home location is good enough
* to use for the offset but for now starting with this conservative filter
*/
if(fabs(gps_data.NED[2] + (altitude_data.altitude - baro_offset)) > 10) {
baro_offset = gps_data.NED[2] + altitude_data.altitude;
} else {
/* IIR filter with 100 second or so tau to keep them crudely in the same frame */
baro_offset = baro_offset * 0.999 + (gps_data.NED[2] + altitude_data.altitude) * 0.001;
}
gps_data.updated = false;
} else if (gps_delay > INSGPS_GPS_TIMEOUT) {
vel[0] = 0;
vel[1] = 0;
vel[2] = 0;
sensors |= VERT_SENSORS | HORIZ_SENSORS;
} }
if(mag_data.updated) { if(mag_data.updated) {
@ -157,6 +153,11 @@ void ins_outdoor_update()
* although probably should occur within INS itself * although probably should occur within INS itself
*/ */
INSCorrection(mag_data.scaled.axis, gps_data.NED, vel, altitude_data.altitude - baro_offset, sensors); INSCorrection(mag_data.scaled.axis, gps_data.NED, vel, altitude_data.altitude - baro_offset, sensors);
if(fabs(Nav.gyro_bias[0]) > 0.1 || fabs(Nav.gyro_bias[1]) > 0.1 || fabs(Nav.gyro_bias[2]) > 0.1) {
float zeros[3] = {0,0,0};
INSSetGyroBias(zeros);
}
} }
uint32_t time_val_a; uint32_t time_val_a;
@ -166,6 +167,8 @@ uint32_t indoor_time;
*/ */
void ins_indoor_update() void ins_indoor_update()
{ {
static uint32_t updated_without_gps = 0;
time_val_a = PIOS_DELAY_GetRaw(); time_val_a = PIOS_DELAY_GetRaw();
float gyro[3], accel[3], vel[3]; float gyro[3], accel[3], vel[3];
@ -180,7 +183,6 @@ void ins_indoor_update()
if(dT > 0.01) if(dT > 0.01)
dT = 0.01; dT = 0.01;
// format data for INS algo // format data for INS algo
gyro[0] = gyro_data.filtered.x; gyro[0] = gyro_data.filtered.x;
gyro[1] = gyro_data.filtered.y; gyro[1] = gyro_data.filtered.y;
@ -245,21 +247,29 @@ void ins_indoor_update()
velocityActual.East = gps_data.groundspeed * sin(gps_data.heading * DEG_TO_RAD); velocityActual.East = gps_data.groundspeed * sin(gps_data.heading * DEG_TO_RAD);
velocityActual.Down = Nav.Vel[2]; velocityActual.Down = Nav.Vel[2];
VelocityActualSet(&velocityActual); VelocityActualSet(&velocityActual);
updated_without_gps = 0;
gps_data.updated = false; gps_data.updated = false;
} else { } else {
PositionActualData positionActual; PositionActualData positionActual;
PositionActualGet(&positionActual); PositionActualGet(&positionActual);
positionActual.North = NAN;
positionActual.East = NAN;
positionActual.Down = Nav.Pos[2];
PositionActualSet(&positionActual);
VelocityActualData velocityActual; VelocityActualData velocityActual;
VelocityActualGet(&velocityActual); VelocityActualGet(&velocityActual);
velocityActual.North = NAN;
velocityActual.East = NAN; positionActual.Down = Nav.Pos[2];
velocityActual.Down = Nav.Vel[2]; velocityActual.Down = Nav.Vel[2];
if(updated_without_gps > 500) {
// After 2-3 seconds without a GPS update set velocity estimate to NAN
positionActual.North = NAN;
positionActual.East = NAN;
velocityActual.North = NAN;
velocityActual.East = NAN;
} else
updated_without_gps++;
PositionActualSet(&positionActual);
VelocityActualSet(&velocityActual); VelocityActualSet(&velocityActual);
} }
@ -269,6 +279,12 @@ void ins_indoor_update()
*/ */
INSCorrection(mag_data.scaled.axis, gps_data.NED, vel, altitude_data.altitude, sensors | HORIZ_SENSORS | VERT_SENSORS); INSCorrection(mag_data.scaled.axis, gps_data.NED, vel, altitude_data.altitude, sensors | HORIZ_SENSORS | VERT_SENSORS);
indoor_time = PIOS_DELAY_DiffuS(time_val_a); indoor_time = PIOS_DELAY_DiffuS(time_val_a);
if(fabs(Nav.gyro_bias[0]) > 0.1 || fabs(Nav.gyro_bias[1]) > 0.1 || fabs(Nav.gyro_bias[2]) > 0.1) {
float zeros[3] = {0,0,0};
INSSetGyroBias(zeros);
}
} }
/** /**
@ -295,7 +311,7 @@ void ins_init_algorithm()
using_mags = (ahrs_algorithm == INSSETTINGS_ALGORITHM_INSGPS_OUTDOOR) || (ahrs_algorithm == INSSETTINGS_ALGORITHM_INSGPS_INDOOR); using_mags = (ahrs_algorithm == INSSETTINGS_ALGORITHM_INSGPS_OUTDOOR) || (ahrs_algorithm == INSSETTINGS_ALGORITHM_INSGPS_INDOOR);
using_mags &= (home.Be[0] != 0) || (home.Be[1] != 0) || (home.Be[2] != 0); /* only use mags when valid home location */ using_mags &= (home.Be[0] != 0) || (home.Be[1] != 0) || (home.Be[2] != 0); /* only use mags when valid home location */
using_gps = (ahrs_algorithm == INSSETTINGS_ALGORITHM_INSGPS_OUTDOOR) && (gps_data.quality != 0); using_gps = (ahrs_algorithm == INSSETTINGS_ALGORITHM_INSGPS_OUTDOOR) && (gps_data.quality >= INSGPS_GPS_MINSAT);
/* Block till a data update */ /* Block till a data update */
get_accel_gyro_data(); get_accel_gyro_data();

8
flight/INS/pios_board.c
View File

@ -329,9 +329,9 @@ static const struct pios_usart_cfg pios_usart_gps_cfg = {
}, },
}; };
#define PIOS_COM_AUX_TX_BUF_LEN 512 #define PIOS_COM_AUX_TX_BUF_LEN 150
static uint8_t pios_com_aux_tx_buffer[PIOS_COM_AUX_TX_BUF_LEN]; static uint8_t pios_com_aux_tx_buffer[PIOS_COM_AUX_TX_BUF_LEN];
#define PIOS_COM_AUX_RX_BUF_LEN 512 #define PIOS_COM_AUX_RX_BUF_LEN 10
static uint8_t pios_com_aux_rx_buffer[PIOS_COM_AUX_RX_BUF_LEN]; static uint8_t pios_com_aux_rx_buffer[PIOS_COM_AUX_RX_BUF_LEN];
#endif /* PIOS_INCLUDE_GPS */ #endif /* PIOS_INCLUDE_GPS */
@ -382,9 +382,9 @@ static const struct pios_usart_cfg pios_usart_aux_cfg = {
}, },
}; };
#define PIOS_COM_GPS_TX_BUF_LEN 192 #define PIOS_COM_GPS_TX_BUF_LEN 10
static uint8_t pios_com_gps_tx_buffer[PIOS_COM_GPS_TX_BUF_LEN]; static uint8_t pios_com_gps_tx_buffer[PIOS_COM_GPS_TX_BUF_LEN];
#define PIOS_COM_GPS_RX_BUF_LEN 96 #define PIOS_COM_GPS_RX_BUF_LEN 192
static uint8_t pios_com_gps_rx_buffer[PIOS_COM_GPS_RX_BUF_LEN]; static uint8_t pios_com_gps_rx_buffer[PIOS_COM_GPS_RX_BUF_LEN];
#endif /* PIOS_COM_AUX */ #endif /* PIOS_COM_AUX */