From a682d5deb661b77948bf32b1fa87ac7cffffc39f Mon Sep 17 00:00:00 2001 From: James Cotton Date: Sun, 11 Sep 2011 22:53:07 -0500 Subject: [PATCH] INS: In indoor mode if GPS is found use that to populate PositionActual --- flight/INS/ins.c | 37 +++++++++++------------------ flight/INS/insgps_helper.c | 48 ++++++++++++++++++++++++++++++++++---- flight/INS/pios_board.c | 4 ++-- 3 files changed, 60 insertions(+), 29 deletions(-) diff --git a/flight/INS/ins.c b/flight/INS/ins.c index bc472ec6e..688341c13 100644 --- a/flight/INS/ins.c +++ b/flight/INS/ins.c @@ -80,7 +80,6 @@ void zero_gyros(bool update_settings); //void send_calibration(void); void send_attitude(void); void send_velocity(void); -void send_position(void); void homelocation_callback(AhrsObjHandle obj); //void calibration_callback(AhrsObjHandle obj); void settings_callback(AhrsObjHandle obj); @@ -89,7 +88,7 @@ void calibration(float result[3], float scale[3][4], float arg[3]); extern void PIOS_Board_Init(void); void panic(uint32_t blinks); -static void print_ekf_binary(); +static void print_ekf_binary(bool ekf); void simple_update(); /* Bootloader related functions and var*/ @@ -256,9 +255,8 @@ int main() time_val2 = PIOS_DELAY_GetRaw(); - float zeros[3] = {0,0,0}; - INSSetGyroBias(zeros); - + //print_ekf_binary(true); + switch(ahrs_algorithm) { case INSSETTINGS_ALGORITHM_SIMPLE: simple_update(); @@ -274,7 +272,7 @@ int main() measure_noise(); break; case INSSETTINGS_ALGORITHM_SENSORRAW: - print_ekf_binary(); + print_ekf_binary(false); // Run at standard rate while(PIOS_DELAY_DiffuS(time_val1) < TYPICAL_PERIOD); break; @@ -317,7 +315,7 @@ void simple_update() { /** * @brief Output all the important inputs and states of the ekf through serial port */ -static void print_ekf_binary() +static void print_ekf_binary(bool ekf) { static uint32_t timeval; uint16_t delay; @@ -337,9 +335,15 @@ static void print_ekf_binary() PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) &gyro_data.temperature, 4); PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) &accel_data.temperature, 4); PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) &delay, 2); - - mag_data.updated = 0; - altitude_data.updated = 0; + PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & gps_data, sizeof(gps_data)); + + if(ekf) + PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & Nav, sizeof(Nav)); // X (86:149) + else { + mag_data.updated = 0; + altitude_data.updated = 0; + gps_data.updated = 0; + } } void panic(uint32_t blinks) @@ -796,19 +800,6 @@ void send_velocity(void) VelocityActualSet(&velocityActual); } -void send_position(void) -{ - PositionActualData positionActual; - PositionActualGet(&positionActual); - - // convert into cm - positionActual.North = Nav.Pos[0] * 100; - positionActual.East = Nav.Pos[1] * 100; - positionActual.Down = Nav.Pos[2] * 100; - - PositionActualSet(&positionActual); -} - int callback_count = 0; void settings_callback(AhrsObjHandle obj) { diff --git a/flight/INS/insgps_helper.c b/flight/INS/insgps_helper.c index 7d94567cf..4b3f2f25f 100644 --- a/flight/INS/insgps_helper.c +++ b/flight/INS/insgps_helper.c @@ -79,10 +79,22 @@ void ins_outdoor_update() attitude_data.quaternion.q3 = Nav.q[2]; attitude_data.quaternion.q4 = Nav.q[3]; send_attitude(); // get message out quickly - send_velocity(); - send_position(); INSCovariancePrediction(dT); + + PositionActualData positionActual; + PositionActualGet(&positionActual); + positionActual.North = Nav.Pos[0] * 100; + positionActual.East = Nav.Pos[1] * 100; + positionActual.Down = Nav.Pos[2] * 100; + PositionActualSet(&positionActual); + VelocityActualData velocityActual; + VelocityActualGet(&velocityActual); + velocityActual.North = Nav.Vel[0] * 100; + velocityActual.East = Nav.Vel[1] * 100; + velocityActual.Down = Nav.Vel[2] * 100; + VelocityActualSet(&velocityActual); + sensors = 0; /* @@ -183,8 +195,6 @@ void ins_indoor_update() attitude_data.quaternion.q3 = Nav.q[2]; attitude_data.quaternion.q4 = Nav.q[3]; send_attitude(); // get message out quickly - send_velocity(); - send_position(); INSCovariancePrediction(dT); /* Indoors, update with zero position and velocity and high covariance */ @@ -221,6 +231,36 @@ void ins_indoor_update() altitude_data.updated = false; } + if(gps_data.updated) { + PositionActualData positionActual; + PositionActualGet(&positionActual); + positionActual.North = gps_data.NED[0] * 100; + positionActual.East = gps_data.NED[1] * 100; + positionActual.Down = Nav.Pos[2] * 100; + PositionActualSet(&positionActual); + + VelocityActualData velocityActual; + VelocityActualGet(&velocityActual); + velocityActual.North = gps_data.groundspeed * cos(gps_data.heading * DEG_TO_RAD); + velocityActual.East = gps_data.groundspeed * sin(gps_data.heading * DEG_TO_RAD); + velocityActual.Down = Nav.Vel[2] * 100; + VelocityActualSet(&velocityActual); + + gps_data.updated = false; + } else { + PositionActualData positionActual; + PositionActualGet(&positionActual); + positionActual.Down = Nav.Pos[2] * 100; + PositionActualSet(&positionActual); + + VelocityActualData velocityActual; + VelocityActualGet(&velocityActual); + velocityActual.North = 0; + velocityActual.East = 0; + velocityActual.Down = Nav.Vel[2] * 100; + VelocityActualSet(&velocityActual); + } + /* * TODO: Need to add a general sanity check for all the inputs to make sure their kosher * although probably should occur within INS itself diff --git a/flight/INS/pios_board.c b/flight/INS/pios_board.c index 4fec2421b..e0d0cfd63 100644 --- a/flight/INS/pios_board.c +++ b/flight/INS/pios_board.c @@ -329,9 +329,9 @@ static const struct pios_usart_cfg pios_usart_gps_cfg = { }, }; -#define PIOS_COM_AUX_TX_BUF_LEN 255 +#define PIOS_COM_AUX_TX_BUF_LEN 512 static uint8_t pios_com_aux_tx_buffer[PIOS_COM_AUX_TX_BUF_LEN]; -#define PIOS_COM_AUX_RX_BUF_LEN 255 +#define PIOS_COM_AUX_RX_BUF_LEN 512 static uint8_t pios_com_aux_rx_buffer[PIOS_COM_AUX_RX_BUF_LEN]; #endif /* PIOS_INCLUDE_GPS */