mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2024-12-01 09:24:10 +01:00
INS: Miscellaneous changes for debugging handling and also some stuff for GPS
signal loss.
This commit is contained in:
James Cotton
parent
5b836db54c
commit
00c9d62f6a
@ -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,7 +260,8 @@ int main()
|
||||
|
||||
time_val2 = PIOS_DELAY_GetRaw();
|
||||
|
||||
//print_ekf_binary(true);
|
||||
print_ekf_binary(true);
|
||||
|
||||
|
||||
switch(ahrs_algorithm) {
|
||||
case INSSETTINGS_ALGORITHM_SIMPLE:
|
||||
@ -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;
|
||||
}
|
||||
}
|
||||
|
||||
@ -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,31 +100,28 @@ 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
|
||||
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
|
||||
@ -135,11 +135,7 @@ void ins_outdoor_update()
|
||||
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;
|
||||
@ -246,20 +248,28 @@ void ins_indoor_update()
|
||||
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);
|
||||
|
||||
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;
|
||||
velocityActual.Down = Nav.Vel[2];
|
||||
} 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();
|
||||
|
||||
@ -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 */
|
||||
|
||||
Loading…
Reference in New Issue
Block a user