Browse Source

mavlink: send MISSION_CURRENT respect signed sequence number

- current_seq -1 indicates nothing currently active
sbg
Daniel Agar 5 years ago
parent
commit
80c1f4e5e6
  1. 6
      src/modules/mavlink/mavlink_mission.cpp
  2. 2
      src/modules/mavlink/mavlink_mission.h

6
src/modules/mavlink/mavlink_mission.cpp

@ -261,9 +261,9 @@ MavlinkMissionManager::send_mission_ack(uint8_t sysid, uint8_t compid, uint8_t t @@ -261,9 +261,9 @@ MavlinkMissionManager::send_mission_ack(uint8_t sysid, uint8_t compid, uint8_t t
}
void
MavlinkMissionManager::send_mission_current(uint16_t seq)
MavlinkMissionManager::send_mission_current(int32_t seq)
{
unsigned item_count = _count[MAV_MISSION_TYPE_MISSION];
int32_t item_count = _count[MAV_MISSION_TYPE_MISSION];
if (seq < item_count) {
mavlink_mission_current_t wpc;
@ -272,7 +272,7 @@ MavlinkMissionManager::send_mission_current(uint16_t seq) @@ -272,7 +272,7 @@ MavlinkMissionManager::send_mission_current(uint16_t seq)
mavlink_msg_mission_current_send_struct(_mavlink->get_channel(), &wpc);
} else if (seq == 0 && item_count == 0) {
} else if (seq <= 0 && item_count == 0) {
/* don't broadcast if no WPs */
} else {

2
src/modules/mavlink/mavlink_mission.h

@ -187,7 +187,7 @@ private: @@ -187,7 +187,7 @@ private:
*
* @param seq The waypoint sequence number the MAV should fly to.
*/
void send_mission_current(uint16_t seq);
void send_mission_current(int32_t seq);
void send_mission_count(uint8_t sysid, uint8_t compid, uint16_t count, MAV_MISSION_TYPE mission_type);

Loading…
Cancel
Save