/*
* GPS G-frame 5Hhz at > 2400 baud
* LAT LON SPD ALT SAT/FIX
*/
static void ltm_gframe(void)
{
#if defined(GPS)
uint8_t gps_fix_type = 0;
int32_t ltm_alt;
if (!sensors(SENSOR_GPS))
return;
if (!STATE(GPS_FIX))
gps_fix_type = 1;
else if (GPS_numSat < 5)
gps_fix_type = 2;
else
gps_fix_type = 3;
ltm_initialise_packet('G');
ltm_serialise_32(GPS_coord[LAT]);
ltm_serialise_32(GPS_coord[LON]);
ltm_serialise_8((uint8_t)(GPS_speed / 100));
#if defined(BARO) || defined(SONAR)
ltm_alt = (sensors(SENSOR_SONAR) || sensors(SENSOR_BARO)) ? altitudeHoldGetEstimatedAltitude() : GPS_altitude * 100;
#else
ltm_alt = GPS_altitude * 100;
#endif
ltm_serialise_32(ltm_alt);
ltm_serialise_8((GPS_numSat << 2) | gps_fix_type);
ltm_finalise();
#endif
}
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();
}
/*
* GPS G-frame 5Hhz at > 2400 baud
* LAT LON SPD ALT SAT/FIX
*/
static void ltm_gframe(void)
{
uint8_t gps_fix_type = 0;
int32_t ltm_lat = 0, ltm_lon = 0, ltm_alt = 0, ltm_gs = 0;
if (sensors(SENSOR_GPS)) {
if (gpsSol.fixType == GPS_NO_FIX)
gps_fix_type = 1;
else if (gpsSol.fixType == GPS_FIX_2D)
gps_fix_type = 2;
else if (gpsSol.fixType == GPS_FIX_3D)
gps_fix_type = 3;
ltm_lat = gpsSol.llh.lat;
ltm_lon = gpsSol.llh.lon;
ltm_gs = gpsSol.groundSpeed / 100;
}
#if defined(NAV)
ltm_alt = getEstimatedActualPosition(Z); // cm
#else
ltm_alt = sensors(SENSOR_GPS) ? gpsSol.llh.alt : 0; // cm
#endif
ltm_initialise_packet('G');
ltm_serialise_32(ltm_lat);
ltm_serialise_32(ltm_lon);
ltm_serialise_8((uint8_t)ltm_gs);
ltm_serialise_32(ltm_alt);
ltm_serialise_8((gpsSol.numSat << 2) | gps_fix_type);
ltm_finalise();
}
/*
* GPS G-frame 5Hhz at > 2400 baud
* LAT LON SPD ALT SAT/FIX
*/
static void ltm_gframe(void)
{
uint8_t gps_fix_type = 0;
int32_t ltm_alt;
if (!sensors(SENSOR_GPS))
return;
if (gpsSol.fixType == GPS_NO_FIX)
gps_fix_type = 1;
else if (gpsSol.fixType == GPS_FIX_2D)
gps_fix_type = 2;
else if (gpsSol.fixType == GPS_FIX_3D)
gps_fix_type = 3;
ltm_initialise_packet('G');
ltm_serialise_32(gpsSol.llh.lat);
ltm_serialise_32(gpsSol.llh.lon);
ltm_serialise_8((uint8_t)(gpsSol.groundSpeed / 100));
#if defined(NAV)
ltm_alt = (sensors(SENSOR_SONAR) || sensors(SENSOR_BARO)) ? getEstimatedActualPosition(Z) : gpsSol.llh.alt; // cm
#else
ltm_alt = gpsSol.llh.alt; // cm
#endif
ltm_serialise_32(ltm_alt);
ltm_serialise_8((gpsSol.numSat << 2) | gps_fix_type);
ltm_finalise();
}
/*
* GPS G-frame 5Hhz at > 2400 baud
* LAT LON SPD ALT SAT/FIX
*/
static void ltm_gframe(void)
{
#if defined(USE_GPS)
uint8_t gps_fix_type = 0;
int32_t ltm_alt;
if (!sensors(SENSOR_GPS))
return;
if (!STATE(GPS_FIX))
gps_fix_type = 1;
else if (gpsSol.numSat < 5)
gps_fix_type = 2;
else
gps_fix_type = 3;
ltm_initialise_packet('G');
ltm_serialise_32(gpsSol.llh.lat);
ltm_serialise_32(gpsSol.llh.lon);
ltm_serialise_8((uint8_t)(gpsSol.groundSpeed / 100));
#if defined(USE_BARO) || defined(USE_RANGEFINDER)
ltm_alt = (sensors(SENSOR_RANGEFINDER) || sensors(SENSOR_BARO)) ? getEstimatedAltitude() : gpsSol.llh.alt * 100;
#else
ltm_alt = gpsSol.llh.alt * 100;
#endif
ltm_serialise_32(ltm_alt);
ltm_serialise_8((gpsSol.numSat << 2) | gps_fix_type);
ltm_finalise();
#endif
}
/*
* 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();
}
/*
* OSD additional data frame, 1 Hz rate
* This frame will be ignored by Ghettostation, but processed by GhettOSD if it is used as standalone onboard OSD
* home pos, home alt, direction to home
*/
static void ltm_oframe()
{
ltm_initialise_packet('O');
ltm_serialise_32(GPS_home.lat);
ltm_serialise_32(GPS_home.lon);
ltm_serialise_32(GPS_home.alt);
ltm_serialise_8(1); // OSD always ON
ltm_serialise_8(STATE(GPS_FIX_HOME) ? 1 : 0);
ltm_finalise();
}
/** OSD additional data frame, ~4 Hz rate, navigation system status
*/
static void ltm_nframe(void)
{
ltm_initialise_packet('N');
ltm_serialise_8(NAV_Status.mode);
ltm_serialise_8(NAV_Status.state);
ltm_serialise_8(NAV_Status.activeWpAction);
ltm_serialise_8(NAV_Status.activeWpNumber);
ltm_serialise_8(NAV_Status.error);
ltm_serialise_8(NAV_Status.flags);
ltm_finalise();
}
/*
* OSD additional data frame, 1 Hz rate
* This frame will be ignored by Ghettostation, but processed by GhettOSD if it is used as standalone onboard OSD
* home pos, home alt, direction to home
*/
static void ltm_oframe()
{
ltm_initialise_packet('O');
#if defined(GPS)
ltm_serialise_32(GPS_home[LAT]);
ltm_serialise_32(GPS_home[LON]);
#else
ltm_serialise_32(0);
ltm_serialise_32(0);
#endif
ltm_serialise_32(0); // Don't have GPS home altitude
ltm_serialise_8(1); // OSD always ON
ltm_serialise_8(STATE(GPS_FIX_HOME) ? 1 : 0);
ltm_finalise();
}
static void process_ltm(void)
{
static uint8_t ltm_scheduler = 0;
uint8_t current_schedule = ltm_schedule[ltm_scheduler];
sbuf_t ltmPayloadBuf;
sbuf_t *dst = <mPayloadBuf;
if (current_schedule & LTM_BIT_AFRAME) {
ltm_initialise_packet(dst);
ltm_aframe(dst);
ltm_finalise(dst);
}
#if defined(GPS)
if (current_schedule & LTM_BIT_GFRAME) {
ltm_initialise_packet(dst);
ltm_gframe(dst);
ltm_finalise(dst);
}
if (current_schedule & LTM_BIT_OFRAME) {
ltm_initialise_packet(dst);
ltm_oframe(dst);
ltm_finalise(dst);
}
if (current_schedule & LTM_BIT_XFRAME) {
ltm_initialise_packet(dst);
ltm_xframe(dst);
ltm_finalise(dst);
}
#endif
if (current_schedule & LTM_BIT_SFRAME) {
ltm_initialise_packet(dst);
ltm_sframe(dst);
ltm_finalise(dst);
}
#if defined(NAV)
if (current_schedule & LTM_BIT_NFRAME) {
ltm_initialise_packet(dst);
ltm_nframe(dst);
ltm_finalise(dst);
}
#endif
ltm_scheduler = (ltm_scheduler + 1) % 10;
}
