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

[fixedwing] energy_ctrl: use ABI instead of imu struct

Browse Source
This commit is contained in:
Felix Ruess
2015-09-01 21:46:25 +02:00
parent b62bc59c67
commit 4bfab9d050
1 changed files with 36 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/firmwares/fixedwing/guidance/energy_ctrl.c
+36 -6
View File
@@ -66,8 +66,7 @@
#include "firmwares/fixedwing/nav.h"
#include "generated/airframe.h"
#include "firmwares/fixedwing/autopilot.h"
#include "subsystems/ahrs.h"
#include "subsystems/imu.h"
#include "subsystems/abi.h"
/////// DEFAULT GUIDANCE_V NECESSITIES //////
@@ -134,6 +133,13 @@ pprz_t v_ctl_throttle_slewed;
float v_ctl_pitch_setpoint;
static struct FloatQuat imu_to_body_quat;
static struct Int32Vect3 accel_imu_meas;
static abi_event accel_ev;
static abi_event body_to_imu_ev;
///////////// DEFAULT SETTINGS ////////////////
#ifndef V_CTL_ALTITUDE_MAX_CLIMB
#define V_CTL_ALTITUDE_MAX_CLIMB 2;
@@ -153,6 +159,10 @@ INFO("V_CTL_GLIDE_RATIO not defined - default is 8.")
#ifndef V_CTL_MAX_ACCELERATION
#define V_CTL_MAX_ACCELERATION 0.5
#endif
#ifndef V_CTL_ENERGY_IMU_ID
#define V_CTL_ENERGY_IMU_ID ABI_BROADCAST
#endif
/////////////////////////////////////////////////
// Automatically found airplane characteristics
@@ -190,6 +200,18 @@ static void ac_char_update(float throttle, float pitch, float climb, float accel
}
}
static void accel_cb(uint8_t sender_id __attribute__((unused)),
uint32_t stamp __attribute__((unused)),
struct Int32Vect3 *accel)
{
accel_imu_meas = *accel;
}
static void body_to_imu_cb(uint8_t sender_id __attribute__((unused)),
struct FloatQuat *q_b2i_f)
{
float_quat_invert(&imu_to_body_quat, q_b2i_f);
}
void v_ctl_init(void)
{
@@ -256,6 +278,11 @@ void v_ctl_init(void)
#endif
v_ctl_throttle_setpoint = 0;
float_quat_identity(&imu_to_body_quat);
AbiBindMsgIMU_ACCEL_INT32(V_CTL_ENERGY_IMU_ID, &accel_ev, accel_cb);
AbiBindMsgBODY_TO_IMU_QUAT(V_CTL_ENERGY_IMU_ID, &body_to_imu_ev, body_to_imu_cb);
}
const float dt_attidude = 1.0 / ((float)CONTROL_FREQUENCY);
@@ -340,10 +367,13 @@ void v_ctl_climb_loop(void)
// Actual Acceleration from IMU: attempt to reconstruct the actual kinematic acceleration
#ifndef SITL
struct Int32Vect3 accel_meas_body;
struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&imu.body_to_imu);
int32_rmat_transp_vmult(&accel_meas_body, body_to_imu_rmat, &imu.accel);
float vdot = ACCEL_FLOAT_OF_BFP(accel_meas_body.x) / 9.81f - sinf(stateGetNedToBodyEulers_f()->theta);
/* convert last imu accel measurement to float */
struct FloatVect3 accel_imu_f;
ACCELS_BFP_OF_REAL(accel_imu_f, accel_imu_meas);
/* rotate from imu to body frame */
struct FloatVect3 accel_meas_body;
float_quat_vmult(&accel_meas_body, &imu_to_body_quat, &accel_imu_f);
float vdot = accel_meas_body.x / 9.81f - sinf(stateGetNedToBodyEulers_f()->theta);
#else
float vdot = 0;
#endif