rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-18 08:54:15 +01:00
Code Issues Releases Activity

INS: Change how the initialization of algorithms is handled to ensure sensor

Browse Source 
dat always good
This commit is contained in:
James Cotton 2011-09-02 13:29:05 -05:00
parent c40e5800ab
commit 63f64003e5
2 changed files with 13 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
flight/INS/ins.c
View File

@ -135,13 +135,14 @@ float pressure, altitude;
int32_t dr;
static volatile bool init_algorithm = false;
int32_t sclk, sclk_prev;
int32_t sclk_count;
uint32_t loop_time;
int main()
{
gps_data.quality = -1;
static int8_t last_ahrs_algorithm;
ahrs_algorithm = INSSETTINGS_ALGORITHM_SIMPLE;
reset_values();
@ -195,7 +196,12 @@ int main()
HomeLocationConnectCallback(homelocation_callback);
//FirmwareIAPObjConnectCallback(firmwareiapobj_callback);
get_accel_gyro_data(); // This function blocks till data avilable
get_mag_data();
get_baro_data();
settings_callback(InsSettingsHandle());
ins_init_algorithm();
/******************* Main EKF loop ****************************/
while(1) {
@ -228,9 +234,10 @@ int main()
//print_ekf_binary();
/* If algorithm changed reinit. This could go in callback but wouldn't be synchronous */
if (ahrs_algorithm != last_ahrs_algorithm)
if (init_algorithm) {
ins_init_algorithm();
last_ahrs_algorithm = ahrs_algorithm;
init_algorithm = false;
}
time_val2 = PIOS_DELAY_GetRaw();
@ -645,6 +652,7 @@ void settings_callback(AhrsObjHandle obj)
InsSettingsData settings;
InsSettingsGet(&settings);
init_algorithm = ahrs_algorithm != settings.Algorithm;
ahrs_algorithm = settings.Algorithm;
bias_corrected_raw = settings.BiasCorrectedRaw == INSSETTINGS_BIASCORRECTEDRAW_TRUE;

2
flight/INS/insgps_helper.c
View File

@ -232,8 +232,10 @@ void ins_indoor_update()
/**
* @brief Initialize the EKF assuming stationary
*/
bool inited = false;
void ins_init_algorithm()
{
inited = true;
float Rbe[3][3], q[4], accels[3], rpy[3], mag;
float ge[3]={0,0,-9.81}, zeros[3]={0,0,0}, Pdiag[16]={25,25,25,5,5,5,1e-5,1e-5,1e-5,1e-5,1e-5,1e-5,1e-5,1e-4,1e-4,1e-4};
bool using_mags, using_gps;