aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_GPS: handle inject packets as part of handle_msg

Browse Source
This commit is contained in:
Peter Barker 2017-07-25 17:59:14 +10:00 committed by Francisco Ferreira
parent 60ffe00164
commit 2ad453fc89
2 changed files with 19 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
libraries/AP_GPS/AP_GPS.cpp
View File

@ -737,6 +737,15 @@ void AP_GPS::update(void)
}
void AP_GPS::handle_gps_inject(const mavlink_message_t *msg)
{
mavlink_gps_inject_data_t packet;
mavlink_msg_gps_inject_data_decode(msg, &packet);
//TODO: check target
inject_data(packet.data, packet.len);
}
/*
pass along a mavlink message (for MAV type)
*/
@ -747,6 +756,10 @@ void AP_GPS::handle_msg(const mavlink_message_t *msg)
handle_gps_rtcm_data(msg);
return;
}
if (msg->msgid == MAVLINK_MSG_ID_GPS_INJECT_DATA) {
handle_gps_inject(msg);
return;
}
uint8_t i;
for (i=0; i<num_instances; i++) {
if ((drivers[i] != nullptr) && (_type[i] != GPS_TYPE_NONE)) {

10
libraries/AP_GPS/AP_GPS.h
View File

@ -346,10 +346,6 @@ public:
// lock out a GPS port, allowing another application to use the port
void lock_port(uint8_t instance, bool locked);
//Inject a packet of raw binary to a GPS
void inject_data(uint8_t *data, uint16_t len);
void inject_data(uint8_t instance, uint8_t *data, uint16_t len);
//MAVLink Status Sending
void send_mavlink_gps_raw(mavlink_channel_t chan);
void send_mavlink_gps2_raw(mavlink_channel_t chan);
@ -494,6 +490,12 @@ private:
// re-assemble GPS_RTCM_DATA message
void handle_gps_rtcm_data(const mavlink_message_t *msg);
void handle_gps_inject(const mavlink_message_t *msg);
//Inject a packet of raw binary to a GPS
void inject_data(uint8_t *data, uint16_t len);
void inject_data(uint8_t instance, uint8_t *data, uint16_t len);
// GPS blending and switching
Vector2f _NE_pos_offset_m[GPS_MAX_RECEIVERS]; // Filtered North,East position offset from GPS instance to blended solution in _output_state.location (m)