rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-03-21 13:28:58 +01:00
Code Issues Releases Activity

INS: Work on initialization. Improve the zeroing of the gyros at powerup.

Browse Source
This commit is contained in:
James Cotton 2011-09-04 11:49:15 -05:00
parent dae11cf877
commit c326c0941d
4 changed files with 58 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

39
flight/INS/ins.c
View File

@ -73,7 +73,7 @@ void get_accel_gyro_data();
void reset_values();
void measure_noise(void);
void zero_gyros();
/* Communication functions */
//void send_calibration(void);
void send_attitude(void);
@ -138,7 +138,7 @@ float pressure, altitude;
int32_t dr;
static volatile bool init_algorithm = false;
static bool zeroed_gyros = false;
int32_t sclk, sclk_prev;
int32_t sclk_count;
uint32_t loop_time;
@ -198,9 +198,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();
for(uint32_t i = 0; i < 200; i++) {
get_accel_gyro_data(); // This function blocks till data avilable
get_mag_data();
get_baro_data();
PIOS_DELAY_WaituS(TYPICAL_PERIOD);
}
settings_callback(InsSettingsHandle());
ins_init_algorithm();
@ -233,8 +236,13 @@ int main()
continue;
}
//print_ekf_binary();
if(total_conversion_blocks <= 3000 && !zeroed_gyros) {
zero_gyros();
if(total_conversion_blocks == 3000)
zeroed_gyros = true;
PIOS_DELAY_WaituS(TYPICAL_PERIOD);
continue;
}
/* If algorithm changed reinit. This could go in callback but wouldn't be synchronous */
if (init_algorithm) {
ins_init_algorithm();
@ -243,6 +251,9 @@ int main()
time_val2 = PIOS_DELAY_GetRaw();
float zeros[3] = {0,0,0};
INSSetGyroBias(zeros);
switch(ahrs_algorithm) {
case INSSETTINGS_ALGORITHM_SIMPLE:
simple_update();
@ -262,6 +273,11 @@ int main()
// Run at standard rate
while(PIOS_DELAY_DiffuS(time_val1) < TYPICAL_PERIOD);
break;
case INSSETTINGS_ALGORITHM_ZEROGYROS:
zero_gyros();
// Run at standard rate
while(PIOS_DELAY_DiffuS(time_val1) < TYPICAL_PERIOD);
break;
}
status.RunningTimePerCycle = PIOS_DELAY_DiffuS(time_val2);
@ -563,10 +579,15 @@ void measure_noise()
PIOS_DELAY_WaituS(TYPICAL_PERIOD);
calibrate_count++;
}
}
void zero_gyros()
{
const float rate = 1e-2;
gyro_data.calibration.bias[0] += -gyro_data.filtered.x * rate;
gyro_data.calibration.bias[1] += -gyro_data.filtered.y * rate;
gyro_data.calibration.bias[2] += -gyro_data.filtered.z * rate;
}
/**
* @brief Populate fields with initial values

20
flight/INS/insgps13state.c
View File

@ -83,13 +83,29 @@ void INSGPSInit() //pretty much just a place holder for now
for (int i = 0; i < NUMX; i++) {
for (int j = 0; j < NUMX; j++) {
P[i][j] = 0; // zero all terms
F[i][j] = 0;
}
for (int j = 0; j < NUMW; j++)
G[i][j] = 0;
for (int j = 0; j < NUMV; j++) {
H[j][i] = 0;
K[i][j] = 0;
}
X[i] = 0;
}
for (int i = 0; i < NUMW; i++)
Q[i] = 0;
for (int i = 0; i < NUMV; i++)
R[i] = 0;
P[0][0] = P[1][1] = P[2][2] = 25; // initial position variance (m^2)
P[3][3] = P[4][4] = P[5][5] = 5; // initial velocity variance (m/s)^2
P[6][6] = P[7][7] = P[8][8] = P[9][9] = 1e-5; // initial quaternion variance
P[10][10] = P[11][11] = P[12][12] = 1e-5; // initial gyro bias variance (rad/s)^2
P[10][10] = P[11][11] = P[12][12] = 1e-9; // initial gyro bias variance (rad/s)^2
X[0] = X[1] = X[2] = X[3] = X[4] = X[5] = 0; // initial pos and vel (m)
X[6] = 1;
@ -98,7 +114,7 @@ void INSGPSInit() //pretty much just a place holder for now
Q[0] = Q[1] = Q[2] = 50e-8; // gyro noise variance (rad/s)^2
Q[3] = Q[4] = Q[5] = 0.01; // accelerometer noise variance (m/s^2)^2
Q[6] = Q[7] = Q[8] = 2e-9; // gyro bias random walk variance (rad/s^2)^2
Q[6] = Q[7] = Q[8] = 2e-15; // gyro bias random walk variance (rad/s^2)^2
R[0] = R[1] = 0.004; // High freq GPS horizontal position noise variance (m^2)
R[2] = 0.036; // High freq GPS vertical position noise variance (m^2)

13
flight/INS/insgps_helper.c
View File

@ -233,6 +233,7 @@ void ins_indoor_update()
* @brief Initialize the EKF assuming stationary
*/
bool inited = false;
float init_q[4];
void ins_init_algorithm()
{
inited = true;
@ -280,11 +281,15 @@ void ins_init_algorithm()
rpy[1] = asinf(-accels[0]/mag);
rpy[0] = atan2(accels[1]/mag,accels[2]/mag);
rpy[2] = 0;
RPY2Quaternion(rpy,q);
RPY2Quaternion(rpy,init_q);
if (using_gps)
INSSetState(gps_data.NED, zeros, q, zeros, zeros);
else
INSSetState(zeros, zeros, q, zeros, zeros);
INSSetState(gps_data.NED, zeros, init_q, zeros, zeros);
else {
for (uint32_t i = 0; i < 5; i++) {
INSSetState(zeros, zeros, init_q, zeros, zeros);
ins_indoor_update();
}
}
}
INSResetP(Pdiag);

2
shared/uavobjectdefinition/inssettings.xml
View File

@ -1,7 +1,7 @@
<xml>
<object name="InsSettings" singleinstance="true" settings="true">
<description>Settings for the INS to control the algorithm and what is updated</description>
<field name="Algorithm" units="" type="enum" elements="1" options="NONE,SIMPLE,CALIBRATION,INSGPS_INDOOR_NOMAG,INSGPS_INDOOR,INSGPS_OUTDOOR,SENSORRAW" defaultvalue="INSGPS_INDOOR_NOMAG"/>
<field name="Algorithm" units="" type="enum" elements="1" options="NONE,SIMPLE,CALIBRATION,INSGPS_INDOOR_NOMAG,INSGPS_INDOOR,INSGPS_OUTDOOR,SENSORRAW,ZEROGYROS" defaultvalue="INSGPS_INDOOR_NOMAG"/>
<field name="BiasCorrectedRaw" units="" type="enum" elements="1" options="TRUE,FALSE" defaultvalue="TRUE"/>
<!-- Sensor noises -->