diff options
Diffstat (limited to 'src/modules/mavlink/mavlink_main.h')
-rw-r--r-- | src/modules/mavlink/mavlink_main.h | 207 |
1 files changed, 144 insertions, 63 deletions
diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 5a118a0ad..f94036a17 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -93,6 +93,7 @@ struct mavlink_wpm_storage { uint8_t current_partner_compid; uint64_t timestamp_lastaction; uint64_t timestamp_last_send_setpoint; + uint64_t timestamp_last_send_request; uint32_t timeout; int current_dataman_id; }; @@ -122,25 +123,41 @@ public: /** * Display the mavlink status. */ - void status(); + void display_status(); - static int stream(int argc, char *argv[]); + static int stream_command(int argc, char *argv[]); - static int instance_count(); + static int instance_count(); - static Mavlink *new_instance(); + static Mavlink *new_instance(); - static Mavlink *get_instance(unsigned instance); + static Mavlink *get_instance(unsigned instance); - static Mavlink *get_instance_for_device(const char *device_name); + static Mavlink *get_instance_for_device(const char *device_name); - static int destroy_all_instances(); + static int destroy_all_instances(); - static bool instance_exists(const char *device_name, Mavlink *self); + static bool instance_exists(const char *device_name, Mavlink *self); - static int get_uart_fd(unsigned index); + static void forward_message(mavlink_message_t *msg, Mavlink *self); - int get_uart_fd(); + static int get_uart_fd(unsigned index); + + int get_uart_fd(); + + /** + * Get the MAVLink system id. + * + * @return The system ID of this vehicle + */ + int get_system_id(); + + /** + * Get the MAVLink component id. + * + * @return The component ID of this vehicle + */ + int get_component_id(); const char *_device_name; @@ -150,26 +167,30 @@ public: MAVLINK_MODE_CAMERA }; - void set_mode(enum MAVLINK_MODE); - enum MAVLINK_MODE get_mode() { return _mode; } + void set_mode(enum MAVLINK_MODE); + enum MAVLINK_MODE get_mode() { return _mode; } - bool get_hil_enabled() { return _hil_enabled; }; + bool get_hil_enabled() { return _hil_enabled; } - bool get_flow_control_enabled() { return _flow_control_enabled; } + bool get_use_hil_gps() { return _use_hil_gps; } + + bool get_flow_control_enabled() { return _flow_control_enabled; } + + bool get_forwarding_on() { return _forwarding_on; } /** * Handle waypoint related messages. */ - void mavlink_wpm_message_handler(const mavlink_message_t *msg); + void mavlink_wpm_message_handler(const mavlink_message_t *msg); - static int start_helper(int argc, char *argv[]); + static int start_helper(int argc, char *argv[]); /** * Handle parameter related messages. */ - void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg); + void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg); - void get_mavlink_mode_and_state(struct vehicle_status_s *status, struct position_setpoint_triplet_s *pos_sp_triplet, uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode); + void get_mavlink_mode_and_state(struct vehicle_status_s *status, struct position_setpoint_triplet_s *pos_sp_triplet, uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode); /** * Enable / disable Hardware in the Loop simulation mode. @@ -179,78 +200,125 @@ public: * requested change could not be made or was * redundant. */ - int set_hil_enabled(bool hil_enabled); + int set_hil_enabled(bool hil_enabled); - MavlinkOrbSubscription *add_orb_subscription(const orb_id_t topic); + MavlinkOrbSubscription *add_orb_subscription(const orb_id_t topic); - int get_instance_id(); + int get_instance_id(); /** * Enable / disable hardware flow control. * * @param enabled True if hardware flow control should be enabled */ - int enable_flow_control(bool enabled); + int enable_flow_control(bool enabled); + + mavlink_channel_t get_channel(); + + void configure_stream_threadsafe(const char *stream_name, float rate); + + bool _task_should_exit; /**< if true, mavlink task should exit */ + + int get_mavlink_fd() { return _mavlink_fd; } + + MavlinkStream * get_streams() const { return _streams; } + - mavlink_channel_t get_channel(); + /* Functions for waiting to start transmission until message received. */ + void set_has_received_messages(bool received_messages) { _received_messages = received_messages; } + bool get_has_received_messages() { return _received_messages; } + void set_wait_to_transmit(bool wait) { _wait_to_transmit = wait; } + bool get_wait_to_transmit() { return _wait_to_transmit; } + bool should_transmit() { return (!_wait_to_transmit || (_wait_to_transmit && _received_messages)); } - bool _task_should_exit; /**< if true, mavlink task should exit */ + bool message_buffer_write(void *ptr, int size); + + void lockMessageBufferMutex(void) { pthread_mutex_lock(&_message_buffer_mutex); } + void unlockMessageBufferMutex(void) { pthread_mutex_unlock(&_message_buffer_mutex); } + + /** + * Count a transmision error + */ + void count_txerr(); protected: - Mavlink *next; + Mavlink *next; private: - int _instance_id; + int _instance_id; - int _mavlink_fd; - bool _task_running; - - perf_counter_t _loop_perf; /**< loop performance counter */ + int _mavlink_fd; + bool _task_running; /* states */ - bool _hil_enabled; /**< Hardware In the Loop mode */ - bool _is_usb_uart; /**< Port is USB */ + bool _hil_enabled; /**< Hardware In the Loop mode */ + bool _use_hil_gps; /**< Accept GPS HIL messages (for example from an external motion capturing system to fake indoor gps) */ + bool _is_usb_uart; /**< Port is USB */ + bool _wait_to_transmit; /**< Wait to transmit until received messages. */ + bool _received_messages; /**< Whether we've received valid mavlink messages. */ - unsigned _main_loop_delay; /**< mainloop delay, depends on data rate */ + unsigned _main_loop_delay; /**< mainloop delay, depends on data rate */ - MavlinkOrbSubscription *_subscriptions; - MavlinkStream *_streams; + MavlinkOrbSubscription *_subscriptions; + MavlinkStream *_streams; - orb_advert_t _mission_pub; - struct mission_s mission; - uint8_t missionlib_msg_buf[MAVLINK_MAX_PACKET_LEN]; - MAVLINK_MODE _mode; + orb_advert_t _mission_pub; + struct mission_s mission; + MAVLINK_MODE _mode; - uint8_t _mavlink_wpm_comp_id; - mavlink_channel_t _channel; + uint8_t _mavlink_wpm_comp_id; + mavlink_channel_t _channel; struct mavlink_logbuffer _logbuffer; - unsigned int _total_counter; + unsigned int _total_counter; - pthread_t _receive_thread; + pthread_t _receive_thread; /* Allocate storage space for waypoints */ - mavlink_wpm_storage _wpm_s; - mavlink_wpm_storage *_wpm; + mavlink_wpm_storage _wpm_s; + mavlink_wpm_storage *_wpm; - bool _verbose; - int _uart_fd; - int _baudrate; - int _datarate; + bool _verbose; + bool _forwarding_on; + bool _passing_on; + bool _ftp_on; + int _uart_fd; + int _baudrate; + int _datarate; /** * If the queue index is not at 0, the queue sending * logic will send parameters from the current index * to len - 1, the end of the param list. */ - unsigned int _mavlink_param_queue_index; + unsigned int _mavlink_param_queue_index; + + bool mavlink_link_termination_allowed; + + char *_subscribe_to_stream; + float _subscribe_to_stream_rate; + + bool _flow_control_enabled; + + struct mavlink_message_buffer { + int write_ptr; + int read_ptr; + int size; + char *data; + }; - bool mavlink_link_termination_allowed; + mavlink_message_buffer _message_buffer; - char *_subscribe_to_stream; - float _subscribe_to_stream_rate; + pthread_mutex_t _message_buffer_mutex; - bool _flow_control_enabled; + bool _param_initialized; + param_t _param_system_id; + param_t _param_component_id; + param_t _param_system_type; + param_t _param_use_hil_gps; + + perf_counter_t _loop_perf; /**< loop performance counter */ + perf_counter_t _txerr_perf; /**< TX error counter */ /** * Send one parameter. @@ -258,7 +326,7 @@ private: * @param param The parameter id to send. * @return zero on success, nonzero on failure. */ - int mavlink_pm_send_param(param_t param); + int mavlink_pm_send_param(param_t param); /** * Send one parameter identified by index. @@ -266,7 +334,7 @@ private: * @param index The index of the parameter to send. * @return zero on success, nonzero else. */ - int mavlink_pm_send_param_for_index(uint16_t index); + int mavlink_pm_send_param_for_index(uint16_t index); /** * Send one parameter identified by name. @@ -274,14 +342,14 @@ private: * @param name The index of the parameter to send. * @return zero on success, nonzero else. */ - int mavlink_pm_send_param_for_name(const char *name); + int mavlink_pm_send_param_for_name(const char *name); /** * Send a queue of parameters, one parameter per function call. * * @return zero on success, nonzero on failure */ - int mavlink_pm_queued_send(void); + int mavlink_pm_queued_send(void); /** * Start sending the parameter queue. @@ -291,12 +359,12 @@ private: * mavlink_pm_queued_send(). * @see mavlink_pm_queued_send() */ - void mavlink_pm_start_queued_send(); + void mavlink_pm_start_queued_send(); - void mavlink_update_system(); + void mavlink_update_system(); - void mavlink_waypoint_eventloop(uint64_t now); - void mavlink_wpm_send_waypoint_reached(uint16_t seq); + 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); @@ -313,7 +381,20 @@ private: int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb); int configure_stream(const char *stream_name, const float rate); - void configure_stream_threadsafe(const char *stream_name, const float rate); + + int message_buffer_init(int size); + + void message_buffer_destroy(); + + int message_buffer_count(); + + int message_buffer_is_empty(); + + int message_buffer_get_ptr(void **ptr, bool *is_part); + + void message_buffer_mark_read(int n); + + void pass_message(mavlink_message_t *msg); static int mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg); |