Skip to content

Commit

Permalink
GCS_MAVLink: support MISSION_CHECKSUM message
Browse files Browse the repository at this point in the history
  • Loading branch information
@peterbarker
peterbarker committed Jul 29, 2024
1 parent d7ecf5f commit e0d4057
Show file tree
Hide file tree
Showing 6 changed files with 271 additions and 19 deletions.
4 changes: 3 additions & 1 deletion libraries/GCS_MAVLink/GCS.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -236,7 +236,8 @@ class GCS_MAVLINK
msg.sysid,
msg.compid,
result,
mission_type);
mission_type,
opaque_id_for_mission_type(mission_type));
}

// packetReceived is called on any successful decode of a mavlink message
Expand Down Expand Up @@ -328,6 +329,7 @@ class GCS_MAVLINK
// mission is up to:
virtual MISSION_STATE mission_state(const class AP_Mission &mission) const;
// send a mission_current message for the supplied waypoint
uint32_t opaque_id_for_mission_type(MAV_MISSION_TYPE type) const;
void send_mission_current(const class AP_Mission &mission, uint16_t seq);

// common send functions
Expand Down
27 changes: 24 additions & 3 deletions libraries/GCS_MAVLink/GCS_Common.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -601,7 +601,8 @@ void GCS_MAVLINK::handle_mission_request_list(const mavlink_message_t &msg)
msg.sysid,
msg.compid,
MAV_MISSION_UNSUPPORTED,
packet.mission_type);
packet.mission_type,
0);
return;
}

Expand Down Expand Up @@ -660,6 +661,21 @@ MISSION_STATE GCS_MAVLINK::mission_state(const AP_Mission &mission) const
return MISSION_STATE_UNKNOWN;
}

// returns the opaque ID used for identifying a mission (or fence, or
// rally) on the vehicle. Returns 0 on failure.
uint32_t GCS_MAVLINK::opaque_id_for_mission_type(MAV_MISSION_TYPE type) const
{
MissionItemProtocol *prot = gcs().get_prot_for_mission_type(type);
if (prot == nullptr) {
return 0;
}

uint32_t opaque_id = 0;
UNUSED_RESULT(prot->opaque_id(opaque_id));

return opaque_id;
}

void GCS_MAVLINK::send_mission_current(const class AP_Mission &mission, uint16_t seq)
{
auto num_commands = mission.num_commands();
Expand All @@ -679,7 +695,11 @@ void GCS_MAVLINK::send_mission_current(const class AP_Mission &mission, uint16_t
seq,
num_commands, // total
mission_state(mission), // mission_state
mission_mode); // mission_mode
mission_mode, // mission_mode
opaque_id_for_mission_type(MAV_MISSION_TYPE_MISSION),
0, // fence_id, 0 meaning unsupported
0 // rally_id, 0 meaning unsupported
);
}

#if AP_MAVLINK_MISSION_SET_CURRENT_ENABLED
Expand Down Expand Up @@ -735,7 +755,8 @@ void GCS_MAVLINK::handle_mission_count(const mavlink_message_t &msg)
msg.sysid,
msg.compid,
MAV_MISSION_UNSUPPORTED,
packet.mission_type);
packet.mission_type,
0);
return;
}

Expand Down
82 changes: 70 additions & 12 deletions libraries/GCS_MAVLink/MissionItemProtocol.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -29,13 +29,21 @@ 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);
if (receiving) {
send_mission_ack(_link, msg, MAV_MISSION_ERROR);
return;
}
if (!clear_all_items()) {
send_mission_ack(_link, msg, MAV_MISSION_ERROR);
return;
}
link = &_link;
receiving = true;
timelast_receive_ms = AP_HAL::millis();
transfer_is_complete(_link, msg);
}

bool MissionItemProtocol::mavlink2_requirement_met(const GCS_MAVLINK &_link, const mavlink_message_t &msg) const
Expand Down Expand Up @@ -94,6 +102,9 @@ void MissionItemProtocol::handle_mission_count(

if (packet.count == 0) {
// no requests to send...
link = &_link;
receiving = true;
timelast_receive_ms = AP_HAL::millis();
transfer_is_complete(_link, msg);
return;
}
Expand Down Expand Up @@ -121,11 +132,15 @@ void MissionItemProtocol::handle_mission_request_list(
// reply with number of commands in the mission. The GCS will
// then request each command separately
CHECK_PAYLOAD_SIZE2_VOID(_link.get_chan(), MISSION_COUNT);
uint32_t _opaque_id = 0;
UNUSED_RESULT(opaque_id(_opaque_id));
mavlink_msg_mission_count_send(_link.get_chan(),
msg.sysid,
msg.compid,
item_count(),
mission_type());
mission_type(),
_opaque_id
);
}

void MissionItemProtocol::handle_mission_request_int(GCS_MAVLINK &_link,
Expand Down Expand Up @@ -314,8 +329,23 @@ void MissionItemProtocol::handle_mission_item(const mavlink_message_t &msg, cons
void MissionItemProtocol::transfer_is_complete(const GCS_MAVLINK &_link, const mavlink_message_t &msg)
{
const MAV_MISSION_RESULT result = complete(_link);
send_mission_ack(_link, msg, result);
free_upload_resources();
uint32_t _opaque_id;
if (!receiving) {
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
}
if (supports_opaque_id() && !opaque_id(_opaque_id)) {
// the opaque ID can't currently be calculated; we definitely
// want to have it in the mission ack to avoid race
// conditions. Defer sending the mission ack until it is
// available:
deferred_mission_ack.link = &_link;
deferred_mission_ack.sysid = msg.sysid;
deferred_mission_ack.compid = msg.compid;
deferred_mission_ack.opaque_id = 0;
return;
}
send_mission_ack(_link, msg, result);
receiving = false;
link = nullptr;
}
Expand All @@ -332,13 +362,23 @@ void MissionItemProtocol::send_mission_ack(const mavlink_message_t &msg,
void MissionItemProtocol::send_mission_ack(const GCS_MAVLINK &_link,
const mavlink_message_t &msg,
MAV_MISSION_RESULT result) const
{
send_mission_ack(_link, msg.sysid, msg.compid, result);
}
void MissionItemProtocol::send_mission_ack(const GCS_MAVLINK &_link,
uint8_t sysid,
uint8_t compid,
MAV_MISSION_RESULT result) const
{
CHECK_PAYLOAD_SIZE2_VOID(_link.get_chan(), MISSION_ACK);
uint32_t _opaque_id = 0;
UNUSED_RESULT(opaque_id(_opaque_id));
mavlink_msg_mission_ack_send(_link.get_chan(),
msg.sysid,
msg.compid,
sysid,
compid,
result,
mission_type());
mission_type(),
_opaque_id);
}

/**
Expand Down Expand Up @@ -377,18 +417,22 @@ void MissionItemProtocol::update()
INTERNAL_ERROR(AP_InternalError::error_t::gcs_bad_missionprotocol_link);
return;
}

const mavlink_channel_t chan = link->get_chan();
// stop waypoint receiving if timeout
const uint32_t tnow = AP_HAL::millis();
if (tnow - timelast_receive_ms > upload_timeout_ms) {
receiving = false;
timeout();
const mavlink_channel_t chan = link->get_chan();
if (HAVE_PAYLOAD_SPACE(chan, MISSION_ACK)) {
uint32_t _opaque_id = 0;
UNUSED_RESULT(opaque_id(_opaque_id));
mavlink_msg_mission_ack_send(chan,
dest_sysid,
dest_compid,
MAV_MISSION_OPERATION_CANCELLED,
mission_type());
mission_type(),
_opaque_id);
}
link = nullptr;
free_upload_resources();
Expand All @@ -400,6 +444,20 @@ void MissionItemProtocol::update()
timelast_request_ms = tnow;
link->send_message(next_item_ap_message_id());
}

// send any deferred transfer acceptance (to allow for
// asynchronous opaque-id calculation)
if (HAVE_PAYLOAD_SPACE(chan, MISSION_ACK) &&
deferred_mission_ack.link != nullptr &&
deferred_mission_ack.opaque_id != 0) {
send_mission_ack(*deferred_mission_ack.link,
deferred_mission_ack.sysid,
deferred_mission_ack.compid,
MAV_MISSION_ACCEPTED);
deferred_mission_ack.link = nullptr;
receiving = false;
link = nullptr;
}
}

#endif // HAL_GCS_ENABLED
18 changes: 16 additions & 2 deletions libraries/GCS_MAVLink/MissionItemProtocol.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -49,11 +49,11 @@ 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();
void update();
virtual void update();

bool active_link_is(const GCS_MAVLINK *_link) const { return _link == link; };

Expand All @@ -64,6 +64,7 @@ class MissionItemProtocol
// a method for GCS_MAVLINK to send warnings about received
// MISSION_ITEM messages (we should be getting MISSION_ITEM_INT)
void send_mission_item_warning();
virtual bool opaque_id(uint32_t &checksum) const { return false; }

protected:

Expand All @@ -77,6 +78,13 @@ class MissionItemProtocol

uint16_t request_last; // last request index

struct {
const GCS_MAVLINK *link;
uint8_t sysid;
uint8_t compid;
uint32_t opaque_id; // subclass fills this in when it is ready
} deferred_mission_ack;

private:

virtual void truncate(const mavlink_mission_count_t &packet) = 0;
Expand Down Expand Up @@ -117,6 +125,10 @@ class MissionItemProtocol

void send_mission_ack(const mavlink_message_t &msg, MAV_MISSION_RESULT result) const;
void send_mission_ack(const GCS_MAVLINK &link, const mavlink_message_t &msg, MAV_MISSION_RESULT result) const;
void send_mission_ack(const GCS_MAVLINK &_link,
uint8_t sysid,
uint8_t compid,
MAV_MISSION_RESULT result) const;

virtual uint16_t item_count() const = 0;
virtual uint16_t max_items() const = 0;
Expand All @@ -140,4 +152,6 @@ class MissionItemProtocol
virtual void timeout() {};

bool mavlink2_requirement_met(const GCS_MAVLINK &_link, const mavlink_message_t &msg) const;

virtual bool supports_opaque_id() const { return false; }
};
Loading

0 comments on commit e0d4057

Please sign in to comment.