rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-18 03:52:11 +01:00
Code Issues Releases Activity

INS: In indoor mode if GPS is found use that to populate PositionActual

Browse Source
This commit is contained in:
James Cotton 2011-09-11 22:53:07 -05:00
parent b8bb2dd2ca
commit a682d5deb6
3 changed files with 60 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

37
flight/INS/ins.c
View File

@ -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)
{

48
flight/INS/insgps_helper.c
View File

@ -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

4
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 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 */