Skip to content

Commit

Permalink
GCS_MAVLink: treat clear-all as zero-item-upload
Browse files Browse the repository at this point in the history
these would appear to be identical operations.
  • Loading branch information
peterbarker committed Aug 15, 2024
1 parent 61bd5e9 commit 0e1077f
Show file tree
Hide file tree
Showing 8 changed files with 26 additions and 43 deletions.
24 changes: 14 additions & 10 deletions libraries/GCS_MAVLink/MissionItemProtocol.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -29,13 +29,10 @@ void MissionItemProtocol::init_send_requests(GCS_MAVLINK &_link,
mission_request_warning_sent = false;
}

void MissionItemProtocol::handle_mission_clear_all(const GCS_MAVLINK &_link,
void MissionItemProtocol::handle_mission_clear_all(GCS_MAVLINK &_link,
const mavlink_message_t &msg)
{
bool success = true;
success = success && !receiving;
success = success && clear_all_items();
send_mission_ack(_link, msg, success ? MAV_MISSION_ACCEPTED : MAV_MISSION_ERROR);
handle_mission_count(_link, 0, msg);
}

bool MissionItemProtocol::mavlink2_requirement_met(const GCS_MAVLINK &_link, const mavlink_message_t &msg) const
Expand All @@ -55,6 +52,13 @@ bool MissionItemProtocol::mavlink2_requirement_met(const GCS_MAVLINK &_link, con
void MissionItemProtocol::handle_mission_count(
GCS_MAVLINK &_link,
const mavlink_mission_count_t &packet,
const mavlink_message_t &msg) {
handle_mission_count(_link, packet.count, msg);
}

void MissionItemProtocol::handle_mission_count(
GCS_MAVLINK &_link,
uint16_t count,
const mavlink_message_t &msg)
{
if (!mavlink2_requirement_met(_link, msg)) {
Expand All @@ -77,29 +81,29 @@ void MissionItemProtocol::handle_mission_count(
link = nullptr;
}

if (packet.count > max_items()) {
if (count > max_items()) {
// FIXME: different items take up different storage space!
send_mission_ack(_link, msg, MAV_MISSION_NO_SPACE);
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Only %u items are supported", (unsigned)max_items());
return;
}

MAV_MISSION_RESULT ret_alloc = allocate_receive_resources(packet.count);
MAV_MISSION_RESULT ret_alloc = allocate_receive_resources(count);
if (ret_alloc != MAV_MISSION_ACCEPTED) {
send_mission_ack(_link, msg, ret_alloc);
return;
}

truncate(packet);
truncate(count);

if (packet.count == 0) {
if (count == 0) {
// no requests to send...
transfer_is_complete(_link, msg);
return;
}

// start waypoint receiving
init_send_requests(_link, msg, 0, packet.count-1);
init_send_requests(_link, msg, 0, count-1);
}

void MissionItemProtocol::handle_mission_request_list(
Expand Down
8 changes: 4 additions & 4 deletions libraries/GCS_MAVLink/MissionItemProtocol.h
Original file line number Diff line number Diff line change
Expand Up @@ -49,7 +49,7 @@ class MissionItemProtocol
void handle_mission_item(const mavlink_message_t &msg,
const mavlink_mission_item_int_t &cmd);

void handle_mission_clear_all(const GCS_MAVLINK &link,
void handle_mission_clear_all(GCS_MAVLINK &link,
const mavlink_message_t &msg);

void queued_request_send();
Expand All @@ -73,13 +73,13 @@ class MissionItemProtocol
// item request to the GCS:
virtual ap_message next_item_ap_message_id() const = 0;

virtual bool clear_all_items() = 0;

uint16_t request_last; // last request index

private:

virtual void truncate(const mavlink_mission_count_t &packet) = 0;
void handle_mission_count(class GCS_MAVLINK &link, uint16_t count, const mavlink_message_t &msg);

virtual void truncate(uint16_t count) = 0;

uint16_t request_i; // request index

Expand Down
7 changes: 1 addition & 6 deletions libraries/GCS_MAVLink/MissionItemProtocol_Fence.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -199,16 +199,11 @@ uint16_t MissionItemProtocol_Fence::max_items() const
return _fence.polyfence().max_items();
}

void MissionItemProtocol_Fence::truncate(const mavlink_mission_count_t &packet)
void MissionItemProtocol_Fence::truncate(uint16_t count)
{
// FIXME: validate packet.count is same as allocated number of items
}

bool MissionItemProtocol_Fence::clear_all_items()
{
return _fence.polyfence().write_fence(nullptr, 0);
}

MAV_MISSION_RESULT MissionItemProtocol_Fence::allocate_receive_resources(const uint16_t count)
{
if (_new_items != nullptr) {
Expand Down
3 changes: 1 addition & 2 deletions libraries/GCS_MAVLink/MissionItemProtocol_Fence.h
Original file line number Diff line number Diff line change
Expand Up @@ -15,7 +15,7 @@ class MissionItemProtocol_Fence : public MissionItemProtocol {
return MAV_MISSION_TYPE_FENCE;
}

void truncate(const mavlink_mission_count_t &packet) override;
void truncate(uint16_t count) override;
MAV_MISSION_RESULT complete(const GCS_MAVLINK &_link) override;
void timeout() override;

Expand All @@ -31,7 +31,6 @@ class MissionItemProtocol_Fence : public MissionItemProtocol {
ap_message next_item_ap_message_id() const override {
return MSG_NEXT_MISSION_REQUEST_FENCE;
}
bool clear_all_items() override WARN_IF_UNUSED;

private:
class AC_Fence &_fence;
Expand Down
10 changes: 2 additions & 8 deletions libraries/GCS_MAVLink/MissionItemProtocol_Rally.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -49,12 +49,6 @@ MAV_MISSION_RESULT MissionItemProtocol_Rally::complete(const GCS_MAVLINK &_link)
return MAV_MISSION_ACCEPTED;
}

bool MissionItemProtocol_Rally::clear_all_items()
{
rally.truncate(0);
return true;
}

MAV_MISSION_RESULT MissionItemProtocol_Rally::convert_MISSION_ITEM_INT_to_RallyLocation(const mavlink_mission_item_int_t &cmd, RallyLocation &ret)
{
if (cmd.command != MAV_CMD_NAV_RALLY_POINT) {
Expand Down Expand Up @@ -184,9 +178,9 @@ void MissionItemProtocol_Rally::timeout()
link->send_text(MAV_SEVERITY_WARNING, "Rally upload timeout");
}

void MissionItemProtocol_Rally::truncate(const mavlink_mission_count_t &packet)
void MissionItemProtocol_Rally::truncate(uint16_t count)
{
rally.truncate(packet.count);
rally.truncate(count);
}

#endif // HAL_GCS_ENABLED && HAL_RALLY_ENABLED
3 changes: 1 addition & 2 deletions libraries/GCS_MAVLink/MissionItemProtocol_Rally.h
Original file line number Diff line number Diff line change
Expand Up @@ -10,7 +10,7 @@ class MissionItemProtocol_Rally : public MissionItemProtocol {
public:
MissionItemProtocol_Rally(class AP_Rally &_rally) :
rally(_rally) {}
void truncate(const mavlink_mission_count_t &packet) override;
void truncate(uint16_t count) override;
MAV_MISSION_TYPE mission_type() const override { return MAV_MISSION_TYPE_RALLY; }

MAV_MISSION_RESULT complete(const GCS_MAVLINK &_link) override;
Expand All @@ -28,7 +28,6 @@ class MissionItemProtocol_Rally : public MissionItemProtocol {
ap_message next_item_ap_message_id() const override {
return MSG_NEXT_MISSION_REQUEST_RALLY;
}
bool clear_all_items() override WARN_IF_UNUSED;

private:
AP_Rally &rally;
Expand Down
9 changes: 2 additions & 7 deletions libraries/GCS_MAVLink/MissionItemProtocol_Waypoints.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -50,11 +50,6 @@ MAV_MISSION_RESULT MissionItemProtocol_Waypoints::append_item(const mavlink_miss
return MAV_MISSION_ACCEPTED;
}

bool MissionItemProtocol_Waypoints::clear_all_items()
{
return mission.clear();
}

MAV_MISSION_RESULT MissionItemProtocol_Waypoints::complete(const GCS_MAVLINK &_link)
{
_link.send_text(MAV_SEVERITY_INFO, "Flight plan received");
Expand Down Expand Up @@ -141,10 +136,10 @@ void MissionItemProtocol_Waypoints::timeout()
link->send_text(MAV_SEVERITY_WARNING, "Mission upload timeout");
}

void MissionItemProtocol_Waypoints::truncate(const mavlink_mission_count_t &packet)
void MissionItemProtocol_Waypoints::truncate(uint16_t count)
{
// new mission arriving, truncate mission to be the same length
mission.truncate(packet.count);
mission.truncate(count);
}

#endif // HAL_GCS_ENABLED && AP_MISSION_ENABLED
5 changes: 1 addition & 4 deletions libraries/GCS_MAVLink/MissionItemProtocol_Waypoints.h
Original file line number Diff line number Diff line change
Expand Up @@ -23,13 +23,10 @@ class MissionItemProtocol_Waypoints : public MissionItemProtocol {
// truncate() is called to set the absolute number of items. It
// must be less than or equal to the current number of items (you
// can't truncate-to a longer list)
void truncate(const mavlink_mission_count_t &packet) override;
void truncate(uint16_t count) override;

protected:

// clear_all_items() is called to clear all items on the vehicle
bool clear_all_items() override WARN_IF_UNUSED;

// next_item_ap_message_id returns an item from the ap_message
// enumeration which (when acted upon by the GCS class) will send
// a mavlink message to the GCS requesting it upload the next
Expand Down

0 comments on commit 0e1077f

Please sign in to comment.