forked from Archive/PX4-Autopilot
MavlinkMissionManager: remove MavlinkStream inheritance
This commit is contained in:
parent
99b29777ba
commit
a761c4189e
|
@ -66,7 +66,7 @@ bool MavlinkMissionManager::_transfer_in_progress = false;
|
|||
(_msg.target_component == MAV_COMP_ID_MISSIONPLANNER) || \
|
||||
(_msg.target_component == MAV_COMP_ID_ALL)))
|
||||
|
||||
MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) : MavlinkStream(mavlink),
|
||||
MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) :
|
||||
_state(MAVLINK_WPM_STATE_IDLE),
|
||||
_time_last_recv(0),
|
||||
_time_last_sent(0),
|
||||
|
@ -86,8 +86,9 @@ MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) : MavlinkStream(m
|
|||
_offboard_mission_sub(-1),
|
||||
_mission_result_sub(-1),
|
||||
_offboard_mission_pub(nullptr),
|
||||
_slow_rate_limiter(_interval / 5.0f),
|
||||
_verbose(false)
|
||||
_slow_rate_limiter(100 * 1000), // Rate limit sending of the current WP sequence to 10 Hz
|
||||
_verbose(false),
|
||||
_mavlink(mavlink)
|
||||
{
|
||||
_offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission));
|
||||
_mission_result_sub = orb_subscribe(ORB_ID(mission_result));
|
||||
|
@ -101,20 +102,6 @@ MavlinkMissionManager::~MavlinkMissionManager()
|
|||
orb_unadvertise(_offboard_mission_pub);
|
||||
}
|
||||
|
||||
unsigned
|
||||
MavlinkMissionManager::get_size()
|
||||
{
|
||||
if (_state == MAVLINK_WPM_STATE_SENDLIST) {
|
||||
return MAVLINK_MSG_ID_MISSION_ITEM_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
|
||||
|
||||
} else if (_state == MAVLINK_WPM_STATE_GETLIST) {
|
||||
return MAVLINK_MSG_ID_MISSION_REQUEST + MAVLINK_NUM_NON_PAYLOAD_BYTES;
|
||||
|
||||
} else {
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
MavlinkMissionManager::init_offboard_mission()
|
||||
{
|
||||
|
|
|
@ -46,7 +46,6 @@
|
|||
|
||||
#include "mavlink_bridge_header.h"
|
||||
#include "mavlink_rate_limiter.h"
|
||||
#include "mavlink_stream.h"
|
||||
|
||||
enum MAVLINK_WPM_STATES {
|
||||
MAVLINK_WPM_STATE_IDLE = 0,
|
||||
|
@ -67,32 +66,20 @@ enum MAVLINK_WPM_CODES {
|
|||
#define MAVLINK_MISSION_PROTOCOL_TIMEOUT_DEFAULT 5000000 ///< Protocol communication action timeout in useconds
|
||||
#define MAVLINK_MISSION_RETRY_TIMEOUT_DEFAULT 500000 ///< Protocol communication retry timeout in useconds
|
||||
|
||||
class MavlinkMissionManager : public MavlinkStream
|
||||
class Mavlink;
|
||||
|
||||
class MavlinkMissionManager
|
||||
{
|
||||
public:
|
||||
explicit MavlinkMissionManager(Mavlink *mavlink);
|
||||
|
||||
~MavlinkMissionManager();
|
||||
|
||||
const char *get_name() const
|
||||
{
|
||||
return MavlinkMissionManager::get_name_static();
|
||||
}
|
||||
|
||||
static const char *get_name_static()
|
||||
{
|
||||
return "MISSION_ITEM";
|
||||
}
|
||||
|
||||
uint16_t get_id()
|
||||
{
|
||||
return MAVLINK_MSG_ID_MISSION_ITEM;
|
||||
}
|
||||
|
||||
static MavlinkStream *new_instance(Mavlink *mavlink)
|
||||
{
|
||||
return new MavlinkMissionManager(mavlink);
|
||||
}
|
||||
|
||||
unsigned get_size();
|
||||
/**
|
||||
* Handle sending of messages. Call this regularly at a fixed frequency.
|
||||
* @param t current time
|
||||
*/
|
||||
void send(const hrt_abstime t);
|
||||
|
||||
void handle_message(const mavlink_message_t *msg);
|
||||
|
||||
|
@ -140,6 +127,8 @@ private:
|
|||
|
||||
bool _verbose;
|
||||
|
||||
Mavlink *_mavlink;
|
||||
|
||||
static constexpr unsigned int FILESYSTEM_ERRCOUNT_NOTIFY_LIMIT =
|
||||
2; ///< Error count limit before stopping to report FS errors
|
||||
|
||||
|
@ -219,8 +208,4 @@ private:
|
|||
int format_mavlink_mission_item(const struct mission_item_s *mission_item,
|
||||
mavlink_mission_item_t *mavlink_mission_item);
|
||||
|
||||
protected:
|
||||
explicit MavlinkMissionManager(Mavlink *mavlink);
|
||||
|
||||
void send(const hrt_abstime t);
|
||||
};
|
||||
|
|
Loading…
Reference in New Issue