Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

[fixedwing] energy_ctrl: use ABI instead of imu struct #1324

Merged
merged 1 commit into from Sep 16, 2015
Merged

[fixedwing] energy_ctrl: use ABI instead of imu struct #1324

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
42 changes: 36 additions & 6 deletions sw/airborne/firmwares/fixedwing/guidance/energy_ctrl.c
View file
Open in desktop
Expand Up @@ -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 //////

Expand Down Expand Up @@ -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;
Expand All @@ -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

Expand Down Expand Up @@ -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)
{
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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
Expand Down