mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-03-15 07:29:15 +01:00
INS: Work on initialization. Improve the zeroing of the gyros at powerup.
This commit is contained in:
parent
dae11cf877
commit
c326c0941d
@ -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
|
||||
|
@ -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)
|
||||
|
@ -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);
|
||||
|
@ -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 -->
|
||||
|
Loading…
x
Reference in New Issue
Block a user