static void ltm_sframe(void)
{
uint8_t lt_flightmode;
uint8_t lt_statemode;
if (FLIGHT_MODE(PASSTHRU_MODE))
lt_flightmode = 0;
else if (FLIGHT_MODE(GPS_HOME_MODE))
lt_flightmode = 13;
else if (FLIGHT_MODE(GPS_HOLD_MODE))
lt_flightmode = 9;
else if (FLIGHT_MODE(HEADFREE_MODE))
lt_flightmode = 4;
else if (FLIGHT_MODE(BARO_MODE))
lt_flightmode = 8;
else if (FLIGHT_MODE(ANGLE_MODE))
lt_flightmode = 2;
else if (FLIGHT_MODE(HORIZON_MODE))
lt_flightmode = 3;
else
lt_flightmode = 1; // Rate mode
lt_statemode = (ARMING_FLAG(ARMED)) ? 1 : 0;
if (failsafeIsActive())
lt_statemode |= 2;
ltm_initialise_packet('S');
ltm_serialise_16(vbat * 100); //vbat converted to mv
ltm_serialise_16(0); // current, not implemented
ltm_serialise_8((uint8_t)((rssi * 254) / 1023)); // scaled RSSI (uchar)
ltm_serialise_8(0); // no airspeed
ltm_serialise_8((lt_flightmode << 2) | lt_statemode);
ltm_finalise();
}
/*
* Attitude A-frame - 10 Hz at > 2400 baud
* PITCH ROLL HEADING
*/
void ltm_aframe(sbuf_t *dst)
{
sbufWriteU8(dst, 'A');
ltm_serialise_16(dst, DECIDEGREES_TO_DEGREES(attitude.values.pitch));
ltm_serialise_16(dst, DECIDEGREES_TO_DEGREES(attitude.values.roll));
ltm_serialise_16(dst, DECIDEGREES_TO_DEGREES(attitude.values.yaw));
}
void ltm_sframe(sbuf_t *dst)
{
uint8_t lt_flightmode;
if (FLIGHT_MODE(PASSTHRU_MODE))
lt_flightmode = 0;
else if (FLIGHT_MODE(NAV_WP_MODE))
lt_flightmode = 10;
else if (FLIGHT_MODE(NAV_RTH_MODE))
lt_flightmode = 13;
else if (FLIGHT_MODE(NAV_POSHOLD_MODE))
lt_flightmode = 9;
else if (FLIGHT_MODE(HEADFREE_MODE) || FLIGHT_MODE(MAG_MODE))
lt_flightmode = 11;
else if (FLIGHT_MODE(NAV_ALTHOLD_MODE))
lt_flightmode = 8;
else if (FLIGHT_MODE(ANGLE_MODE))
lt_flightmode = 2;
else if (FLIGHT_MODE(HORIZON_MODE))
lt_flightmode = 3;
else
lt_flightmode = 1; // Rate mode
uint8_t lt_statemode = (ARMING_FLAG(ARMED)) ? 1 : 0;
if (failsafeIsActive())
lt_statemode |= 2;
sbufWriteU8(dst, 'S');
ltm_serialise_16(dst, vbat * 100); //vbat converted to mv
ltm_serialise_16(dst, (uint16_t)constrain(mAhDrawn, 0, 0xFFFF)); // current mAh (65535 mAh max)
ltm_serialise_8(dst, (uint8_t)((rssi * 254) / 1023)); // scaled RSSI (uchar)
ltm_serialise_8(dst, 0); // no airspeed
ltm_serialise_8(dst, (lt_flightmode << 2) | lt_statemode);
}
/*
* Attitude A-frame - 10 Hz at > 2400 baud
* PITCH ROLL HEADING
*/
static void ltm_aframe()
{
ltm_initialise_packet('A');
ltm_serialise_16(DECIDEGREES_TO_DEGREES(attitude.values.pitch));
ltm_serialise_16(DECIDEGREES_TO_DEGREES(attitude.values.roll));
ltm_serialise_16(DECIDEGREES_TO_DEGREES(attitude.values.yaw));
ltm_finalise();
}
/*
* Extended information data frame, 1 Hz rate
* This frame is intended to report extended GPS and NAV data, however at the moment it contains only HDOP value
*/
static void ltm_xframe()
{
ltm_initialise_packet('X');
ltm_serialise_16(gpsSol.hdop);
ltm_serialise_8(0);
ltm_serialise_8(0);
ltm_serialise_8(0);
ltm_serialise_8(0);
ltm_finalise();
}
/*
* Extended information data frame, 1 Hz rate
* This frame is intended to report extended GPS and NAV data, however at the moment it contains only HDOP value
*/
void ltm_xframe(sbuf_t *dst)
{
uint8_t sensorStatus =
(isHardwareHealthy() ? 0 : 1) << 0; // bit 0 - hardware failure indication (1 - something is wrong with the hardware sensors)
sbufWriteU8(dst, 'X');
ltm_serialise_16(dst, gpsSol.hdop);
ltm_serialise_8(dst, sensorStatus);
ltm_serialise_8(dst, 0);
ltm_serialise_8(dst, 0);
ltm_serialise_8(dst, 0);
}
