fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-27 09:03:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

mavlink: mission manager moved to separate class and reworked

Browse Source
This commit is contained in:
Anton Babushkin
2014-06-14 23:57:29 +02:00
parent ffd9ac7e08
commit 5be741607c
14 changed files with 1167 additions and 1016 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_main.h
+17 -73
View File
@@ -56,51 +56,7 @@
#include "mavlink_orb_subscription.h"
#include "mavlink_stream.h"
#include "mavlink_messages.h"
// FIXME XXX - TO BE MOVED TO XML
enum MAVLINK_WPM_STATES {
MAVLINK_WPM_STATE_IDLE = 0,
MAVLINK_WPM_STATE_SENDLIST,
MAVLINK_WPM_STATE_SENDLIST_SENDWPS,
MAVLINK_WPM_STATE_GETLIST,
MAVLINK_WPM_STATE_GETLIST_GETWPS,
MAVLINK_WPM_STATE_GETLIST_GOTALL,
MAVLINK_WPM_STATE_ENUM_END
};
enum MAVLINK_WPM_CODES {
MAVLINK_WPM_CODE_OK = 0,
MAVLINK_WPM_CODE_ERR_WAYPOINT_ACTION_NOT_SUPPORTED,
MAVLINK_WPM_CODE_ERR_WAYPOINT_FRAME_NOT_SUPPORTED,
MAVLINK_WPM_CODE_ERR_WAYPOINT_OUT_OF_BOUNDS,
MAVLINK_WPM_CODE_ERR_WAYPOINT_MAX_NUMBER_EXCEEDED,
MAVLINK_WPM_CODE_ENUM_END
};
#define MAVLINK_WPM_MAX_WP_COUNT 255
#define MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT 5000000 ///< Protocol communication action timeout in useconds
#define MAVLINK_WPM_RETRY_TIMEOUT_DEFAULT 500000 ///< Protocol communication retry timeout in useconds
#define MAVLINK_WPM_SETPOINT_DELAY_DEFAULT 1000000 ///< When to send a new setpoint
#define MAVLINK_WPM_PROTOCOL_DELAY_DEFAULT 40000
struct mavlink_wpm_storage {
uint16_t size; ///< Count of waypoints in active mission
uint16_t max_size; ///< Maximal count of waypoints
int dataman_id; ///< Dataman storage ID for active mission
enum MAVLINK_WPM_STATES current_state; ///< Current waypoint manager state
int16_t current_wp_id; ///< Waypoint ID in current transmission
uint16_t current_count; ///< Waypoints count in current transmission
uint8_t current_partner_sysid;
uint8_t current_partner_compid;
int current_dataman_id; ///< Dataman storage ID for current transmission
uint64_t timestamp_lastaction;
uint64_t timestamp_last_send_setpoint;
uint64_t timestamp_last_send_request;
uint32_t action_timeout;
uint32_t retry_timeout;
};
#include "mavlink_mission.h"
class Mavlink
@@ -143,7 +99,7 @@ public:
static bool instance_exists(const char *device_name, Mavlink *self);
static void forward_message(mavlink_message_t *msg, Mavlink *self);
static void forward_message(const mavlink_message_t *msg, Mavlink *self);
static int get_uart_fd(unsigned index);
@@ -168,10 +124,6 @@ public:
bool get_forwarding_on() { return _forwarding_on; }
/**
* Handle waypoint related messages.
*/
void mavlink_wpm_message_handler(const mavlink_message_t *msg);
static int start_helper(int argc, char *argv[]);
@@ -192,6 +144,10 @@ public:
*/
int set_hil_enabled(bool hil_enabled);
void send_message(const mavlink_message_t *msg);
void handle_message(const mavlink_message_t *msg);
MavlinkOrbSubscription *add_orb_subscription(const orb_id_t topic);
int get_instance_id();
@@ -209,6 +165,10 @@ public:
int get_mavlink_fd() { return _mavlink_fd; }
int send_statustext(const char *string);
int send_statustext(enum MAV_SEVERITY severity, const char *string);
float get_rate_mult();
/* Functions for waiting to start transmission until message received. */
void set_has_received_messages(bool received_messages) { _received_messages = received_messages; }
@@ -238,12 +198,12 @@ private:
MavlinkOrbSubscription *_subscriptions;
MavlinkStream *_streams;
MavlinkMissionManager *_mission_manager;
orb_advert_t _mission_pub;
struct mission_s _mission;
struct mission_result_s _mission_result;
int _mission_result_sub;
MAVLINK_MODE _mode;
uint8_t _mavlink_wpm_comp_id;
mavlink_channel_t _channel;
struct mavlink_logbuffer _logbuffer;
@@ -251,10 +211,6 @@ private:
pthread_t _receive_thread;
/* Allocate storage space for waypoints */
mavlink_wpm_storage _wpm_s;
mavlink_wpm_storage *_wpm;
bool _verbose;
bool _forwarding_on;
bool _passing_on;
@@ -336,20 +292,9 @@ private:
void mavlink_update_system();
void mavlink_waypoint_eventloop(uint64_t now);
void mavlink_wpm_send_waypoint_reached(uint16_t seq);
void mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, uint16_t seq);
void mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t seq);
void mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uint16_t count);
void mavlink_wpm_send_waypoint_current(uint16_t seq);
void mavlink_wpm_send_waypoint_ack(uint8_t sysid, uint8_t compid, uint8_t type);
void mavlink_wpm_init(mavlink_wpm_storage *state);
int map_mission_item_to_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item);
int map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavlink_mission_item, struct mission_item_s *mission_item);
void publish_mission();
uint8_t get_system_id();
void mavlink_missionlib_send_message(mavlink_message_t *msg);
int mavlink_missionlib_send_gcs_string(const char *string);
uint8_t get_component_id();
int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb);
@@ -364,13 +309,13 @@ private:
int message_buffer_is_empty();
bool message_buffer_write(void *ptr, int size);
bool message_buffer_write(const void *ptr, int size);
int message_buffer_get_ptr(void **ptr, bool *is_part);
void message_buffer_mark_read(int n);
void pass_message(mavlink_message_t *msg);
void pass_message(const mavlink_message_t *msg);
static int mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg);
@@ -378,5 +323,4 @@ private:
* Main mavlink task.
*/
int task_main(int argc, char *argv[]);
};