void imu_SetBodyToImuCurrent(float set)
{
imu.b2i_set_current = set;
if (imu.b2i_set_current) {
// adjust imu_to_body roll and pitch by current NedToBody roll and pitch
struct FloatEulers body_to_imu_eulers;
memcpy(&body_to_imu_eulers, orientationGetEulers_f(&imu.body_to_imu), sizeof(struct FloatEulers));
if (stateIsAttitudeValid()) {
// adjust imu_to_body roll and pitch by current NedToBody roll and pitch
body_to_imu_eulers.phi += stateGetNedToBodyEulers_f()->phi;
body_to_imu_eulers.theta += stateGetNedToBodyEulers_f()->theta;
orientationSetEulers_f(&imu.body_to_imu, &body_to_imu_eulers);
AbiSendMsgBODY_TO_IMU_QUAT(1, orientationGetQuat_f(&imu.body_to_imu));
} else {
// indicate that we couldn't set to current roll/pitch
imu.b2i_set_current = FALSE;
}
} else {
// reset to BODY_TO_IMU as defined in airframe file
struct FloatEulers body_to_imu_eulers =
{IMU_BODY_TO_IMU_PHI, IMU_BODY_TO_IMU_THETA, IMU_BODY_TO_IMU_PSI};
orientationSetEulers_f(&imu.body_to_imu, &body_to_imu_eulers);
AbiSendMsgBODY_TO_IMU_QUAT(1, orientationGetQuat_f(&imu.body_to_imu));
}
}
void ahrs_fc_realign_heading(float heading)
{
FLOAT_ANGLE_NORMALIZE(heading);
/* quaternion representing only the heading rotation from ltp to body */
struct FloatQuat q_h_new;
q_h_new.qx = 0.0;
q_h_new.qy = 0.0;
q_h_new.qz = sinf(heading / 2.0);
q_h_new.qi = cosf(heading / 2.0);
struct FloatQuat *body_to_imu_quat = orientationGetQuat_f(&ahrs_fc.body_to_imu);
struct FloatQuat ltp_to_body_quat;
float_quat_comp_inv(<p_to_body_quat, &ahrs_fc.ltp_to_imu_quat, body_to_imu_quat);
/* quaternion representing current heading only */
struct FloatQuat q_h;
QUAT_COPY(q_h, ltp_to_body_quat);
q_h.qx = 0.;
q_h.qy = 0.;
float_quat_normalize(&q_h);
/* quaternion representing rotation from current to new heading */
struct FloatQuat q_c;
float_quat_inv_comp_norm_shortest(&q_c, &q_h, &q_h_new);
/* correct current heading in body frame */
struct FloatQuat q;
float_quat_comp_norm_shortest(&q, &q_c, <p_to_body_quat);
/* compute ltp to imu rotation quaternion and matrix */
float_quat_comp(&ahrs_fc.ltp_to_imu_quat, &q, body_to_imu_quat);
float_rmat_of_quat(&ahrs_fc.ltp_to_imu_rmat, &ahrs_fc.ltp_to_imu_quat);
ahrs_fc.heading_aligned = TRUE;
}
static inline void main_init(void)
{
mcu_init();
sys_time_register_timer((1. / PERIODIC_FREQUENCY), NULL);
stateInit();
actuators_init();
imu_init();
#if USE_AHRS_ALIGNER
ahrs_aligner_init();
#endif
ahrs_init();
settings_init();
mcu_int_enable();
downlink_init();
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_AUTOPILOT_VERSION, send_autopilot_version);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ALIVE, send_alive);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_COMMANDS, send_commands);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ACTUATORS, send_actuators);
// send body_to_imu from here for now
AbiSendMsgBODY_TO_IMU_QUAT(1, orientationGetQuat_f(&imu.body_to_imu));
}
static void send_att(struct transport_tx *trans, struct link_device *dev)
{
/* compute eulers in int (IMU frame) */
struct FloatEulers ltp_to_imu_euler;
float_eulers_of_quat(<p_to_imu_euler, &ahrs_float_inv.state.quat);
struct Int32Eulers eulers_imu;
EULERS_BFP_OF_REAL(eulers_imu, ltp_to_imu_euler);
/* compute Eulers in int (body frame) */
struct FloatQuat ltp_to_body_quat;
struct FloatQuat *body_to_imu_quat = orientationGetQuat_f(&ahrs_float_inv.body_to_imu);
float_quat_comp_inv(<p_to_body_quat, &ahrs_float_inv.state.quat, body_to_imu_quat);
struct FloatEulers ltp_to_body_euler;
float_eulers_of_quat(<p_to_body_euler, <p_to_body_quat);
struct Int32Eulers eulers_body;
EULERS_BFP_OF_REAL(eulers_body, ltp_to_body_euler);
pprz_msg_send_AHRS_EULER_INT(trans, dev, AC_ID,
&eulers_imu.phi,
&eulers_imu.theta,
&eulers_imu.psi,
&eulers_body.phi,
&eulers_body.theta,
&eulers_body.psi,
&ahrs_finv_id);
}
void imu_SetBodyToImuPsi(float psi)
{
struct FloatEulers body_to_imu_eulers;
memcpy(&body_to_imu_eulers, orientationGetEulers_f(&imu.body_to_imu), sizeof(struct FloatEulers));
body_to_imu_eulers.psi = psi;
orientationSetEulers_f(&imu.body_to_imu, &body_to_imu_eulers);
AbiSendMsgBODY_TO_IMU_QUAT(1, orientationGetQuat_f(&imu.body_to_imu));
}
void ahrs_fc_set_body_to_imu_quat(struct FloatQuat *q_b2i)
{
orientationSetQuat_f(&ahrs_fc.body_to_imu, q_b2i);
if (!ahrs_fc.is_aligned) {
/* Set ltp_to_imu so that body is zero */
ahrs_fc.ltp_to_imu_quat = *orientationGetQuat_f(&ahrs_fc.body_to_imu);
ahrs_fc.ltp_to_imu_rmat = *orientationGetRMat_f(&ahrs_fc.body_to_imu);
}
}
void ahrs_init(void) {
ahrs.status = AHRS_UNINIT;
/* set ltp_to_imu so that body is zero */
struct FloatQuat *body_to_imu_quat = orientationGetQuat_f(&imuf.body_to_imu);
QUAT_COPY(ahrs_impl.ltp_to_imu_quat, *body_to_imu_quat);
#ifdef IMU_MAG_OFFSET
ahrs_impl.mag_offset = IMU_MAG_OFFSET;
#else
ahrs_impl.mag_offset = 0.0;
#endif
ahrs_aligner.status = AHRS_ALIGNER_LOCKED;
}
/**
* Compute body orientation and rates from imu orientation and rates
*/
static inline void compute_body_orientation_and_rates(void) {
/* Compute LTP to BODY quaternion */
struct FloatQuat ltp_to_body_quat;
struct FloatQuat *body_to_imu_quat = orientationGetQuat_f(&imu.body_to_imu);
float_quat_comp_inv(<p_to_body_quat, &ahrs_impl.ltp_to_imu_quat, body_to_imu_quat);
/* Set state */
stateSetNedToBodyQuat_f(<p_to_body_quat);
/* compute body rates */
struct FloatRates body_rate;
struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&imu.body_to_imu);
float_rmat_transp_ratemult(&body_rate, body_to_imu_rmat, &ahrs_impl.imu_rate);
stateSetBodyRates_f(&body_rate);
}
void ahrs_fc_update_heading(float heading)
{
FLOAT_ANGLE_NORMALIZE(heading);
struct FloatQuat *body_to_imu_quat = orientationGetQuat_f(&ahrs_fc.body_to_imu);
struct FloatQuat ltp_to_body_quat;
float_quat_comp_inv(<p_to_body_quat, &ahrs_fc.ltp_to_imu_quat, body_to_imu_quat);
struct FloatRMat ltp_to_body_rmat;
float_rmat_of_quat(<p_to_body_rmat, <p_to_body_quat);
// row 0 of ltp_to_body_rmat = body x-axis in ltp frame
// we only consider x and y
struct FloatVect2 expected_ltp = {
RMAT_ELMT(ltp_to_body_rmat, 0, 0),
RMAT_ELMT(ltp_to_body_rmat, 0, 1)
};
// expected_heading cross measured_heading
struct FloatVect3 residual_ltp = {
0,
0,
expected_ltp.x * sinf(heading) - expected_ltp.y * cosf(heading)
};
struct FloatVect3 residual_imu;
float_rmat_vmult(&residual_imu, &ahrs_fc.ltp_to_imu_rmat, &residual_ltp);
const float heading_rate_update_gain = 2.5;
RATES_ADD_SCALED_VECT(ahrs_fc.rate_correction, residual_imu, heading_rate_update_gain);
float heading_bias_update_gain;
/* crude attempt to only update bias if deviation is small
* e.g. needed when you only have gps providing heading
* and the inital heading is totally different from
* the gps course information you get once you have a gps fix.
* Otherwise the bias will be falsely "corrected".
*/
if (fabs(residual_ltp.z) < sinf(RadOfDeg(5.))) {
heading_bias_update_gain = -2.5e-4;
} else {
heading_bias_update_gain = 0.0;
}
RATES_ADD_SCALED_VECT(ahrs_fc.gyro_bias, residual_imu, heading_bias_update_gain);
}
void ahrs_init(void) {
ahrs.status = AHRS_UNINIT;
ahrs_impl.ltp_vel_norm_valid = FALSE;
ahrs_impl.heading_aligned = FALSE;
/* Set ltp_to_imu so that body is zero */
memcpy(&ahrs_impl.ltp_to_imu_quat, orientationGetQuat_f(&imu.body_to_imu),
sizeof(struct FloatQuat));
memcpy(&ahrs_impl.ltp_to_imu_rmat, orientationGetRMat_f(&imu.body_to_imu),
sizeof(struct FloatRMat));
FLOAT_RATES_ZERO(ahrs_impl.imu_rate);
/* set default filter cut-off frequency and damping */
ahrs_impl.accel_omega = AHRS_ACCEL_OMEGA;
ahrs_impl.accel_zeta = AHRS_ACCEL_ZETA;
ahrs_impl.mag_omega = AHRS_MAG_OMEGA;
ahrs_impl.mag_zeta = AHRS_MAG_ZETA;
#if AHRS_GRAVITY_UPDATE_COORDINATED_TURN
ahrs_impl.correct_gravity = TRUE;
#else
ahrs_impl.correct_gravity = FALSE;
#endif
ahrs_impl.gravity_heuristic_factor = AHRS_GRAVITY_HEURISTIC_FACTOR;
VECT3_ASSIGN(ahrs_impl.mag_h, AHRS_H_X, AHRS_H_Y, AHRS_H_Z);
ahrs_impl.accel_cnt = 0;
ahrs_impl.mag_cnt = 0;
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, "AHRS_EULER_INT", send_att);
register_periodic_telemetry(DefaultPeriodic, "GEO_MAG", send_geo_mag);
#endif
}
void ahrs_fc_set_body_to_imu(struct OrientationReps *body_to_imu)
{
ahrs_fc_set_body_to_imu_quat(orientationGetQuat_f(body_to_imu));
}
STATIC_INLINE void main_init(void)
{
mcu_init();
electrical_init();
stateInit();
actuators_init();
#if USE_MOTOR_MIXING
motor_mixing_init();
#endif
radio_control_init();
#if USE_BARO_BOARD
baro_init();
#endif
#if USE_IMU
imu_init();
#endif
#if USE_AHRS_ALIGNER
ahrs_aligner_init();
#endif
#if USE_AHRS
ahrs_init();
#endif
ins_init();
#if USE_GPS
gps_init();
#endif
autopilot_init();
modules_init();
settings_init();
mcu_int_enable();
#if DOWNLINK
downlink_init();
#endif
// register the timers for the periodic functions
main_periodic_tid = sys_time_register_timer((1. / PERIODIC_FREQUENCY), NULL);
modules_tid = sys_time_register_timer(1. / MODULES_FREQUENCY, NULL);
radio_control_tid = sys_time_register_timer((1. / 60.), NULL);
failsafe_tid = sys_time_register_timer(0.05, NULL);
electrical_tid = sys_time_register_timer(0.1, NULL);
telemetry_tid = sys_time_register_timer((1. / TELEMETRY_FREQUENCY), NULL);
#if USE_BARO_BOARD
baro_tid = sys_time_register_timer(1. / BARO_PERIODIC_FREQUENCY, NULL);
#endif
#if USE_IMU
// send body_to_imu from here for now
AbiSendMsgBODY_TO_IMU_QUAT(1, orientationGetQuat_f(&imu.body_to_imu));
#endif
// Do a failsafe check first
failsafe_check();
}
void init_ap(void)
{
#ifndef SINGLE_MCU /** init done in main_fbw in single MCU */
mcu_init();
#endif /* SINGLE_MCU */
/** - start interrupt task */
mcu_int_enable();
#if defined(PPRZ_TRIG_INT_COMPR_FLASH)
pprz_trig_int_init();
#endif
/****** initialize and reset state interface ********/
stateInit();
/************* Sensors initialization ***************/
#if USE_AHRS
ahrs_init();
#endif
#if USE_BARO_BOARD
baro_init();
#endif
/************* Links initialization ***************/
#if defined MCU_SPI_LINK || defined MCU_UART_LINK || defined MCU_CAN_LINK
link_mcu_init();
#endif
/************ Internal status ***************/
autopilot_init();
modules_init();
// call autopilot implementation init after guidance modules init
// it will set startup mode
#if USE_GENERATED_AUTOPILOT
autopilot_generated_init();
#else
autopilot_static_init();
#endif
settings_init();
/**** start timers for periodic functions *****/
sensors_tid = sys_time_register_timer(1. / PERIODIC_FREQUENCY, NULL);
#if !USE_GENERATED_AUTOPILOT
navigation_tid = sys_time_register_timer(1. / NAVIGATION_FREQUENCY, NULL);
#endif
attitude_tid = sys_time_register_timer(1. / CONTROL_FREQUENCY, NULL);
modules_tid = sys_time_register_timer(1. / MODULES_FREQUENCY, NULL);
telemetry_tid = sys_time_register_timer(1. / TELEMETRY_FREQUENCY, NULL);
monitor_tid = sys_time_register_timer(1.0, NULL);
#if USE_BARO_BOARD
baro_tid = sys_time_register_timer(1. / BARO_PERIODIC_FREQUENCY, NULL);
#endif
#if DOWNLINK
downlink_init();
#endif
/* set initial trim values.
* these are passed to fbw via inter_mcu.
*/
PPRZ_MUTEX_LOCK(ap_state_mtx);
ap_state->command_roll_trim = COMMAND_ROLL_TRIM;
ap_state->command_pitch_trim = COMMAND_PITCH_TRIM;
ap_state->command_yaw_trim = COMMAND_YAW_TRIM;
PPRZ_MUTEX_UNLOCK(ap_state_mtx);
#if USE_IMU
// send body_to_imu from here for now
AbiSendMsgBODY_TO_IMU_QUAT(1, orientationGetQuat_f(&imu.body_to_imu));
#endif
}
STATIC_INLINE void main_init(void)
{
mcu_init();
#if defined(PPRZ_TRIG_INT_COMPR_FLASH)
pprz_trig_int_init();
#endif
electrical_init();
stateInit();
#ifndef INTER_MCU_AP
actuators_init();
#else
intermcu_init();
#endif
#if USE_MOTOR_MIXING
motor_mixing_init();
#endif
#ifndef INTER_MCU_AP
radio_control_init();
#endif
#if USE_BARO_BOARD
baro_init();
#endif
#if USE_AHRS_ALIGNER
ahrs_aligner_init();
#endif
#if USE_AHRS
ahrs_init();
#endif
autopilot_init();
modules_init();
/* temporary hack:
* Since INS is now a module, LTP_DEF is not yet initialized when autopilot_init is called
* This led to the problem that global waypoints were not "localized",
* so as a stop-gap measure we localize them all (again) here..
*/
waypoints_localize_all();
settings_init();
mcu_int_enable();
#if DOWNLINK
downlink_init();
#endif
#ifdef INTER_MCU_AP
intermcu_init();
#endif
// register the timers for the periodic functions
main_periodic_tid = sys_time_register_timer((1. / PERIODIC_FREQUENCY), NULL);
#if PERIODIC_FREQUENCY != MODULES_FREQUENCY
modules_tid = sys_time_register_timer(1. / MODULES_FREQUENCY, NULL);
#endif
radio_control_tid = sys_time_register_timer((1. / 60.), NULL);
failsafe_tid = sys_time_register_timer(0.05, NULL);
electrical_tid = sys_time_register_timer(0.1, NULL);
telemetry_tid = sys_time_register_timer((1. / TELEMETRY_FREQUENCY), NULL);
#if USE_BARO_BOARD
baro_tid = sys_time_register_timer(1. / BARO_PERIODIC_FREQUENCY, NULL);
#endif
#if USE_IMU
// send body_to_imu from here for now
AbiSendMsgBODY_TO_IMU_QUAT(1, orientationGetQuat_f(&imu.body_to_imu));
#endif
// Do a failsafe check first
failsafe_check();
}
