rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-01 09:24:10 +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) ||
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) ||
GYRO_OOB(gyro_data.filtered.x + gyro_data.filtered.y + gyro_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_OOB(gyro_data.filtered.y) || GYRO_OOB(gyro_data.filtered.z)) {
// If any values are NaN or huge don't update
//TODO: add field to ahrs status to track number of these events
continue;
}
if(total_conversion_blocks <= 3000 && !zeroed_gyros) {
zero_gyros(total_conversion_blocks == 3000);
if(total_conversion_blocks == 3000)
if(total_conversion_blocks <= 100 && !zeroed_gyros) {
// TODO: Replace this with real init
zero_gyros(total_conversion_blocks == 100);
if(total_conversion_blocks == 100)
zeroed_gyros = true;
PIOS_DELAY_WaituS(TYPICAL_PERIOD);
float zeros[3] = {0,0,0};
INSSetGyroBias(zeros);
continue;
}
/* 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();
//print_ekf_binary(true);
print_ekf_binary(true);
switch(ahrs_algorithm) {
case INSSETTINGS_ALGORITHM_SIMPLE:
simple_update();
@ -322,8 +328,11 @@ static void print_ekf_binary(bool ekf)
delay = PIOS_DELAY_DiffuS(timeval);
timeval = PIOS_DELAY_GetRaw();
PIOS_DELAY_WaituS(500);
uint8_t framing[2] = { 0xff, 0x00 };
// Dump raw buffer
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 *) & accel_data.filtered.x, 4*3);
@ -615,7 +624,7 @@ void get_gps_data()
gps_data.heading = pos.Heading;
gps_data.groundspeed = pos.Groundspeed;
gps_data.quality = 1; /* currently unused */
gps_data.quality = pos.Satellites;
gps_data.updated = true;
const uint32_t INSGPS_GPS_MINSAT = 6;
@ -628,7 +637,6 @@ void get_gps_data()
(home.Set == HOMELOCATION_SET_FALSE) ||
((home.ECEF[0] == 0) && (home.ECEF[1] == 0) && (home.ECEF[2] == 0)))
{
gps_data.quality = 0;
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_MINSAT 6 /* 2 seconds triggers reinit of position */
#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_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.
*/
uint32_t total_far_count = 0;
uint32_t relocated = 0;
void ins_outdoor_update()
{
float gyro[3], accel[3], vel[3];
static uint32_t last_gps_time = 0;
float dT;
uint16_t sensors;
static uint32_t gps_far_count = 0;
dT = PIOS_DELAY_DiffuS(ins_last_time) / 1e6;
ins_last_time = PIOS_DELAY_GetRaw();
@ -97,49 +100,42 @@ void ins_outdoor_update()
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)
{
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[2] = 0;
if(gps_delay > INSGPS_GPS_TIMEOUT)
INSPosVelReset(gps_data.NED,vel); // position stale, reset
else {
if (abs(gps_data.NED[0] - Nav.Pos[0]) > INSGPS_GPS_FAR ||
abs(gps_data.NED[1] - Nav.Pos[1]) > INSGPS_GPS_FAR ||
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;
/*
* 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) {
@ -157,6 +153,11 @@ void ins_outdoor_update()
* although probably should occur within INS itself
*/
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;
@ -166,6 +167,8 @@ uint32_t indoor_time;
*/
void ins_indoor_update()
{
static uint32_t updated_without_gps = 0;
time_val_a = PIOS_DELAY_GetRaw();
float gyro[3], accel[3], vel[3];
@ -180,7 +183,6 @@ void ins_indoor_update()
if(dT > 0.01)
dT = 0.01;
// format data for INS algo
gyro[0] = gyro_data.filtered.x;
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.Down = Nav.Vel[2];
VelocityActualSet(&velocityActual);
updated_without_gps = 0;
gps_data.updated = false;
} else {
PositionActualData positionActual;
PositionActualGet(&positionActual);
positionActual.North = NAN;
positionActual.East = NAN;
positionActual.Down = Nav.Pos[2];
PositionActualSet(&positionActual);
VelocityActualData velocityActual;
VelocityActualGet(&velocityActual);
velocityActual.North = NAN;
velocityActual.East = NAN;
positionActual.Down = Nav.Pos[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);
}
@ -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);
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 &= (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 */
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];
#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];
#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];
#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];
#endif /* PIOS_COM_AUX */