C++ AP_GPS::status方法代码示例

/*
  send raw GPS position information (GPS_RAW_INT, GPS2_RAW, GPS_RTK and GPS2_RTK).
  returns true if messages fit into transmit buffer, false otherwise.
 */
bool GCS_MAVLINK::send_gps_raw(AP_GPS &gps)
{
    if (comm_get_txspace(chan) >=
            MAVLINK_MSG_ID_GPS_RAW_INT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) {
        gps.send_mavlink_gps_raw(chan);
    } else {
        return false;
    }

    if (gps.highest_supported_status(0) > AP_GPS::GPS_OK_FIX_3D) {
        if (comm_get_txspace(chan) >= MAVLINK_NUM_NON_PAYLOAD_BYTES+MAVLINK_MSG_ID_GPS_RTK_LEN) {
            gps.send_mavlink_gps_rtk(chan);
        }

    }

    if (gps.num_sensors() > 1 && gps.status(1) > AP_GPS::NO_GPS) {

        if (comm_get_txspace(chan) >= MAVLINK_NUM_NON_PAYLOAD_BYTES+MAVLINK_MSG_ID_GPS2_RAW_LEN) {
            gps.send_mavlink_gps2_raw(chan);
        }

        if (gps.highest_supported_status(1) > AP_GPS::GPS_OK_FIX_3D) {
            if (comm_get_txspace(chan) >=
                    MAVLINK_NUM_NON_PAYLOAD_BYTES+MAVLINK_MSG_ID_GPS2_RTK_LEN) {
                gps.send_mavlink_gps2_rtk(chan);
            }
        }
    }

    //TODO: Should check what else managed to get through...
    return true;

}

/*
  send raw GPS position information (GPS_RAW_INT, GPS2_RAW, GPS_RTK and GPS2_RTK).
  returns true if messages fit into transmit buffer, false otherwise.
 */
bool GCS_MAVLINK::send_gps_raw(AP_GPS &gps)
{

    int16_t payload_space = comm_get_txspace(chan) - MAVLINK_NUM_NON_PAYLOAD_BYTES;
    if (payload_space >= MAVLINK_MSG_ID_GPS_RAW_INT_LEN) {
        gps.send_mavlink_gps_raw(chan);
    } else {
        return false;
    }

#if GPS_RTK_AVAILABLE
    if (gps.highest_supported_status(0) > AP_GPS::GPS_OK_FIX_3D) {
        payload_space = comm_get_txspace(chan) - MAVLINK_NUM_NON_PAYLOAD_BYTES;
        if (payload_space >= MAVLINK_MSG_ID_GPS_RTK_LEN) {
            gps.send_mavlink_gps_rtk(chan);
        }

    }
#endif

#if GPS_MAX_INSTANCES > 1

    if (gps.num_sensors() > 1 && gps.status(1) > AP_GPS::NO_GPS) {

        payload_space = comm_get_txspace(chan) - MAVLINK_NUM_NON_PAYLOAD_BYTES;
        if (payload_space >= MAVLINK_MSG_ID_GPS2_RAW_LEN) {
            gps.send_mavlink_gps2_raw(chan);
        }

#if GPS_RTK_AVAILABLE
        if (gps.highest_supported_status(1) > AP_GPS::GPS_OK_FIX_3D) {
            payload_space = comm_get_txspace(chan) - MAVLINK_NUM_NON_PAYLOAD_BYTES;
            if (payload_space >= MAVLINK_MSG_ID_GPS2_RTK_LEN) {
                gps.send_mavlink_gps2_rtk(chan);
            }
        }
#endif
    }
#endif

    //TODO: Should check what else managed to get through...
    return true;

}

void loop()
{
    static uint32_t last_msg_ms;

    // Update GPS state based on possible bytes received from the module.
    gps.update();

    // If new GPS data is received, output it's contents to the console
    // Here we rely on the time of the message in GPS class and the time of last message
    // saved in static variable last_msg_ms. When new message is received, the time
    // in GPS class will be updated.
    if (last_msg_ms != gps.last_message_time_ms()) {
        // Reset the time of message
        last_msg_ms = gps.last_message_time_ms();

        // Acquire location
        const Location &loc = gps.location();

        // Print the contents of message
        hal.console->print("Lat: ");
        print_latlon(hal.console, loc.lat);
        hal.console->print(" Lon: ");
        print_latlon(hal.console, loc.lng);
        hal.console->printf(" Alt: %.2fm GSP: %.2fm/s CoG: %d SAT: %d TIM: %u/%lu STATUS: %u\n",
                            loc.alt * 0.01f,
                            gps.ground_speed(),
                            (int)gps.ground_course_cd() / 100,
                            gps.num_sats(),
                            gps.time_week(),
                            (unsigned long)gps.time_week_ms(),
                            gps.status());
    }
    else
    {
        hal.console->println("It seemed like NO GPS");        
    }

    // Delay for 10 mS will give us 100 Hz invocation rate
    hal.scheduler->delay(10);
}

void AP_ADSB::send_dynamic_out(const mavlink_channel_t chan)
{
    // --------------
    // Knowns
    AP_GPS gps = _ahrs.get_gps();
    Vector3f gps_velocity = gps.velocity();

    int32_t latitude = _my_loc.lat;
    int32_t longitude = _my_loc.lng;
    int32_t altGNSS = _my_loc.alt*0.1f; // convert cm to mm
    int16_t velVert = gps_velocity.z * 1E2; // convert m/s to cm/s
    int16_t nsVog = gps_velocity.x * 1E2; // convert m/s to cm/s
    int16_t ewVog = gps_velocity.y * 1E2; // convert m/s to cm/s
    uint8_t fixType = gps.status(); // this lines up perfectly with our enum
    uint8_t emStatus = 0; // TODO: implement this ENUM. no emergency = 0
    uint8_t numSats = gps.num_sats();
    uint16_t squawk = 1200; // Mode A code (typically 1200 [0x04B0] for VFR)

    uint32_t accHoriz = UINT_MAX;
    float accHoriz_f;
    if (gps.horizontal_accuracy(accHoriz_f)) {
        accHoriz = accHoriz_f * 1E3; // convert m to mm
    }

    uint16_t accVert = USHRT_MAX;
    float accVert_f;
    if (gps.vertical_accuracy(accVert_f)) {
        accVert = accVert_f * 1E2; // convert m to cm
    }

    uint16_t accVel = USHRT_MAX;
    float accVel_f;
    if (gps.speed_accuracy(accVel_f)) {
        accVel = accVel_f * 1E3; // convert m/s to mm/s
    }

    uint16_t state = 0;
    if (out_state._is_in_auto_mode) {
        state |= UAVIONIX_ADSB_OUT_DYNAMIC_STATE_AUTOPILOT_ENABLED;
    }
    if (!out_state.is_flying) {
        state |= UAVIONIX_ADSB_OUT_DYNAMIC_STATE_ON_GROUND;
    }



    // --------------
    // Not Sure
    uint32_t utcTime = UINT_MAX; //    uint32_t utcTime,
    // TODO: confirm this sets utcTime correctly
    const uint64_t gps_time = gps.time_epoch_usec();
    utcTime = gps_time / 1000000ULL;



    // --------------
    // Unknowns
    // TODO: implement http://www.srh.noaa.gov/images/epz/wxcalc/pressureAltitude.pdf
    int32_t altPres = INT_MAX; //_ahrs.get_baro().get_altitude() relative to home, not MSL



    mavlink_msg_uavionix_adsb_out_dynamic_send(
            chan,
            utcTime,
            latitude,
            longitude,
            altGNSS,
            fixType,
            numSats,
            altPres,
            accHoriz,
            accVert,
            accVel,
            velVert,
            nsVog,
            ewVog,
            emStatus,
            state,
            squawk);
}