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);
#if USE_IMU_FLOAT
orientationSetEulers_f(&imuf.body_to_imu, &body_to_imu_eulers);
#endif
} 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);
#if USE_IMU_FLOAT
orientationSetEulers_f(&imuf.body_to_imu, &body_to_imu_eulers);
#endif
}
}
/**
* Send a heartbeat
*/
static void mavlink_send_heartbeat(struct transport_tx *trans, struct link_device *dev)
{
uint8_t mav_state = MAV_STATE_CALIBRATING;
uint8_t mav_mode = 0;
#ifdef AP
uint8_t mav_type = MAV_TYPE_FIXED_WING;
switch (pprz_mode) {
case PPRZ_MODE_MANUAL:
mav_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
break;
case PPRZ_MODE_AUTO1:
mav_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
break;
case PPRZ_MODE_AUTO2:
mav_mode |= MAV_MODE_FLAG_AUTO_ENABLED;
break;
case PPRZ_MODE_HOME:
mav_mode |= MAV_MODE_FLAG_GUIDED_ENABLED | MAV_MODE_FLAG_AUTO_ENABLED;
break;
default:
break;
}
#else
uint8_t mav_type = MAV_TYPE_QUADROTOR;
switch (autopilot_mode) {
case AP_MODE_HOME:
mav_mode |= MAV_MODE_FLAG_AUTO_ENABLED;
break;
case AP_MODE_RATE_DIRECT:
case AP_MODE_RATE_RC_CLIMB:
case AP_MODE_RATE_Z_HOLD:
case AP_MODE_RC_DIRECT:
mav_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
break;
case AP_MODE_ATTITUDE_DIRECT:
case AP_MODE_ATTITUDE_CLIMB:
case AP_MODE_ATTITUDE_Z_HOLD:
case AP_MODE_ATTITUDE_RC_CLIMB:
case AP_MODE_HOVER_DIRECT:
case AP_MODE_HOVER_CLIMB:
case AP_MODE_HOVER_Z_HOLD:
case AP_MODE_CARE_FREE_DIRECT:
mav_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
break;
case AP_MODE_NAV:
mav_mode |= MAV_MODE_FLAG_AUTO_ENABLED;
break;
case AP_MODE_GUIDED:
mav_mode = MAV_MODE_FLAG_GUIDED_ENABLED;
default:
break;
}
#endif
if (stateIsAttitudeValid()) {
if (kill_throttle) {
mav_state = MAV_STATE_STANDBY;
} else {
mav_state = MAV_STATE_ACTIVE;
mav_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
}
}
mavlink_msg_heartbeat_send(MAVLINK_COMM_0,
mav_type,
MAV_AUTOPILOT_PPZ,
mav_mode,
0, // custom_mode
mav_state);
MAVLinkSendMessage();
}
static inline int ahrs_is_aligned(void)
{
return stateIsAttitudeValid();
}
