Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Sub: move rally-point handling up

This commit is contained in:
Peter Barker 2017-07-13 10:48:56 +10:00 committed by Francisco Ferreira
parent 983686745e
commit 20937e755e
2 changed files with 11 additions and 62 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

71
ArduSub/GCS_Mavlink.cpp
View File

@ -1644,68 +1644,6 @@ void GCS_MAVLINK_Sub::handleMessage(mavlink_message_t* msg)
#endif
break;
#if AC_RALLY == ENABLED
// receive a rally point from GCS and store in EEPROM
case MAVLINK_MSG_ID_RALLY_POINT: {
mavlink_rally_point_t packet;
mavlink_msg_rally_point_decode(msg, &packet);
if (packet.idx >= sub.rally.get_rally_total() ||
packet.idx >= sub.rally.get_rally_max()) {
send_text(MAV_SEVERITY_NOTICE,"Bad rally point message ID");
break;
}
if (packet.count != sub.rally.get_rally_total()) {
send_text(MAV_SEVERITY_NOTICE,"Bad rally point message count");
break;
}
// sanity check location
if (!check_latlng(packet.lat, packet.lng)) {
break;
}
RallyLocation rally_point;
rally_point.lat = packet.lat;
rally_point.lng = packet.lng;
rally_point.alt = packet.alt;
rally_point.break_alt = packet.break_alt;
rally_point.land_dir = packet.land_dir;
rally_point.flags = packet.flags;
if (!sub.rally.set_rally_point_with_index(packet.idx, rally_point)) {
send_text(MAV_SEVERITY_CRITICAL, "Error setting rally point");
}
break;
}
//send a rally point to the GCS
case MAVLINK_MSG_ID_RALLY_FETCH_POINT: {
mavlink_rally_fetch_point_t packet;
mavlink_msg_rally_fetch_point_decode(msg, &packet);
if (packet.idx > sub.rally.get_rally_total()) {
send_text(MAV_SEVERITY_NOTICE, "Bad rally point index");
break;
}
RallyLocation rally_point;
if (!sub.rally.get_rally_point_with_index(packet.idx, rally_point)) {
send_text(MAV_SEVERITY_NOTICE, "Failed to set rally point");
break;
}
mavlink_msg_rally_point_send_buf(msg,
chan, msg->sysid, msg->compid, packet.idx,
sub.rally.get_rally_total(), rally_point.lat, rally_point.lng,
rally_point.alt, rally_point.break_alt, rally_point.land_dir,
rally_point.flags);
break;
}
#endif // AC_RALLY == ENABLED
case MAVLINK_MSG_ID_AUTOPILOT_VERSION_REQUEST:
send_autopilot_version(FIRMWARE_VERSION);
break;
@ -1814,3 +1752,12 @@ AP_Mission *GCS_MAVLINK_Sub::get_mission()
{
return &sub.mission;
}
AP_Rally *GCS_MAVLINK_Sub::get_rally() const
{
#if AC_RALLY == ENABLED
return &sub.rally;
#else
return nullptr;
#endif
}

2
ArduSub/GCS_Mavlink.h
View File

@ -15,6 +15,8 @@ protected:
};
AP_Mission *get_mission() override;
AP_Rally *get_rally() const override;
uint8_t sysid_my_gcs() const override;
private: