diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index bbb1f76679..1345e6cc16 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -294,7 +294,7 @@ MavlinkMissionManager::send_mission_current(uint16_t seq) void -MavlinkMissionManager::send_mission_count(uint8_t sysid, uint8_t compid, uint16_t count) +MavlinkMissionManager::send_mission_count(uint8_t sysid, uint8_t compid, uint16_t count, MAV_MISSION_TYPE mission_type) { _time_last_sent = hrt_absolute_time(); @@ -303,6 +303,7 @@ MavlinkMissionManager::send_mission_count(uint8_t sysid, uint8_t compid, uint16_ wpc.target_system = sysid; wpc.target_component = compid; wpc.count = count; + wpc.mission_type = mission_type; mavlink_msg_mission_count_send_struct(_mavlink->get_channel(), &wpc); @@ -529,9 +530,7 @@ MavlinkMissionManager::send(const hrt_abstime now) && hrt_elapsed_time(&_time_last_sent) > _retry_timeout) { if (_transfer_seq == 0) { /* try to send items count again after timeout */ - if (_verbose) { warnx("WPM: send count timeout"); } - - send_mission_count(_transfer_partner_sysid, _transfer_partner_compid, _transfer_count); + send_mission_count(_transfer_partner_sysid, _transfer_partner_compid, _transfer_count, _mission_type); } else { /* try to send item again after timeout */ @@ -741,7 +740,7 @@ MavlinkMissionManager::handle_mission_request_list(const mavlink_message_t *msg) if (_verbose) { warnx("WPM: MISSION_REQUEST_LIST OK nothing to send, mission is empty"); } } - send_mission_count(msg->sysid, msg->compid, _transfer_count); + send_mission_count(msg->sysid, msg->compid, _transfer_count, _mission_type); } else { if (_verbose) { warnx("WPM: MISSION_REQUEST_LIST ERROR: busy"); } @@ -1541,6 +1540,7 @@ void MavlinkMissionManager::check_active_mission() if (_verbose) { warnx("WPM: New mission detected (possibly over different Mavlink instance) Updating"); } _my_dataman_id = _dataman_id; - send_mission_count(_transfer_partner_sysid, _transfer_partner_compid, _count[(uint8_t)MAV_MISSION_TYPE_MISSION]); + send_mission_count(_transfer_partner_sysid, _transfer_partner_compid, _count[(uint8_t)MAV_MISSION_TYPE_MISSION], + MAV_MISSION_TYPE_MISSION); } } diff --git a/src/modules/mavlink/mavlink_mission.h b/src/modules/mavlink/mavlink_mission.h index a3be0c4056..aef649358d 100644 --- a/src/modules/mavlink/mavlink_mission.h +++ b/src/modules/mavlink/mavlink_mission.h @@ -185,7 +185,7 @@ private: */ void send_mission_current(uint16_t seq); - void send_mission_count(uint8_t sysid, uint8_t compid, uint16_t count); + void send_mission_count(uint8_t sysid, uint8_t compid, uint16_t count, MAV_MISSION_TYPE mission_type); void send_mission_item(uint8_t sysid, uint8_t compid, uint16_t seq); diff --git a/src/modules/navigator/geofence.h b/src/modules/navigator/geofence.h index 3bebd69af9..bd093bda7b 100644 --- a/src/modules/navigator/geofence.h +++ b/src/modules/navigator/geofence.h @@ -43,10 +43,6 @@ #include -#include -#include -#include -#include #include #include #include @@ -142,9 +138,6 @@ public: private: Navigator *_navigator{nullptr}; - home_position_s _home_pos{0}; - bool _home_pos_set{false}; - hrt_abstime _last_horizontal_range_warning{0}; hrt_abstime _last_vertical_range_warning{0};