Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-06 16:58:48 +08:00
Code Issues Actions 26 Packages Projects Releases Wiki Activity

[fixedwing] fix nav_catapult and energy_ctrl to take acceleration of body and not imu x-axis

Browse Source
This commit is contained in:
Felix Ruess
2012-09-25 11:37:24 +02:00
parent 2b7b89a86f
commit 22989e61dd
2 changed files with 6 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/firmwares/fixedwing/guidance/energy_ctrl.c
+3 -3
View File
@@ -237,9 +237,9 @@ void v_ctl_climb_loop( void )
// Actual Acceleration from IMU: attempt to reconstruct the actual kinematic acceleration
#ifndef SITL
struct FloatVect3 accel_float = {0,0,0};
ACCELS_FLOAT_OF_BFP(accel_float, imu.accel);
float vdot = ( accel_float.x / 9.81f - sinf(stateGetNedToBodyEulers_f()->theta) );
struct Int32Vect3 accel_meas_body;
INT32_RMAT_TRANSP_VMULT(accel_meas_body, imu.body_to_imu_rmat, imu.accel);
float vdot = ACCEL_FLOAT_OF_BFP(accel_meas_body.x) / 9.81f - sinf(stateGetNedToBodyEulers_f()->theta);
#else
float vdot = 0;
#endif
sw/airborne/modules/nav/nav_catapult.c
+3 -1
View File
@@ -110,7 +110,9 @@ void nav_catapult_highrate_module(void)
{
// Five consecutive measurements > 1.5
#ifndef SITL
if (ACCEL_FLOAT_OF_BFP(imu.accel.x) < (nav_catapult_acceleration_threshold * 9.81))
struct Int32Vect3 accel_meas_body;
INT32_RMAT_TRANSP_VMULT(accel_meas_body, imu.body_to_imu_rmat, imu.accel);
if (ACCEL_FLOAT_OF_BFP(accel_meas_body.x) < (nav_catapult_acceleration_threshold * 9.81))
#else
if (launch != 1)
#endif