mirror of https://github.com/ArduPilot/ardupilot
Plane: move rally-point handling up
This commit is contained in:
parent
cf50c43f75
commit
983686745e
|
@ -1605,60 +1605,6 @@ void GCS_MAVLINK_Plane::handleMessage(mavlink_message_t* msg)
|
|||
}
|
||||
#endif // GEOFENCE_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 >= plane.rally.get_rally_total() ||
|
||||
packet.idx >= plane.rally.get_rally_max()) {
|
||||
send_text(MAV_SEVERITY_WARNING,"Bad rally point message ID");
|
||||
break;
|
||||
}
|
||||
|
||||
if (packet.count != plane.rally.get_rally_total()) {
|
||||
send_text(MAV_SEVERITY_WARNING,"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;
|
||||
plane.rally.set_rally_point_with_index(packet.idx, 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 > plane.rally.get_rally_total()) {
|
||||
send_text(MAV_SEVERITY_WARNING, "Bad rally point index");
|
||||
break;
|
||||
}
|
||||
RallyLocation rally_point;
|
||||
if (!plane.rally.get_rally_point_with_index(packet.idx, rally_point)) {
|
||||
send_text(MAV_SEVERITY_WARNING, "Failed to set rally point");
|
||||
break;
|
||||
}
|
||||
|
||||
mavlink_msg_rally_point_send_buf(msg,
|
||||
chan, msg->sysid, msg->compid, packet.idx,
|
||||
plane.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;
|
||||
}
|
||||
|
||||
case MAVLINK_MSG_ID_PARAM_SET:
|
||||
{
|
||||
handle_param_set(msg, &plane.DataFlash);
|
||||
|
@ -2113,3 +2059,8 @@ void GCS_MAVLINK_Plane::handle_mission_set_current(AP_Mission &mission, mavlink_
|
|||
plane.mission.resume();
|
||||
}
|
||||
}
|
||||
|
||||
AP_Rally *GCS_MAVLINK_Plane::get_rally() const
|
||||
{
|
||||
return &plane.rally;
|
||||
}
|
||||
|
|
|
@ -21,6 +21,8 @@ protected:
|
|||
AP_Mission *get_mission() override;
|
||||
void handle_mission_set_current(AP_Mission &mission, mavlink_message_t *msg) override;
|
||||
|
||||
AP_Rally *get_rally() const override;
|
||||
|
||||
uint8_t sysid_my_gcs() const override;
|
||||
|
||||
private:
|
||||
|
|
Loading…
Reference in New Issue