Ejemplo n.º 1
0
/*
 * 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();
}
Ejemplo n.º 2
0
/*
 * 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
}
Ejemplo n.º 3
0
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();
}
Ejemplo n.º 4
0
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);
}
Ejemplo n.º 5
0
/*
 * 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
}
Ejemplo n.º 6
0
Archivo: ltm.c Proyecto: damdam2/inav
/*
 * 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();
}
Ejemplo n.º 7
0
/*
 * 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
 */
void ltm_oframe(sbuf_t *dst)
{
    sbufWriteU8(dst, 'O');
    ltm_serialise_32(dst, GPS_home.lat);
    ltm_serialise_32(dst, GPS_home.lon);
    ltm_serialise_32(dst, GPS_home.alt);
    ltm_serialise_8(dst, 1);                 // OSD always ON
    ltm_serialise_8(dst, STATE(GPS_FIX_HOME) ? 1 : 0);
}
Ejemplo n.º 8
0
/*
 * 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();
}
Ejemplo n.º 9
0
/*
 * 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();
}
Ejemplo n.º 10
0
/*
 * 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);
}
Ejemplo n.º 11
0
/*
 * 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();
}
Ejemplo n.º 12
0
static void ltm_serialise_32(uint32_t v)
{
    ltm_serialise_8((uint8_t)v);
    ltm_serialise_8((v >> 8));
    ltm_serialise_8((v >> 16));
    ltm_serialise_8((v >> 24));
}
Ejemplo n.º 13
0
static void ltm_serialise_32(sbuf_t *dst, uint32_t v)
{
    ltm_serialise_8(dst, (uint8_t)v);
    ltm_serialise_8(dst, (v >> 8));
    ltm_serialise_8(dst, (v >> 16));
    ltm_serialise_8(dst, (v >> 24));
}
Ejemplo n.º 14
0
/** OSD additional data frame, ~4 Hz rate, navigation system status
 */
void ltm_nframe(sbuf_t *dst)
{
    sbufWriteU8(dst, 'N');
    ltm_serialise_8(dst, NAV_Status.mode);
    ltm_serialise_8(dst, NAV_Status.state);
    ltm_serialise_8(dst, NAV_Status.activeWpAction);
    ltm_serialise_8(dst, NAV_Status.activeWpNumber);
    ltm_serialise_8(dst, NAV_Status.error);
    ltm_serialise_8(dst, NAV_Status.flags);
}
Ejemplo n.º 15
0
/** 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();
}
Ejemplo n.º 16
0
static void ltm_serialise_16(uint16_t v)
{
    ltm_serialise_8((uint8_t)v);
    ltm_serialise_8((v >> 8));
}
Ejemplo n.º 17
0
static void ltm_serialise_16(sbuf_t *dst, uint16_t v)
{
    ltm_serialise_8(dst, (uint8_t)v);
    ltm_serialise_8(dst,  (v >> 8));
}