示例#1
0
bool
AP_GPS_GSOF::process_message(void)
{
    //http://www.trimble.com/OEM_ReceiverHelp/V4.81/en/default.html#welcome.html

    if (gsof_msg.packettype == 0x40) { // GSOF
#if gsof_DEBUGGING
        uint8_t trans_number = gsof_msg.data[0];
        uint8_t pageidx = gsof_msg.data[1];
        uint8_t maxpageidx = gsof_msg.data[2];

        Debug("GSOF page: %u of %u (trans_number=%u)",
              pageidx, maxpageidx, trans_number);
#endif

        int valid = 0;

        // want 1 2 8 9 12
        for (uint32_t a = 3; a < gsof_msg.length; a++)
        {
            uint8_t output_type = gsof_msg.data[a];
            a++;
            uint8_t output_length = gsof_msg.data[a];
            a++;
            //Debug("GSOF type: " + output_type + " len: " + output_length);

            if (output_type == 1) // pos time
            {
                state.time_week_ms = SwapUint32(gsof_msg.data, a);
                state.time_week = SwapUint16(gsof_msg.data, a + 4);
                state.num_sats = gsof_msg.data[a + 6];
                uint8_t posf1 = gsof_msg.data[a + 7];
                uint8_t posf2 = gsof_msg.data[a + 8];

                //Debug("POSTIME: " + posf1 + " " + posf2);
                
                if ((posf1 & 1)) { // New position
                    state.status = AP_GPS::GPS_OK_FIX_3D;
                    if ((posf2 & 1)) { // Differential position 
                        state.status = AP_GPS::GPS_OK_FIX_3D_DGPS;
                        if (posf2 & 2) { // Differential position method
                            if (posf2 & 4) {// Differential position method
                                state.status = AP_GPS::GPS_OK_FIX_3D_RTK_FIXED;
                            } else {
                                state.status = AP_GPS::GPS_OK_FIX_3D_RTK_FLOAT;
                            }
                        }
                    }
                } else {
                    state.status = AP_GPS::NO_FIX;
                }
                valid++;
            }
            else if (output_type == 2) // position
            {
                state.location.lat = (int32_t)(RAD_TO_DEG_DOUBLE * (SwapDouble(gsof_msg.data, a)) * (double)1e7);
                state.location.lng = (int32_t)(RAD_TO_DEG_DOUBLE * (SwapDouble(gsof_msg.data, a + 8)) * (double)1e7);
                state.location.alt = (int32_t)(SwapDouble(gsof_msg.data, a + 16) * 100);

                state.last_gps_time_ms = AP_HAL::millis();

                valid++;
            }
            else if (output_type == 8) // velocity
            {
                uint8_t vflag = gsof_msg.data[a];
                if ((vflag & 1) == 1)
                {
                    state.ground_speed = SwapFloat(gsof_msg.data, a + 1);
                    state.ground_course = degrees(SwapFloat(gsof_msg.data, a + 5));
                    fill_3d_velocity();
                    state.velocity.z = -SwapFloat(gsof_msg.data, a + 9);
                    state.have_vertical_velocity = true;
                }
                valid++;
            }
            else if (output_type == 9) //dop
            {
                state.hdop = (uint16_t)(SwapFloat(gsof_msg.data, a + 4) * 100);
                valid++;
            }
            else if (output_type == 12) // position sigma
            {
                state.horizontal_accuracy = (SwapFloat(gsof_msg.data, a + 4) + SwapFloat(gsof_msg.data, a + 8)) / 2;
                state.vertical_accuracy = SwapFloat(gsof_msg.data, a + 16);
                state.have_horizontal_accuracy = true;
                state.have_vertical_accuracy = true;
                valid++;
            }

            a += output_length-1u;
        }

        if (valid == 5) {
            return true;
        } else {
            state.status = AP_GPS::NO_FIX;
        }
    }

    return false;
}
示例#2
0
bool apm::AP_GPS_GSOF::process_message(void)
{
    //http://www.trimble.com/OEM_ReceiverHelp/V4.81/en/default.html#welcome.html

    if (gsof_msg.packettype == 0x40) { // GSOF
        int valid = 0;
        // want 1 2 8 9 12
        for (uint32_t a = 3; a < gsof_msg.length; a++) {
            uint8_t output_type = gsof_msg.data[a];
            a++;
            uint8_t output_length = gsof_msg.data[a];
            a++;
            //Debug("GSOF type: " + output_type + " len: " + output_length);
            if (output_type == 1) { // pos time
                gps.state.time_week_ms = SwapUint32(gsof_msg.data, a);
                gps.state.time_week = SwapUint16(gsof_msg.data, a + 4);
                gps.state.num_sats = gsof_msg.data[a + 6];
                uint8_t posf1 = gsof_msg.data[a + 7];
                uint8_t posf2 = gsof_msg.data[a + 8];
                //Debug("POSTIME: " + posf1 + " " + posf2);
                if ((posf1 & 1) == 1) {
                    gps.state.status = gps_t::FIX_3D;
                    if ((posf2 & 1) == 1) {
                        gps.state.status = gps_t::FIX_3D_DGPS;
                        if ((posf2 & 4) == 4) {
                            gps.state.status = gps_t::FIX_3D_RTK;
                        }
                    }
                } else {
                    gps.state.status = gps_t::NO_FIX;
                }
                valid++;
            } else if (output_type == 2) { // position
                gps.state.location.lat = gps_t::lat_lon_type {RAD_TO_DEG_DOUBLE * (SwapDouble(gsof_msg.data, a)) * 1e7}; // deg1e7
                gps.state.location.lon = gps_t::lat_lon_type {RAD_TO_DEG_DOUBLE * (SwapDouble(gsof_msg.data, a + 8)) * 1e7}; // deg1e7
                gps.state.location.alt = gps_t::altitude_type {SwapDouble(gsof_msg.data, a + 16) * 1e2}; //cm

                gps.state.last_gps_time_ms = gps.state.time_week_ms;

                valid++;
            } else if (output_type == 8) { // velocity
                uint8_t vflag = gsof_msg.data[a];
                if ((vflag & 1) == 1) {
                    gps.state.ground_speed = SwapFloat(gsof_msg.data, a + 1);
                    gps.state.ground_course_cd = (int32_t)(ToDeg(SwapFloat(gsof_msg.data, a + 5)) * 100);
                    fill_3d_velocity();
                    gps.state.velocity.z = gps_t::velocity_type {-SwapFloat(gsof_msg.data, a + 9)};
                    gps.state.have_vertical_velocity = true;
                }
                valid++;
            } else if (output_type == 9) { //dop
                gps.state.hdop = (uint16_t)(SwapFloat(gsof_msg.data, a + 4) * 100);
                valid++;
            } else if (output_type == 12) { // position sigma
                gps.state.horizontal_accuracy = (SwapFloat(gsof_msg.data, a + 4) + SwapFloat(gsof_msg.data, a + 8)) / 2;
                gps.state.vertical_accuracy = SwapFloat(gsof_msg.data, a + 16);
                gps.state.have_horizontal_accuracy = true;
                gps.state.have_vertical_accuracy = true;
                valid++;
            }

            a += output_length-1u;
        }

        if (valid == 5) {
            return true;
        } else {
            gps.state.status = gps_t::NO_FIX;
        }
    }

    return false;
}