aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Arducopter

Browse Source 
Simplified the inertial calcs for now
This commit is contained in:
Jason Short 2012-07-18 22:50:58 -07:00
parent a7b1252b51
commit e09d6ad7a8
1 changed files with 26 additions and 64 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

90
ArduCopter/inertia.pde
View File

@ -7,28 +7,32 @@ 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 += 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
// ------------------------------------
Vector3f temp = accels_rotated * (G_Dt * 100);
temp.z = -temp.z; // Temp is changed to world frame and we can use it normaly
accels_velocity += temp;
// Integrate velocity to get the Position
// ------------------------------------
accels_position += accels_velocity * G_Dt;
/*
current_loc.lng += accels_velocity.x * G_Dt;
current_loc.lat += accels_velocity.y * G_Dt;
current_loc.alt += accels_velocity.z * G_Dt;
*/
}
void xy_error_correction()
@ -38,21 +42,24 @@ void xy_error_correction()
speed_error.x = x_actual_speed - accels_velocity.x;
speed_error.y = y_actual_speed - accels_velocity.y;
// Calculate position error
// ------------------------
//position_error.x = accels_position.x - current_loc.lng;
//position_error.y = accels_position.y - current_loc.lat;
// 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.02; // g.speed_correction_x;
accels_velocity.y += speed_error.y * 0.02;
accels_velocity.x += speed_error.x * 0.03; // g.speed_correction_x;
accels_velocity.y += speed_error.y * 0.03;
// Error correct the accels to deal with calibration, drift and noise
// ------------------------------------------------------------------
accels_position.x -= accels_position.x * 0.03; // g.loiter_offset_correction; //.001;
accels_position.y -= accels_position.y * 0.03; // g.loiter_offset_correction; //.001;
//accels_position.x -= position_error.x * 0.08; // g.loiter_offset_correction; //.001;
//accels_position.y -= position_error.y * 0.08; // g.loiter_offset_correction; //.001;
// update our accel offsets
// -------------------------
accels_offset.x -= accels_position.x * 0.000001; // g.loiter_i_correction;
accels_offset.y -= accels_position.y * 0.000001; // g.loiter_i_correction;
accels_position.x = 0;
accels_position.y = 0;
}
void z_error_correction()
@ -60,6 +67,7 @@ void z_error_correction()
// Calculate speed error
// ---------------------
speed_error.z = climb_rate - accels_velocity.z;
//position_error.z = accels_position.z - current_loc.alt;
// correct integrated velocity by speed_error
// this number must be small or we will bring back sensor latency
@ -67,12 +75,9 @@ void z_error_correction()
accels_velocity.z += speed_error.z * 0.0350; //speed_correction_z;
// ------------------------------------------------------------------
accels_velocity.z -= accels_position.z * 0.006; //g.alt_offset_correction; // OK
// update our accel offsets
// -------------------------
accels_offset.z -= accels_position.z * 0.000003; //g.alt_i_correction ; // .000002;
//accels_position.z -= position_error.z * 0.006; //g.alt_offset_correction; // OK
accels_position.z = 0;
// For developement only
// ---------------------
@ -80,47 +85,4 @@ void z_error_correction()
Log_Write_Raw();
}
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