/* * 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; }