Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-09 09:28:31 -04:00
Code Issues Packages Projects Releases Wiki Activity

uncrustify ArduCopter/inertia.pde

Browse Source
This commit is contained in:
uncrustify 2012-08-16 17:50:02 -07:00 committed by Pat Hickey
parent 7c8f4421a3
commit a20f75e12c
1 changed files with 58 additions and 58 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

116
ArduCopter/inertia.pde
View File

@ -4,85 +4,85 @@
// Calc 100 hz // Calc 100 hz
void calc_inertia() void calc_inertia()
{ {
// rotate accels based on DCM // rotate accels based on DCM
// -------------------------- // --------------------------
accels_rotated = ahrs.get_dcm_matrix() * imu.get_accel(); 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 accels_rotated.z += 9.805; // remove influence of gravity
// rising = 2 // rising = 2
// neutral = 0 // neutral = 0
// falling = -2 // falling = -2
// ACC Y POS = going EAST // ACC Y POS = going EAST
// ACC X POS = going North // ACC X POS = going North
// ACC Z POS = going DOWN (lets flip this) // ACC Z POS = going DOWN (lets flip this)
// Integrate accels to get the velocity // Integrate accels to get the velocity
// ------------------------------------ // ------------------------------------
Vector3f temp = accels_rotated * (G_Dt * 100); Vector3f temp = accels_rotated * (G_Dt * 100);
temp.z = -temp.z; // Temp is changed to world frame and we can use it normaly temp.z = -temp.z; // Temp is changed to world frame and we can use it normaly
accels_velocity += temp; accels_velocity += temp;
// Integrate velocity to get the Position // Integrate velocity to get the Position
// ------------------------------------ // ------------------------------------
accels_position += accels_velocity * G_Dt; accels_position += accels_velocity * G_Dt;
/* /*
current_loc.lng += accels_velocity.x * G_Dt; * current_loc.lng += accels_velocity.x * G_Dt;
current_loc.lat += accels_velocity.y * G_Dt; * current_loc.lat += accels_velocity.y * G_Dt;
current_loc.alt += accels_velocity.z * G_Dt; * current_loc.alt += accels_velocity.z * G_Dt;
*/ */
} }
void xy_error_correction() void xy_error_correction()
{ {
// Calculate speed error // Calculate speed error
// --------------------- // ---------------------
speed_error.x = x_actual_speed - accels_velocity.x; speed_error.x = x_actual_speed - accels_velocity.x;
speed_error.y = y_actual_speed - accels_velocity.y; speed_error.y = y_actual_speed - accels_velocity.y;
// Calculate position error // Calculate position error
// ------------------------ // ------------------------
//position_error.x = accels_position.x - current_loc.lng; //position_error.x = accels_position.x - current_loc.lng;
//position_error.y = accels_position.y - current_loc.lat; //position_error.y = accels_position.y - current_loc.lat;
// correct integrated velocity by speed_error // correct integrated velocity by speed_error
// this number must be small or we will bring back sensor latency // this number must be small or we will bring back sensor latency
// ------------------------------------------- // -------------------------------------------
accels_velocity.x += speed_error.x * 0.03; // g.speed_correction_x; accels_velocity.x += speed_error.x * 0.03; // g.speed_correction_x;
accels_velocity.y += speed_error.y * 0.03; accels_velocity.y += speed_error.y * 0.03;
// Error correct the accels to deal with calibration, drift and noise // Error correct the accels to deal with calibration, drift and noise
// ------------------------------------------------------------------ // ------------------------------------------------------------------
//accels_position.x -= position_error.x * 0.08; // 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; //accels_position.y -= position_error.y * 0.08; // g.loiter_offset_correction; //.001;
accels_position.x = 0; accels_position.x = 0;
accels_position.y = 0; accels_position.y = 0;
} }
void z_error_correction() void z_error_correction()
{ {
// Calculate speed error // Calculate speed error
// --------------------- // ---------------------
speed_error.z = climb_rate - accels_velocity.z; speed_error.z = climb_rate - accels_velocity.z;
//position_error.z = accels_position.z - current_loc.alt; //position_error.z = accels_position.z - current_loc.alt;
// correct integrated velocity by speed_error // correct integrated velocity by speed_error
// this number must be small or we will bring back sensor latency // this number must be small or we will bring back sensor latency
// ------------------------------------------- // -------------------------------------------
accels_velocity.z += speed_error.z * 0.0350; //speed_correction_z; accels_velocity.z += speed_error.z * 0.0350; //speed_correction_z;
// ------------------------------------------------------------------ // ------------------------------------------------------------------
//accels_position.z -= position_error.z * 0.006; //g.alt_offset_correction; // OK //accels_position.z -= position_error.z * 0.006; //g.alt_offset_correction; // OK
accels_position.z = 0; accels_position.z = 0;
// For developement only // For developement only
// --------------------- // ---------------------
if(motors.armed()) if(motors.armed())
Log_Write_Raw(); Log_Write_Raw();
} }
#endif #endif