自学教程：C++ AP_GPS类代码示例

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

/* @brief Generates pseudorandom ICAO from gps time, lat, and lon. Reference: DO282B, 2.2.4.5.1.3.2 Note gps.time is the number of seconds since UTC midnight*/uint32_t AP_ADSB::genICAO(const Location_Class &loc){    // gps_time is not seconds since UTC midnight, but it is an equivalent random number    // TODO: use UTC time instead of GPS time    AP_GPS gps = _ahrs.get_gps();    const uint64_t gps_time = gps.time_epoch_usec();    uint32_t timeSum = 0;    uint32_t M3 = 4096 * (loc.lat & 0x00000FFF) + (loc.lng & 0x00000FFF);    for (uint8_t i=0; i<24; i++) {        timeSum += (((gps_time & 0x00FFFFFF)>> i) & 0x00000001);    }    return( (timeSum ^ M3) & 0x00FFFFFF);}

/*  send the SYSTEM_TIME message */void GCS_MAVLINK::send_system_time(AP_GPS &gps){    mavlink_msg_system_time_send(        chan,        gps.time_epoch_usec(),        hal.scheduler->millis());}

/*  send the SYSTEM_TIME message */void GCS_MAVLINK::send_system_time(AP_GPS &gps){    mavlink_msg_system_time_send(        chan,        gps.time_epoch_usec(),        AP_HAL::millis());}

void GCS_MAVLINK::handle_gps_inject(const mavlink_message_t *msg, AP_GPS &gps){    mavlink_gps_inject_data_t packet;    mavlink_msg_gps_inject_data_decode(msg, &packet);    //TODO: check target    gps.inject_data(packet.data, packet.len);}

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

/*  Send camera feedback to the GCS */void AP_Camera::send_feedback(mavlink_channel_t chan, AP_GPS &gps, const AP_AHRS &ahrs, const Location &current_loc){    float altitude, altitude_rel;    if (current_loc.flags.relative_alt) {        altitude = current_loc.alt+ahrs.get_home().alt;        altitude_rel = current_loc.alt;    } else {        altitude = current_loc.alt;        altitude_rel = current_loc.alt - ahrs.get_home().alt;    }    mavlink_msg_camera_feedback_send(chan,         gps.time_epoch_usec(),        0, 0, _image_index,        current_loc.lat, current_loc.lng,        altitude/100.0f, altitude_rel/100.0f,        ahrs.roll_sensor/100.0f, ahrs.pitch_sensor/100.0f, ahrs.yaw_sensor/100.0f,        0.0f,CAMERA_FEEDBACK_PHOTO);}

/*  send an initialisation blob to configure the GPS */void AP_GPS_MTK::send_init_blob(uint8_t instance, AP_GPS &gps){    gps.send_blob_start(instance, _initialisation_blob, sizeof(_initialisation_blob));}

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);}

/**   handle a SERIAL_CONTROL message */void GCS_MAVLINK::handle_serial_control(mavlink_message_t *msg, AP_GPS &gps){    mavlink_serial_control_t packet;    mavlink_msg_serial_control_decode(msg, &packet);    AP_HAL::UARTDriver *port = NULL;    if (packet.flags & SERIAL_CONTROL_FLAG_REPLY) {        // how did this packet get to us?        return;    }    bool exclusive = (packet.flags & SERIAL_CONTROL_FLAG_EXCLUSIVE) != 0;    switch (packet.device) {    case SERIAL_CONTROL_DEV_TELEM1:        port = hal.uartC;        lock_channel(MAVLINK_COMM_1, exclusive);        break;    case SERIAL_CONTROL_DEV_TELEM2:        port = hal.uartD;        lock_channel(MAVLINK_COMM_2, exclusive);        break;    case SERIAL_CONTROL_DEV_GPS1:        port = hal.uartB;        gps.lock_port(0, exclusive);        break;    case SERIAL_CONTROL_DEV_GPS2:        port = hal.uartE;        gps.lock_port(1, exclusive);        break;    default:        // not supported yet        return;    }    if (exclusive) {        // force flow control off for exclusive access. This protocol        // is used to talk to bootloaders which may not have flow        // control support        port->set_flow_control(AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE);    }    // optionally change the baudrate    if (packet.baudrate != 0) {        port->begin(packet.baudrate);    }    // write the data    if (packet.count != 0) {        if ((packet.flags & SERIAL_CONTROL_FLAG_BLOCKING) == 0) {            port->write(packet.data, packet.count);        } else {            const uint8_t *data = &packet.data[0];            uint8_t count = packet.count;            while (count > 0) {                while (port->txspace() <= 0) {                    hal.scheduler->delay(5);                }                uint16_t n = port->txspace();                if (n > packet.count) {                    n = packet.count;                }                port->write(data, n);                data += n;                count -= n;            }        }    }    if ((packet.flags & SERIAL_CONTROL_FLAG_RESPOND) == 0) {        // no response expected        return;    }    uint8_t flags = packet.flags;more_data:    // sleep for the timeout    while (packet.timeout != 0 &&            port->available() < (int16_t)sizeof(packet.data)) {        hal.scheduler->delay(1);        packet.timeout--;    }    packet.flags = SERIAL_CONTROL_FLAG_REPLY;    // work out how many bytes are available    int16_t available = port->available();    if (available < 0) {        available = 0;    }    if (available > (int16_t)sizeof(packet.data)) {        available = sizeof(packet.data);    }    // read any reply data//.........这里部分代码省略.........

void setup(void){    serial_manager.init();    ins.init(100);    baro.init();    ahrs.init();    gps.init(NULL, serial_manager);}

void setup(){    hal.console->println("GPS AUTO library test");    hal.console->println("Test Starting");    // Initialise the leds    board_led.init();    // Initialize the UART for GPS system    serial_manager.init();    gps.init(NULL, serial_manager);}

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);}

//to be called only once on boot for initializing objectsvoid setup(){    hal.console->printf("GPS AUTO library test/n");    board_config.init();    // Initialise the leds    board_led.init();    // Initialize the UART for GPS system    serial_manager.init();    gps.init(serial_manager);}

void setup(void){    ins.init(100);    ahrs.init();    serial_manager.init();    if( compass.init() ) {        hal.console->printf("Enabling compass/n");        ahrs.set_compass(&compass);    } else {        hal.console->printf("No compass detected/n");    }    gps.init(NULL, serial_manager);}

void setup(void){#ifdef APM2_HARDWARE    // we need to stop the barometer from holding the SPI bus    hal.gpio->pinMode(40, HAL_HAL_GPIO_OUTPUT);    hal.gpio->write(40, HIGH);#endif    ins.init(AP_InertialSensor::RATE_100HZ);    ahrs.init();    serial_manager.init();    if( compass.init() ) {        hal.console->printf("Enabling compass/n");        ahrs.set_compass(&compass);    } else {        hal.console->printf("No compass detected/n");    }    gps.init(NULL, serial_manager);}