mirror of https://github.com/ArduPilot/ardupilot
115 lines
3.2 KiB
Plaintext
115 lines
3.2 KiB
Plaintext
#if INERTIAL_NAV == ENABLED
|
|
|
|
// generates a new location and velocity in space based on inertia
|
|
// Calc 100 hz
|
|
void calc_inertia()
|
|
{
|
|
// rotate accels based on DCM
|
|
// --------------------------
|
|
accels_rotated = ahrs.get_dcm_matrix() * imu.get_accel();
|
|
accels_rotated += accels_offset; // skew accels to account for long term error using calibration
|
|
accels_rotated.z += 9.805; // remove influence of gravity
|
|
|
|
// rising = 2
|
|
// neutral = 0
|
|
// falling = -2
|
|
|
|
|
|
// ACC Y POS = going EAST
|
|
// ACC X POS = going North
|
|
// ACC Z POS = going DOWN (lets flip this)
|
|
|
|
// Integrate accels to get the velocity
|
|
// ------------------------------------
|
|
Vector3f temp = accels_rotated * (G_Dt * 100);
|
|
temp.z = -temp.z;
|
|
// Temp is changed to world frame and we can use it normaly
|
|
|
|
// Integrate accels to get the velocity
|
|
// ------------------------------------
|
|
accels_velocity += temp;
|
|
}
|
|
|
|
void z_error_correction()
|
|
{
|
|
speed_error.z = climb_rate - accels_velocity.z;
|
|
accels_velocity.z += speed_error.z * 0.0350; //speed_correction_z;
|
|
accels_velocity.z -= g.pid_throttle.get_integrator() * 0.0045; //g.alt_offset_correction; // OK
|
|
accels_offset.z -= g.pid_throttle.get_integrator() * 0.000003; //g.alt_i_correction ; // .000002;
|
|
|
|
// For developement only
|
|
// ---------------------
|
|
if(motors.armed())
|
|
Log_Write_Raw();
|
|
}
|
|
|
|
void xy_error_correction()
|
|
{
|
|
// Calculate speed error
|
|
// ---------------------
|
|
speed_error.x = x_actual_speed - accels_velocity.x;
|
|
speed_error.y = y_actual_speed - accels_velocity.y;
|
|
|
|
// correct integrated velocity by speed_error
|
|
// this number must be small or we will bring back sensor latency
|
|
// -------------------------------------------
|
|
accels_velocity.x += speed_error.x * 0.0175; // g.speed_correction_x;
|
|
accels_velocity.y += speed_error.y * 0.0175;
|
|
|
|
// Error correct the accels to deal with calibration, drift and noise
|
|
// ------------------------------------------------------------------
|
|
accels_velocity.x -= g.pid_loiter_rate_lon.get_integrator() * 0.007; // g.loiter_offset_correction; //.001;
|
|
accels_velocity.y -= g.pid_loiter_rate_lat.get_integrator() * 0.007; // g.loiter_offset_correction; //.001;
|
|
|
|
// update our accel offsets
|
|
// -------------------------
|
|
accels_offset.x -= g.pid_loiter_rate_lon.get_integrator() * 0.000003; // g.loiter_i_correction;
|
|
accels_offset.y -= g.pid_loiter_rate_lat.get_integrator() * 0.000003; // g.loiter_i_correction;
|
|
|
|
|
|
}
|
|
|
|
static void calibrate_accels()
|
|
{
|
|
// sets accels_velocity to 0,0,0
|
|
zero_accels();
|
|
|
|
accels_offset.x = 0;
|
|
accels_offset.y = 0;
|
|
accels_offset.z = 0;
|
|
|
|
for (int i = 0; i < 200; i++){
|
|
delay(10);
|
|
read_AHRS();
|
|
}
|
|
|
|
for (int i = 0; i < 100; i++){
|
|
delay(10);
|
|
read_AHRS();
|
|
calc_inertia();
|
|
Serial.printf("call accels: %1.5f, %1.5f, %1.5f,\n", accels_rotated.x, accels_rotated.y, accels_rotated.z);
|
|
}
|
|
|
|
accels_velocity /= 100;
|
|
accels_offset = accels_velocity;
|
|
zero_accels();
|
|
calc_inertia();
|
|
|
|
Log_Write_Data(25, (float)accels_offset.x);
|
|
Log_Write_Data(26, (float)accels_offset.y);
|
|
Log_Write_Data(27, (float)accels_offset.z);
|
|
}
|
|
|
|
void zero_accels()
|
|
{
|
|
accels_rotated.x = 0;
|
|
accels_rotated.y = 0;
|
|
accels_rotated.z = 0;
|
|
|
|
accels_velocity.x = 0;
|
|
accels_velocity.y = 0;
|
|
accels_velocity.z = 0;
|
|
}
|
|
|
|
|
|
#endif |