2011-02-17 05:36:33 -04:00
|
|
|
/// @file GCS.h
|
2012-08-16 21:50:03 -03:00
|
|
|
/// @brief Interface definition for the various Ground Control System
|
|
|
|
// protocols.
|
2016-02-17 21:25:55 -04:00
|
|
|
#pragma once
|
2011-02-17 05:36:33 -04:00
|
|
|
|
2015-08-11 03:28:46 -03:00
|
|
|
#include <AP_HAL/AP_HAL.h>
|
|
|
|
#include <AP_Common/AP_Common.h>
|
|
|
|
#include "GCS_MAVLink.h"
|
|
|
|
#include <AP_Mission/AP_Mission.h>
|
2013-11-23 06:45:42 -04:00
|
|
|
#include <stdint.h>
|
2015-08-11 03:28:46 -03:00
|
|
|
#include "MAVLink_routing.h"
|
2016-10-26 19:06:54 -03:00
|
|
|
#include <AP_Frsky_Telem/AP_Frsky_Telem.h>
|
2017-07-25 03:36:53 -03:00
|
|
|
#include <AP_AdvancedFailsafe/AP_AdvancedFailsafe.h>
|
2018-06-21 23:46:56 -03:00
|
|
|
#include <AP_RTC/JitterCorrection.h>
|
2018-05-20 23:18:57 -03:00
|
|
|
#include <AP_Common/Bitmask.h>
|
2019-02-13 19:11:52 -04:00
|
|
|
#include <AP_Devo_Telem/AP_Devo_Telem.h>
|
2019-04-04 07:49:44 -03:00
|
|
|
#include <RC_Channel/RC_Channel.h>
|
2019-09-26 00:42:14 -03:00
|
|
|
#include <AP_Filesystem/AP_Filesystem_Available.h>
|
2019-07-16 23:32:05 -03:00
|
|
|
|
|
|
|
#include "MissionItemProtocol_Waypoints.h"
|
|
|
|
#include "MissionItemProtocol_Rally.h"
|
2019-04-29 00:05:59 -03:00
|
|
|
#include "MissionItemProtocol_Fence.h"
|
2019-07-16 23:32:05 -03:00
|
|
|
#include "ap_message.h"
|
2018-05-20 23:18:57 -03:00
|
|
|
|
2018-12-06 19:14:23 -04:00
|
|
|
#define GCS_DEBUG_SEND_MESSAGE_TIMINGS 0
|
2011-02-17 05:36:33 -04:00
|
|
|
|
2020-05-11 00:59:17 -03:00
|
|
|
#ifndef HAL_NO_GCS
|
2015-07-24 04:18:14 -03:00
|
|
|
// check if a message will fit in the payload space available
|
2018-05-15 21:43:09 -03:00
|
|
|
#define PAYLOAD_SIZE(chan, id) (GCS_MAVLINK::packet_overhead_chan(chan)+MAVLINK_MSG_ID_ ## id ## _LEN)
|
|
|
|
#define HAVE_PAYLOAD_SPACE(chan, id) (comm_get_txspace(chan) >= PAYLOAD_SIZE(chan, id))
|
2016-04-05 01:09:47 -03:00
|
|
|
#define CHECK_PAYLOAD_SIZE(id) if (comm_get_txspace(chan) < packet_overhead()+MAVLINK_MSG_ID_ ## id ## _LEN) return false
|
2015-07-24 04:18:14 -03:00
|
|
|
#define CHECK_PAYLOAD_SIZE2(id) if (!HAVE_PAYLOAD_SPACE(chan, id)) return false
|
|
|
|
|
2017-08-21 00:04:47 -03:00
|
|
|
// convenience macros for defining which ap_message ids are in which streams:
|
|
|
|
#define MAV_STREAM_ENTRY(stream_name) \
|
|
|
|
{ \
|
|
|
|
GCS_MAVLINK::stream_name, \
|
|
|
|
stream_name ## _msgs, \
|
|
|
|
ARRAY_SIZE(stream_name ## _msgs) \
|
|
|
|
}
|
2018-05-21 00:49:10 -03:00
|
|
|
#define MAV_STREAM_TERMINATOR { (streams)0, nullptr, 0 }
|
2017-08-21 00:04:47 -03:00
|
|
|
|
2018-12-10 03:56:32 -04:00
|
|
|
#define GCS_MAVLINK_NUM_STREAM_RATES 10
|
|
|
|
class GCS_MAVLINK_Parameters
|
|
|
|
{
|
|
|
|
public:
|
|
|
|
|
|
|
|
GCS_MAVLINK_Parameters();
|
|
|
|
|
|
|
|
static const struct AP_Param::GroupInfo var_info[];
|
|
|
|
|
|
|
|
// saveable rate of each stream
|
|
|
|
AP_Int16 streamRates[GCS_MAVLINK_NUM_STREAM_RATES];
|
|
|
|
};
|
|
|
|
|
2011-02-17 05:36:33 -04:00
|
|
|
///
|
|
|
|
/// @class GCS_MAVLINK
|
2014-12-09 23:39:32 -04:00
|
|
|
/// @brief MAVLink transport control class
|
2011-02-17 05:36:33 -04:00
|
|
|
///
|
2014-12-09 23:39:32 -04:00
|
|
|
class GCS_MAVLINK
|
2011-02-17 05:36:33 -04:00
|
|
|
{
|
|
|
|
public:
|
2018-03-25 07:45:48 -03:00
|
|
|
friend class GCS;
|
2018-12-10 03:56:32 -04:00
|
|
|
|
|
|
|
GCS_MAVLINK(GCS_MAVLINK_Parameters ¶meters, AP_HAL::UARTDriver &uart);
|
|
|
|
virtual ~GCS_MAVLINK() {}
|
|
|
|
|
2018-05-20 23:18:57 -03:00
|
|
|
void update_receive(uint32_t max_time_us=1000);
|
|
|
|
void update_send();
|
2018-12-10 03:56:32 -04:00
|
|
|
bool init(uint8_t instance);
|
2012-08-16 21:50:03 -03:00
|
|
|
void send_message(enum ap_message id);
|
2019-07-23 05:33:56 -03:00
|
|
|
void send_text(MAV_SEVERITY severity, const char *fmt, ...) const FMT_PRINTF(3, 4);
|
2012-08-16 21:50:03 -03:00
|
|
|
void queued_param_send();
|
2019-01-15 20:14:08 -04:00
|
|
|
void queued_mission_request_send();
|
2019-04-28 23:52:02 -03:00
|
|
|
|
2019-08-02 04:54:44 -03:00
|
|
|
bool sending_mavlink1() const;
|
|
|
|
|
2019-04-28 23:52:02 -03:00
|
|
|
// returns true if we are requesting any items from the GCS:
|
|
|
|
bool requesting_mission_items() const;
|
|
|
|
|
|
|
|
void send_mission_ack(const mavlink_message_t &msg,
|
|
|
|
MAV_MISSION_TYPE mission_type,
|
|
|
|
MAV_MISSION_RESULT result) const {
|
|
|
|
mavlink_msg_mission_ack_send(chan,
|
|
|
|
msg.sysid,
|
|
|
|
msg.compid,
|
|
|
|
result,
|
|
|
|
mission_type);
|
|
|
|
}
|
|
|
|
|
2019-04-29 00:05:59 -03:00
|
|
|
static const MAV_MISSION_TYPE supported_mission_types[3];
|
2019-04-28 23:52:02 -03:00
|
|
|
|
2016-06-14 22:04:40 -03:00
|
|
|
// packetReceived is called on any successful decode of a mavlink message
|
|
|
|
virtual void packetReceived(const mavlink_status_t &status,
|
2019-07-11 05:31:45 -03:00
|
|
|
const mavlink_message_t &msg);
|
2012-08-16 21:50:03 -03:00
|
|
|
|
2018-11-06 22:11:08 -04:00
|
|
|
// send a mavlink_message_t out this GCS_MAVLINK connection.
|
|
|
|
// Caller is responsible for ensuring space.
|
|
|
|
void send_message(uint32_t msgid, const char *pkt) const {
|
|
|
|
const mavlink_msg_entry_t *entry = mavlink_get_msg_entry(msgid);
|
|
|
|
if (entry == nullptr) {
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
send_message(pkt, entry);
|
|
|
|
}
|
|
|
|
void send_message(const char *pkt, const mavlink_msg_entry_t *entry) const {
|
|
|
|
_mav_finalize_message_chan_send(chan,
|
|
|
|
entry->msgid,
|
|
|
|
pkt,
|
|
|
|
entry->min_msg_len,
|
|
|
|
entry->max_msg_len,
|
|
|
|
entry->crc_extra);
|
|
|
|
}
|
|
|
|
|
2015-01-27 09:31:51 -04:00
|
|
|
// accessor for uart
|
|
|
|
AP_HAL::UARTDriver *get_uart() { return _port; }
|
|
|
|
|
2017-07-12 04:21:15 -03:00
|
|
|
virtual uint8_t sysid_my_gcs() const = 0;
|
2018-12-13 19:12:09 -04:00
|
|
|
virtual bool sysid_enforce() const { return false; }
|
2017-07-12 04:21:15 -03:00
|
|
|
|
2019-06-04 01:15:36 -03:00
|
|
|
void send_parameter_value(const char *param_name,
|
|
|
|
ap_var_type param_type,
|
|
|
|
float param_value);
|
|
|
|
|
2012-04-01 22:18:42 -03:00
|
|
|
// NOTE! The streams enum below and the
|
|
|
|
// set of AP_Int16 stream rates _must_ be
|
|
|
|
// kept in the same order
|
2018-05-21 00:49:10 -03:00
|
|
|
enum streams : uint8_t {
|
|
|
|
STREAM_RAW_SENSORS,
|
|
|
|
STREAM_EXTENDED_STATUS,
|
|
|
|
STREAM_RC_CHANNELS,
|
|
|
|
STREAM_RAW_CONTROLLER,
|
|
|
|
STREAM_POSITION,
|
|
|
|
STREAM_EXTRA1,
|
|
|
|
STREAM_EXTRA2,
|
|
|
|
STREAM_EXTRA3,
|
|
|
|
STREAM_PARAMS,
|
|
|
|
STREAM_ADSB,
|
|
|
|
NUM_STREAMS
|
|
|
|
};
|
2012-04-01 22:18:42 -03:00
|
|
|
|
2018-12-10 03:56:32 -04:00
|
|
|
// streams must be moved out into the top level for
|
|
|
|
// GCS_MAVLINK_Parameters to be able to use it. This is an
|
|
|
|
// extensive change, so we 'll just keep them in sync with a
|
|
|
|
// static assert for now:
|
|
|
|
static_assert(NUM_STREAMS == GCS_MAVLINK_NUM_STREAM_RATES, "num streams must equal num stream rates");
|
|
|
|
|
2017-06-15 03:09:02 -03:00
|
|
|
bool is_high_bandwidth() { return chan == MAVLINK_COMM_0; }
|
|
|
|
// return true if this channel has hardware flow control
|
|
|
|
bool have_flow_control();
|
|
|
|
|
2018-11-06 21:06:47 -04:00
|
|
|
bool is_active() const {
|
|
|
|
return GCS_MAVLINK::active_channel_mask() & (1 << (chan-MAVLINK_COMM_0));
|
|
|
|
}
|
|
|
|
bool is_streaming() const {
|
2019-06-06 20:23:06 -03:00
|
|
|
return sending_bucket_id != no_bucket_to_send;
|
2018-11-06 21:06:47 -04:00
|
|
|
}
|
|
|
|
|
2017-06-15 03:09:02 -03:00
|
|
|
mavlink_channel_t get_chan() const { return chan; }
|
|
|
|
uint32_t get_last_heartbeat_time() const { return last_heartbeat_time; };
|
|
|
|
|
2013-12-15 18:57:28 -04:00
|
|
|
uint32_t last_heartbeat_time; // milliseconds
|
|
|
|
|
2014-03-18 18:44:58 -03:00
|
|
|
// last time we got a non-zero RSSI from RADIO_STATUS
|
|
|
|
static uint32_t last_radio_status_remrssi_ms;
|
|
|
|
|
2015-07-24 04:18:14 -03:00
|
|
|
// mission item index to be sent on queued msg, delayed or not
|
|
|
|
uint16_t mission_item_reached_index = AP_MISSION_CMD_INDEX_NONE;
|
|
|
|
|
2013-12-28 01:00:19 -04:00
|
|
|
// common send functions
|
2018-05-02 05:37:08 -03:00
|
|
|
void send_heartbeat(void) const;
|
2013-12-28 01:00:19 -04:00
|
|
|
void send_meminfo(void);
|
2019-01-30 22:10:35 -04:00
|
|
|
void send_fence_status() const;
|
2014-02-13 07:07:13 -04:00
|
|
|
void send_power_status(void);
|
2019-06-13 19:57:04 -03:00
|
|
|
void send_battery_status(const uint8_t instance) const;
|
2018-05-02 05:10:18 -03:00
|
|
|
bool send_battery_status() const;
|
2018-12-18 06:47:01 -04:00
|
|
|
void send_distance_sensor() const;
|
|
|
|
// send_rangefinder sends only if a downward-facing instance is
|
|
|
|
// found. Rover overrides this!
|
|
|
|
virtual void send_rangefinder() const;
|
|
|
|
void send_proximity() const;
|
2018-05-04 21:07:59 -03:00
|
|
|
virtual void send_nav_controller_output() const = 0;
|
2019-02-28 19:24:13 -04:00
|
|
|
virtual void send_pid_tuning() = 0;
|
2018-01-05 21:27:09 -04:00
|
|
|
void send_ahrs2();
|
2018-12-18 07:55:24 -04:00
|
|
|
void send_ahrs3();
|
2017-10-25 01:32:35 -03:00
|
|
|
void send_system_time();
|
2019-07-06 19:48:16 -03:00
|
|
|
void send_rc_channels() const;
|
|
|
|
void send_rc_channels_raw() const;
|
2018-05-02 22:31:47 -03:00
|
|
|
void send_raw_imu();
|
2018-12-07 02:30:32 -04:00
|
|
|
|
|
|
|
void send_scaled_pressure_instance(uint8_t instance, void (*send_fn)(mavlink_channel_t chan, uint32_t time_boot_ms, float press_abs, float press_diff, int16_t temperature));
|
2018-03-05 16:36:45 -04:00
|
|
|
void send_scaled_pressure();
|
2018-12-07 02:30:32 -04:00
|
|
|
void send_scaled_pressure2();
|
|
|
|
virtual void send_scaled_pressure3(); // allow sub to override this
|
2018-05-02 22:38:22 -03:00
|
|
|
void send_sensor_offsets();
|
2018-01-01 18:21:00 -04:00
|
|
|
virtual void send_simstate() const;
|
2018-01-05 21:27:09 -04:00
|
|
|
void send_ahrs();
|
2018-05-02 05:10:18 -03:00
|
|
|
void send_battery2();
|
2018-09-02 03:32:50 -03:00
|
|
|
void send_opticalflow();
|
2018-05-02 03:50:28 -03:00
|
|
|
virtual void send_attitude() const;
|
2017-09-05 02:58:32 -03:00
|
|
|
void send_autopilot_version() const;
|
2018-05-23 06:07:10 -03:00
|
|
|
void send_extended_sys_state() const;
|
2018-01-05 21:27:09 -04:00
|
|
|
void send_local_position() const;
|
2018-05-09 07:55:37 -03:00
|
|
|
void send_vfr_hud();
|
2018-05-02 06:27:31 -03:00
|
|
|
void send_vibration() const;
|
2018-10-13 19:24:24 -03:00
|
|
|
void send_mount_status() const;
|
2018-05-15 02:20:30 -03:00
|
|
|
void send_named_float(const char *name, float value) const;
|
2018-10-13 19:24:24 -03:00
|
|
|
void send_gimbal_report() const;
|
2019-02-24 21:43:47 -04:00
|
|
|
void send_home_position() const;
|
|
|
|
void send_gps_global_origin() const;
|
2018-05-11 08:54:48 -03:00
|
|
|
virtual void send_position_target_global_int() { };
|
2019-03-28 06:38:08 -03:00
|
|
|
virtual void send_position_target_local_ned() { };
|
2018-05-10 03:56:25 -03:00
|
|
|
void send_servo_output_raw();
|
2017-05-29 18:03:10 -03:00
|
|
|
void send_accelcal_vehicle_position(uint32_t position);
|
2019-12-08 22:25:18 -04:00
|
|
|
void send_scaled_imu(uint8_t instance, void (*send_fn)(mavlink_channel_t chan, uint32_t time_ms, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag, int16_t temperature));
|
2019-01-22 21:36:45 -04:00
|
|
|
void send_sys_status();
|
2019-03-08 01:39:59 -04:00
|
|
|
void send_set_position_target_global_int(uint8_t target_system, uint8_t target_component, const Location& loc);
|
2019-03-01 18:38:12 -04:00
|
|
|
void send_rpm() const;
|
2013-12-28 01:00:19 -04:00
|
|
|
|
2019-07-17 02:45:07 -03:00
|
|
|
bool locked() const;
|
|
|
|
|
2014-07-17 08:03:12 -03:00
|
|
|
// return a bitmap of active channels. Used by libraries to loop
|
|
|
|
// over active channels to send to all active channels
|
|
|
|
static uint8_t active_channel_mask(void) { return mavlink_active; }
|
|
|
|
|
2016-05-30 07:05:00 -03:00
|
|
|
// return a bitmap of streaming channels
|
|
|
|
static uint8_t streaming_channel_mask(void) { return chan_is_streaming; }
|
|
|
|
|
2018-11-21 19:17:59 -04:00
|
|
|
// set a channel as private. Private channels get sent heartbeats, but
|
|
|
|
// don't get broadcast packets or forwarded packets
|
|
|
|
static void set_channel_private(mavlink_channel_t chan);
|
|
|
|
|
|
|
|
// return true if channel is private
|
|
|
|
static bool is_private(mavlink_channel_t _chan) {
|
|
|
|
return (mavlink_private & (1U<<(unsigned)_chan)) != 0;
|
|
|
|
}
|
|
|
|
|
|
|
|
// return true if channel is private
|
|
|
|
bool is_private(void) const { return is_private(chan); }
|
|
|
|
|
2015-03-28 22:36:06 -03:00
|
|
|
/*
|
|
|
|
send a MAVLink message to all components with this vehicle's system id
|
|
|
|
This is a no-op if no routes to components have been learned
|
|
|
|
*/
|
2019-10-17 04:07:23 -03:00
|
|
|
static void send_to_components(uint32_t msgid, const char *pkt, uint8_t pkt_len) { routing.send_to_components(msgid, pkt, pkt_len); }
|
|
|
|
|
2016-05-11 06:24:59 -03:00
|
|
|
/*
|
|
|
|
allow forwarding of packets / heartbeats to be blocked as required by some components to reduce traffic
|
|
|
|
*/
|
|
|
|
static void disable_channel_routing(mavlink_channel_t chan) { routing.no_route_mask |= (1U<<(chan-MAVLINK_COMM_0)); }
|
|
|
|
|
2015-07-24 08:52:56 -03:00
|
|
|
/*
|
|
|
|
search for a component in the routing table with given mav_type and retrieve it's sysid, compid and channel
|
|
|
|
returns if a matching component is found
|
|
|
|
*/
|
|
|
|
static bool find_by_mavtype(uint8_t mav_type, uint8_t &sysid, uint8_t &compid, mavlink_channel_t &channel) { return routing.find_by_mavtype(mav_type, sysid, compid, channel); }
|
|
|
|
|
2016-01-21 02:55:50 -04:00
|
|
|
// update signing timestamp on GPS lock
|
|
|
|
static void update_signing_timestamp(uint64_t timestamp_usec);
|
2016-04-05 01:09:47 -03:00
|
|
|
|
|
|
|
// return current packet overhead for a channel
|
|
|
|
static uint8_t packet_overhead_chan(mavlink_channel_t chan);
|
|
|
|
|
2018-03-25 07:45:48 -03:00
|
|
|
// alternative protocol function handler
|
|
|
|
FUNCTOR_TYPEDEF(protocol_handler_fn_t, bool, uint8_t, AP_HAL::UARTDriver *);
|
2017-08-21 00:04:47 -03:00
|
|
|
|
|
|
|
struct stream_entries {
|
2018-05-21 00:49:10 -03:00
|
|
|
const streams stream_id;
|
|
|
|
const ap_message *ap_message_ids;
|
2017-08-21 00:04:47 -03:00
|
|
|
const uint8_t num_ap_message_ids;
|
|
|
|
};
|
|
|
|
// vehicle subclass cpp files should define this:
|
|
|
|
static const struct stream_entries all_stream_entries[];
|
|
|
|
|
2018-03-28 21:49:34 -03:00
|
|
|
virtual uint64_t capabilities() const;
|
2019-04-28 23:52:02 -03:00
|
|
|
uint8_t get_stream_slowdown_ms() const { return stream_slowdown_ms; }
|
2018-03-28 21:49:34 -03:00
|
|
|
|
2019-09-10 22:26:15 -03:00
|
|
|
MAV_RESULT set_message_interval(uint32_t msg_id, int32_t interval_us);
|
|
|
|
|
2016-05-27 09:52:24 -03:00
|
|
|
protected:
|
|
|
|
|
2017-08-21 00:04:47 -03:00
|
|
|
virtual bool in_hil_mode() const { return false; }
|
|
|
|
|
2019-07-31 22:02:31 -03:00
|
|
|
bool mavlink_coordinate_frame_to_location_alt_frame(MAV_FRAME coordinate_frame,
|
2019-03-14 22:46:05 -03:00
|
|
|
Location::AltFrame &frame);
|
2019-02-05 17:46:28 -04:00
|
|
|
|
2016-08-13 18:42:40 -03:00
|
|
|
// overridable method to check for packet acceptance. Allows for
|
|
|
|
// enforcement of GCS sysid
|
2019-07-11 05:31:45 -03:00
|
|
|
bool accept_packet(const mavlink_status_t &status, const mavlink_message_t &msg);
|
2018-05-16 00:38:15 -03:00
|
|
|
void set_ekf_origin(const Location& loc);
|
2017-07-07 23:51:37 -03:00
|
|
|
|
2018-03-22 05:49:33 -03:00
|
|
|
virtual MAV_MODE base_mode() const = 0;
|
2019-11-24 17:24:44 -04:00
|
|
|
MAV_STATE system_status() const;
|
|
|
|
virtual MAV_STATE vehicle_system_status() const = 0;
|
2018-03-22 05:49:33 -03:00
|
|
|
|
2018-05-23 06:07:10 -03:00
|
|
|
virtual MAV_VTOL_STATE vtol_state() const { return MAV_VTOL_STATE_UNDEFINED; }
|
|
|
|
virtual MAV_LANDED_STATE landed_state() const { return MAV_LANDED_STATE_UNDEFINED; }
|
|
|
|
|
2019-07-17 01:50:12 -03:00
|
|
|
// return a MAVLink parameter type given a AP_Param type
|
|
|
|
static MAV_PARAM_TYPE mav_param_type(enum ap_var_type t);
|
|
|
|
|
2016-05-27 09:52:24 -03:00
|
|
|
AP_Param * _queued_parameter; ///< next parameter to
|
|
|
|
// be sent in queue
|
|
|
|
mavlink_channel_t chan;
|
|
|
|
uint8_t packet_overhead(void) const { return packet_overhead_chan(chan); }
|
|
|
|
|
|
|
|
// saveable rate of each stream
|
2018-12-10 03:56:32 -04:00
|
|
|
AP_Int16 *streamRates;
|
2016-05-27 09:52:24 -03:00
|
|
|
|
2018-05-21 09:34:22 -03:00
|
|
|
virtual bool persist_streamrates() const { return false; }
|
2019-07-11 05:31:45 -03:00
|
|
|
void handle_request_data_stream(const mavlink_message_t &msg);
|
2016-05-27 09:52:24 -03:00
|
|
|
|
2019-07-11 05:31:45 -03:00
|
|
|
virtual void handle_command_ack(const mavlink_message_t &msg);
|
|
|
|
void handle_set_mode(const mavlink_message_t &msg);
|
|
|
|
void handle_command_int(const mavlink_message_t &msg);
|
2018-07-05 21:56:06 -03:00
|
|
|
|
|
|
|
MAV_RESULT handle_command_int_do_set_home(const mavlink_command_int_t &packet);
|
2018-07-03 22:43:27 -03:00
|
|
|
virtual MAV_RESULT handle_command_int_packet(const mavlink_command_int_t &packet);
|
|
|
|
|
2018-07-05 21:56:06 -03:00
|
|
|
virtual bool set_home_to_current_location(bool lock) = 0;
|
|
|
|
virtual bool set_home(const Location& loc, bool lock) = 0;
|
|
|
|
|
2019-03-05 07:55:53 -04:00
|
|
|
MAV_RESULT handle_command_do_set_home(const mavlink_command_long_t &packet);
|
|
|
|
|
2019-07-11 05:31:45 -03:00
|
|
|
void handle_mission_request_list(const mavlink_message_t &msg);
|
|
|
|
void handle_mission_request(const mavlink_message_t &msg);
|
|
|
|
void handle_mission_request_int(const mavlink_message_t &msg);
|
|
|
|
void handle_mission_clear_all(const mavlink_message_t &msg);
|
|
|
|
virtual void handle_mission_set_current(AP_Mission &mission, const mavlink_message_t &msg);
|
|
|
|
void handle_mission_count(const mavlink_message_t &msg);
|
|
|
|
void handle_mission_write_partial_list(const mavlink_message_t &msg);
|
|
|
|
void handle_mission_item(const mavlink_message_t &msg);
|
|
|
|
|
|
|
|
void handle_common_param_message(const mavlink_message_t &msg);
|
|
|
|
void handle_param_set(const mavlink_message_t &msg);
|
|
|
|
void handle_param_request_list(const mavlink_message_t &msg);
|
|
|
|
void handle_param_request_read(const mavlink_message_t &msg);
|
2017-08-19 08:23:19 -03:00
|
|
|
virtual bool params_ready() const { return true; }
|
2019-07-11 05:31:45 -03:00
|
|
|
virtual void handle_rc_channels_override(const mavlink_message_t &msg);
|
|
|
|
void handle_system_time_message(const mavlink_message_t &msg);
|
|
|
|
void handle_common_rally_message(const mavlink_message_t &msg);
|
|
|
|
void handle_rally_fetch_point(const mavlink_message_t &msg);
|
|
|
|
void handle_rally_point(const mavlink_message_t &msg);
|
|
|
|
virtual void handle_mount_message(const mavlink_message_t &msg);
|
|
|
|
void handle_fence_message(const mavlink_message_t &msg);
|
|
|
|
void handle_param_value(const mavlink_message_t &msg);
|
|
|
|
void handle_radio_status(const mavlink_message_t &msg, bool log_radio);
|
|
|
|
void handle_serial_control(const mavlink_message_t &msg);
|
|
|
|
void handle_vision_position_delta(const mavlink_message_t &msg);
|
|
|
|
|
|
|
|
void handle_common_message(const mavlink_message_t &msg);
|
|
|
|
void handle_set_gps_global_origin(const mavlink_message_t &msg);
|
|
|
|
void handle_setup_signing(const mavlink_message_t &msg);
|
2017-07-23 20:53:51 -03:00
|
|
|
virtual bool should_zero_rc_outputs_on_reboot() const { return false; }
|
2019-10-04 06:07:03 -03:00
|
|
|
virtual MAV_RESULT handle_preflight_reboot(const mavlink_command_long_t &packet);
|
2018-05-20 23:18:57 -03:00
|
|
|
|
|
|
|
// reset a message interval via mavlink:
|
|
|
|
MAV_RESULT handle_command_set_message_interval(const mavlink_command_long_t &packet);
|
2018-12-24 01:22:00 -04:00
|
|
|
MAV_RESULT handle_command_get_message_interval(const mavlink_command_long_t &packet);
|
|
|
|
bool get_ap_message_interval(ap_message id, uint16_t &interval_ms) const;
|
2019-02-07 22:05:54 -04:00
|
|
|
MAV_RESULT handle_command_request_message(const mavlink_command_long_t &packet);
|
2018-05-20 23:18:57 -03:00
|
|
|
|
2017-07-16 20:49:21 -03:00
|
|
|
MAV_RESULT handle_rc_bind(const mavlink_command_long_t &packet);
|
2017-07-25 03:36:53 -03:00
|
|
|
virtual MAV_RESULT handle_flight_termination(const mavlink_command_long_t &packet);
|
2016-11-06 21:42:47 -04:00
|
|
|
|
2019-07-11 05:31:45 -03:00
|
|
|
void handle_send_autopilot_version(const mavlink_message_t &msg);
|
2017-08-18 20:07:42 -03:00
|
|
|
MAV_RESULT handle_command_request_autopilot_capabilities(const mavlink_command_long_t &packet);
|
|
|
|
|
2017-08-19 08:23:19 -03:00
|
|
|
virtual void send_banner();
|
|
|
|
|
2019-07-11 05:31:45 -03:00
|
|
|
void handle_device_op_read(const mavlink_message_t &msg);
|
|
|
|
void handle_device_op_write(const mavlink_message_t &msg);
|
2017-03-29 03:47:32 -03:00
|
|
|
|
2018-05-16 04:17:25 -03:00
|
|
|
void send_timesync();
|
|
|
|
// returns the time a timesync message was most likely received:
|
|
|
|
uint64_t timesync_receive_timestamp_ns() const;
|
|
|
|
// returns a timestamp suitable for packing into the ts1 field of TIMESYNC:
|
|
|
|
uint64_t timesync_timestamp_ns() const;
|
2019-07-11 05:31:45 -03:00
|
|
|
void handle_timesync(const mavlink_message_t &msg);
|
2018-05-16 04:17:25 -03:00
|
|
|
struct {
|
|
|
|
int64_t sent_ts1;
|
|
|
|
uint32_t last_sent_ms;
|
|
|
|
const uint16_t interval_ms = 10000;
|
|
|
|
} _timesync_request;
|
|
|
|
|
2019-07-11 05:31:45 -03:00
|
|
|
void handle_statustext(const mavlink_message_t &msg);
|
2017-07-12 04:21:15 -03:00
|
|
|
|
2017-07-12 05:11:41 -03:00
|
|
|
bool telemetry_delayed() const;
|
|
|
|
virtual uint32_t telem_delay() const = 0;
|
|
|
|
|
2017-07-16 22:04:32 -03:00
|
|
|
MAV_RESULT handle_command_preflight_set_sensor_offsets(const mavlink_command_long_t &packet);
|
2018-06-27 03:00:43 -03:00
|
|
|
MAV_RESULT handle_command_flash_bootloader(const mavlink_command_long_t &packet);
|
2018-03-17 06:25:52 -03:00
|
|
|
|
|
|
|
// generally this should not be overridden; Plane overrides it to ensure
|
2019-03-27 04:19:38 -03:00
|
|
|
// failsafe isn't triggered during calibration
|
2018-03-17 06:25:52 -03:00
|
|
|
virtual MAV_RESULT handle_command_preflight_calibration(const mavlink_command_long_t &packet);
|
|
|
|
|
2018-03-17 05:19:14 -03:00
|
|
|
virtual MAV_RESULT _handle_command_preflight_calibration(const mavlink_command_long_t &packet);
|
2018-03-18 01:22:39 -03:00
|
|
|
virtual MAV_RESULT _handle_command_preflight_calibration_baro();
|
2018-03-17 05:19:14 -03:00
|
|
|
|
2018-08-13 13:01:11 -03:00
|
|
|
MAV_RESULT handle_command_preflight_can(const mavlink_command_long_t &packet);
|
|
|
|
|
2019-06-18 06:30:35 -03:00
|
|
|
MAV_RESULT handle_command_battery_reset(const mavlink_command_long_t &packet);
|
2019-07-11 05:31:45 -03:00
|
|
|
void handle_command_long(const mavlink_message_t &msg);
|
2018-07-04 00:43:07 -03:00
|
|
|
MAV_RESULT handle_command_accelcal_vehicle_pos(const mavlink_command_long_t &packet);
|
2018-10-13 19:24:24 -03:00
|
|
|
virtual MAV_RESULT handle_command_mount(const mavlink_command_long_t &packet);
|
2017-07-16 21:46:54 -03:00
|
|
|
MAV_RESULT handle_command_mag_cal(const mavlink_command_long_t &packet);
|
2018-07-03 23:16:16 -03:00
|
|
|
virtual MAV_RESULT handle_command_long_packet(const mavlink_command_long_t &packet);
|
2017-07-26 02:48:01 -03:00
|
|
|
MAV_RESULT handle_command_camera(const mavlink_command_long_t &packet);
|
2017-08-19 11:29:01 -03:00
|
|
|
MAV_RESULT handle_command_do_send_banner(const mavlink_command_long_t &packet);
|
2018-10-13 19:24:24 -03:00
|
|
|
MAV_RESULT handle_command_do_set_roi(const mavlink_command_int_t &packet);
|
|
|
|
MAV_RESULT handle_command_do_set_roi(const mavlink_command_long_t &packet);
|
|
|
|
virtual MAV_RESULT handle_command_do_set_roi(const Location &roi_loc);
|
2018-07-03 23:16:16 -03:00
|
|
|
MAV_RESULT handle_command_do_gripper(const mavlink_command_long_t &packet);
|
2017-09-16 08:50:59 -03:00
|
|
|
MAV_RESULT handle_command_do_set_mode(const mavlink_command_long_t &packet);
|
2018-03-16 00:57:18 -03:00
|
|
|
MAV_RESULT handle_command_get_home_position(const mavlink_command_long_t &packet);
|
2019-02-03 20:07:23 -04:00
|
|
|
MAV_RESULT handle_command_do_fence_enable(const mavlink_command_long_t &packet);
|
2017-07-13 22:43:30 -03:00
|
|
|
|
2019-07-11 05:31:45 -03:00
|
|
|
void handle_optical_flow(const mavlink_message_t &msg);
|
2019-03-27 04:19:13 -03:00
|
|
|
|
2019-11-20 23:54:07 -04:00
|
|
|
MAV_RESULT handle_fixed_mag_cal_yaw(const mavlink_command_long_t &packet);
|
|
|
|
|
2017-07-17 20:53:41 -03:00
|
|
|
// vehicle-overridable message send function
|
|
|
|
virtual bool try_send_message(enum ap_message id);
|
2019-02-28 17:38:55 -04:00
|
|
|
virtual void send_global_position_int();
|
2017-07-17 20:53:41 -03:00
|
|
|
|
|
|
|
// message sending functions:
|
2017-07-18 05:55:06 -03:00
|
|
|
bool try_send_mission_message(enum ap_message id);
|
2017-07-18 08:50:23 -03:00
|
|
|
void send_hwstatus();
|
2019-07-11 05:31:45 -03:00
|
|
|
void handle_data_packet(const mavlink_message_t &msg);
|
2018-03-25 07:45:48 -03:00
|
|
|
|
2018-05-01 09:04:00 -03:00
|
|
|
// these two methods are called after current_loc is updated:
|
|
|
|
virtual int32_t global_position_int_alt() const;
|
|
|
|
virtual int32_t global_position_int_relative_alt() const;
|
|
|
|
|
2018-05-09 07:55:37 -03:00
|
|
|
virtual float vfr_hud_climbrate() const;
|
|
|
|
virtual float vfr_hud_airspeed() const;
|
|
|
|
virtual int16_t vfr_hud_throttle() const { return 0; }
|
2018-11-05 21:03:34 -04:00
|
|
|
virtual float vfr_hud_alt() const;
|
2018-05-09 07:55:37 -03:00
|
|
|
|
2017-06-06 22:38:05 -03:00
|
|
|
static constexpr const float magic_force_arm_value = 2989.0f;
|
|
|
|
static constexpr const float magic_force_disarm_value = 21196.0f;
|
|
|
|
|
2019-05-05 22:32:47 -03:00
|
|
|
virtual bool allow_disarm() const { return true; }
|
|
|
|
|
2018-05-06 02:24:30 -03:00
|
|
|
void manual_override(RC_Channel *c, int16_t value_in, uint16_t offset, float scaler, const uint32_t tnow, bool reversed = false);
|
|
|
|
|
2011-02-17 05:36:33 -04:00
|
|
|
private:
|
2016-05-29 07:47:57 -03:00
|
|
|
|
2019-02-06 22:29:55 -04:00
|
|
|
void log_mavlink_stats();
|
|
|
|
|
2017-09-15 23:29:54 -03:00
|
|
|
MAV_RESULT _set_mode_common(const MAV_MODE base_mode, const uint32_t custom_mode);
|
|
|
|
|
2019-07-11 05:31:45 -03:00
|
|
|
virtual void handleMessage(const mavlink_message_t &msg) = 0;
|
2012-08-16 21:50:03 -03:00
|
|
|
|
2018-07-03 23:16:16 -03:00
|
|
|
MAV_RESULT handle_servorelay_message(const mavlink_command_long_t &packet);
|
2017-07-13 23:44:21 -03:00
|
|
|
|
2019-08-22 22:23:57 -03:00
|
|
|
static bool command_long_stores_location(const MAV_CMD command);
|
|
|
|
static void convert_COMMAND_LONG_to_COMMAND_INT(const mavlink_command_long_t &in, mavlink_command_int_t &out);
|
|
|
|
|
2018-03-17 08:55:57 -03:00
|
|
|
bool calibrate_gyros();
|
|
|
|
|
2014-12-09 23:39:32 -04:00
|
|
|
/// The stream we are communicating over
|
|
|
|
AP_HAL::UARTDriver *_port;
|
|
|
|
|
2012-08-16 21:50:03 -03:00
|
|
|
/// Perform queued sending operations
|
|
|
|
///
|
|
|
|
enum ap_var_type _queued_parameter_type; ///< type of the next
|
|
|
|
// parameter
|
|
|
|
AP_Param::ParamToken _queued_parameter_token; ///AP_Param token for
|
|
|
|
// next() call
|
|
|
|
uint16_t _queued_parameter_index; ///< next queued
|
|
|
|
// parameter's index
|
|
|
|
uint16_t _queued_parameter_count; ///< saved count of
|
|
|
|
// parameters for
|
|
|
|
// queued send
|
2013-11-23 06:45:42 -04:00
|
|
|
uint32_t _queued_parameter_send_time_ms;
|
2012-08-16 21:50:03 -03:00
|
|
|
|
|
|
|
/// Count the number of reportable parameters.
|
|
|
|
///
|
|
|
|
/// Not all parameters can be reported via MAVlink. We count the number
|
|
|
|
// that are
|
|
|
|
/// so that we can report to a GCS the number of parameters it should
|
|
|
|
// expect when it
|
|
|
|
/// requests the full set.
|
|
|
|
///
|
|
|
|
/// @return The number of reportable parameters.
|
|
|
|
///
|
|
|
|
uint16_t packet_drops;
|
2011-04-15 10:24:05 -03:00
|
|
|
|
2019-02-15 20:56:22 -04:00
|
|
|
// number of extra ms to add to slow things down for the radio
|
|
|
|
uint16_t stream_slowdown_ms;
|
2013-03-20 23:54:04 -03:00
|
|
|
|
2017-04-28 01:35:53 -03:00
|
|
|
// perf counters
|
2017-08-02 06:23:07 -03:00
|
|
|
AP_HAL::Util::perf_counter_t _perf_packet;
|
|
|
|
AP_HAL::Util::perf_counter_t _perf_update;
|
2017-09-12 14:24:13 -03:00
|
|
|
char _perf_packet_name[16];
|
|
|
|
char _perf_update_name[16];
|
2017-08-02 06:23:07 -03:00
|
|
|
|
2018-05-20 23:18:57 -03:00
|
|
|
// outbound ("deferred message") queue.
|
|
|
|
|
2018-12-06 19:14:38 -04:00
|
|
|
// "special" messages such as heartbeat, next_param etc are stored
|
|
|
|
// separately to stream-rated messages like AHRS2 etc. If these
|
|
|
|
// were to be stored in buckets then they would be slowed down
|
|
|
|
// based on stream_slowdown, which we have not traditionally done.
|
2018-05-20 23:18:57 -03:00
|
|
|
struct deferred_message_t {
|
|
|
|
const ap_message id;
|
|
|
|
uint16_t interval_ms;
|
|
|
|
uint16_t last_sent_ms; // from AP_HAL::millis16()
|
|
|
|
} deferred_message[2] = {
|
|
|
|
{ MSG_HEARTBEAT, },
|
|
|
|
{ MSG_NEXT_PARAM, },
|
|
|
|
};
|
|
|
|
// returns index of id in deferred_message[] or -1 if not present
|
|
|
|
int8_t get_deferred_message_index(const ap_message id) const;
|
|
|
|
// returns index of a message in deferred_message[] which should
|
|
|
|
// be sent (or -1 if none to send at the moment)
|
|
|
|
int8_t deferred_message_to_send_index();
|
|
|
|
// cache of which deferred message should be sent next:
|
|
|
|
int8_t next_deferred_message_to_send_cache = -1;
|
|
|
|
|
|
|
|
struct deferred_message_bucket_t {
|
2019-04-11 09:13:14 -03:00
|
|
|
Bitmask<MSG_LAST> ap_message_ids;
|
2018-05-20 23:18:57 -03:00
|
|
|
uint16_t interval_ms;
|
|
|
|
uint16_t last_sent_ms; // from AP_HAL::millis16()
|
|
|
|
};
|
2018-12-06 20:11:02 -04:00
|
|
|
deferred_message_bucket_t deferred_message_bucket[10];
|
2018-05-20 23:18:57 -03:00
|
|
|
static const uint8_t no_bucket_to_send = -1;
|
|
|
|
static const ap_message no_message_to_send = (ap_message)-1;
|
|
|
|
uint8_t sending_bucket_id = no_bucket_to_send;
|
2019-04-11 09:13:14 -03:00
|
|
|
Bitmask<MSG_LAST> bucket_message_ids_to_send;
|
2018-05-20 23:18:57 -03:00
|
|
|
|
2018-12-06 19:35:46 -04:00
|
|
|
ap_message next_deferred_bucket_message_to_send();
|
2018-05-20 23:18:57 -03:00
|
|
|
void find_next_bucket_to_send();
|
|
|
|
void remove_message_from_bucket(int8_t bucket, ap_message id);
|
|
|
|
|
|
|
|
// bitmask of IDs the code has spontaneously decided it wants to
|
|
|
|
// send out. Examples include HEARTBEAT (gcs_send_heartbeat)
|
2019-04-11 09:13:14 -03:00
|
|
|
Bitmask<MSG_LAST> pushed_ap_message_ids;
|
2018-05-20 23:18:57 -03:00
|
|
|
|
|
|
|
// returns true if it is OK to send a message while we are in
|
|
|
|
// delay callback. In particular, when we are doing sensor init
|
|
|
|
// we still send heartbeats.
|
|
|
|
bool should_send_message_in_delay_callback(const ap_message id) const;
|
|
|
|
|
|
|
|
// if true is returned, interval will contain the default interval for id
|
|
|
|
bool get_default_interval_for_ap_message(const ap_message id, uint16_t &interval) const;
|
|
|
|
// if true is returned, interval will contain the default interval for id
|
|
|
|
bool get_default_interval_for_mavlink_message_id(const uint32_t mavlink_message_id, uint16_t &interval) const;
|
|
|
|
// returns an interval in milliseconds for any ap_message in stream id
|
|
|
|
uint16_t get_interval_for_stream(GCS_MAVLINK::streams id) const;
|
|
|
|
// set an inverval for a specific mavlink message. Returns false
|
|
|
|
// on failure (typically because there is no mapping from that
|
|
|
|
// mavlink ID to an ap_message)
|
|
|
|
bool set_mavlink_message_id_interval(const uint32_t mavlink_id,
|
|
|
|
const uint16_t interval_ms);
|
|
|
|
// map a mavlink ID to an ap_message which, if passed to
|
|
|
|
// try_send_message, will cause a mavlink message with that id to
|
|
|
|
// be emitted. Returns MSG_LAST if no such mapping exists.
|
|
|
|
ap_message mavlink_id_to_ap_message_id(const uint32_t mavlink_id) const;
|
|
|
|
// set the interval at which an ap_message should be emitted (in ms)
|
|
|
|
bool set_ap_message_interval(enum ap_message id, uint16_t interval_ms);
|
|
|
|
// call set_ap_message_interval for each entry in a stream,
|
|
|
|
// the interval being based on the stream's rate
|
|
|
|
void initialise_message_intervals_for_stream(GCS_MAVLINK::streams id);
|
|
|
|
// call initialise_message_intervals_for_stream on every stream:
|
|
|
|
void initialise_message_intervals_from_streamrates();
|
|
|
|
// boolean that indicated that message intervals have been set
|
|
|
|
// from streamrates:
|
|
|
|
bool deferred_messages_initialised;
|
|
|
|
// return interval deferred message bucket should be sent after.
|
|
|
|
// When sending parameters and waypoints this may be longer than
|
|
|
|
// the interval specified in "deferred"
|
|
|
|
uint16_t get_reschedule_interval_ms(const deferred_message_bucket_t &deferred) const;
|
|
|
|
|
|
|
|
bool do_try_send_message(const ap_message id);
|
2014-03-18 20:56:09 -03:00
|
|
|
|
2016-05-27 21:41:52 -03:00
|
|
|
// time when we missed sending a parameter for GCS
|
|
|
|
static uint32_t reserve_param_space_start_ms;
|
|
|
|
|
2014-07-17 08:03:12 -03:00
|
|
|
// bitmask of what mavlink channels are active
|
|
|
|
static uint8_t mavlink_active;
|
2014-05-20 23:38:07 -03:00
|
|
|
|
2018-11-21 19:17:59 -04:00
|
|
|
// bitmask of what mavlink channels are private
|
|
|
|
static uint8_t mavlink_private;
|
|
|
|
|
2016-06-13 17:36:34 -03:00
|
|
|
// bitmask of what mavlink channels are streaming
|
|
|
|
static uint8_t chan_is_streaming;
|
|
|
|
|
2014-12-09 23:31:31 -04:00
|
|
|
// mavlink routing object
|
|
|
|
static MAVLink_routing routing;
|
|
|
|
|
2017-04-29 02:30:57 -03:00
|
|
|
struct pending_param_request {
|
|
|
|
mavlink_channel_t chan;
|
|
|
|
int16_t param_index;
|
|
|
|
char param_name[AP_MAX_NAME_SIZE+1];
|
|
|
|
};
|
|
|
|
|
|
|
|
struct pending_param_reply {
|
|
|
|
mavlink_channel_t chan;
|
|
|
|
float value;
|
|
|
|
enum ap_var_type p_type;
|
|
|
|
int16_t param_index;
|
|
|
|
uint16_t count;
|
|
|
|
char param_name[AP_MAX_NAME_SIZE+1];
|
|
|
|
};
|
|
|
|
|
|
|
|
// queue of pending parameter requests and replies
|
|
|
|
static ObjectBuffer<pending_param_request> param_requests;
|
|
|
|
static ObjectBuffer<pending_param_reply> param_replies;
|
|
|
|
|
|
|
|
// have we registered the IO timer callback?
|
|
|
|
static bool param_timer_registered;
|
|
|
|
|
|
|
|
// IO timer callback for parameters
|
|
|
|
void param_io_timer(void);
|
2019-03-13 22:22:04 -03:00
|
|
|
|
|
|
|
uint8_t send_parameter_async_replies();
|
2014-12-09 23:31:31 -04:00
|
|
|
|
2019-09-26 00:42:14 -03:00
|
|
|
#if HAVE_FILESYSTEM_SUPPORT
|
|
|
|
|
|
|
|
enum class FTP_OP : uint8_t {
|
|
|
|
None = 0,
|
|
|
|
TerminateSession = 1,
|
|
|
|
ResetSessions = 2,
|
|
|
|
ListDirectory = 3,
|
|
|
|
OpenFileRO = 4,
|
|
|
|
ReadFile = 5,
|
|
|
|
CreateFile = 6,
|
|
|
|
WriteFile = 7,
|
|
|
|
RemoveFile = 8,
|
|
|
|
CreateDirectory = 9,
|
|
|
|
RemoveDirectory = 10,
|
|
|
|
OpenFileWO = 11,
|
|
|
|
TruncateFile = 12,
|
|
|
|
Rename = 13,
|
|
|
|
CalcFileCRC32 = 14,
|
|
|
|
BurstReadFile = 15,
|
|
|
|
Ack = 128,
|
|
|
|
Nack = 129,
|
|
|
|
};
|
|
|
|
|
|
|
|
enum class FTP_ERROR : uint8_t {
|
|
|
|
None = 0,
|
|
|
|
Fail = 1,
|
|
|
|
FailErrno = 2,
|
|
|
|
InvalidDataSize = 3,
|
|
|
|
InvalidSession = 4,
|
|
|
|
NoSessionsAvailable = 5,
|
|
|
|
EndOfFile = 6,
|
|
|
|
UnknownCommand = 7,
|
|
|
|
FileExists = 8,
|
|
|
|
FileProtected = 9,
|
|
|
|
FileNotFound = 10,
|
|
|
|
};
|
|
|
|
|
|
|
|
struct pending_ftp {
|
|
|
|
uint32_t offset;
|
|
|
|
mavlink_channel_t chan;
|
|
|
|
uint16_t seq_number;
|
|
|
|
FTP_OP opcode;
|
|
|
|
FTP_OP req_opcode;
|
|
|
|
bool burst_complete;
|
|
|
|
uint8_t size;
|
|
|
|
uint8_t session;
|
|
|
|
uint8_t sysid;
|
|
|
|
uint8_t compid;
|
|
|
|
uint8_t data[239];
|
|
|
|
};
|
|
|
|
|
|
|
|
enum class FTP_FILE_MODE {
|
|
|
|
Read,
|
|
|
|
Write,
|
|
|
|
};
|
|
|
|
|
|
|
|
struct ftp_state {
|
|
|
|
ObjectBuffer<pending_ftp> *requests;
|
|
|
|
ObjectBuffer<pending_ftp> *replies;
|
|
|
|
|
|
|
|
// session specific info, currently only support a single session over all links
|
|
|
|
int fd = -1;
|
|
|
|
FTP_FILE_MODE mode; // work around AP_Filesystem not supporting file modes
|
|
|
|
int16_t current_session;
|
|
|
|
};
|
|
|
|
static struct ftp_state ftp;
|
|
|
|
|
|
|
|
static void ftp_error(struct pending_ftp &response, FTP_ERROR error); // FTP helper method for packing a NAK
|
2019-11-02 06:49:52 -03:00
|
|
|
static int gen_dir_entry(char *dest, size_t space, const char * path, const struct dirent * entry); // FTP helper for emitting a dir response
|
2019-09-26 00:42:14 -03:00
|
|
|
static void ftp_list_dir(struct pending_ftp &request, struct pending_ftp &response);
|
|
|
|
|
|
|
|
bool ftp_init(void);
|
|
|
|
void handle_file_transfer_protocol(const mavlink_message_t &msg);
|
|
|
|
void send_ftp_replies(void);
|
|
|
|
void ftp_worker(void);
|
2019-11-02 07:15:51 -03:00
|
|
|
void ftp_push_replies(pending_ftp &reply);
|
2019-09-26 00:42:14 -03:00
|
|
|
#endif // HAVE_FILESYSTEM_SUPPORT
|
|
|
|
|
2019-07-11 03:03:14 -03:00
|
|
|
void send_distance_sensor(const class AP_RangeFinder_Backend *sensor, const uint8_t instance) const;
|
2018-05-09 04:46:20 -03:00
|
|
|
|
2016-05-27 09:52:24 -03:00
|
|
|
virtual bool handle_guided_request(AP_Mission::Mission_Command &cmd) = 0;
|
|
|
|
virtual void handle_change_alt_request(AP_Mission::Mission_Command &cmd) = 0;
|
2019-07-11 05:31:45 -03:00
|
|
|
void handle_common_mission_message(const mavlink_message_t &msg);
|
2014-03-18 19:59:40 -03:00
|
|
|
|
2019-07-11 05:31:45 -03:00
|
|
|
void handle_vicon_position_estimate(const mavlink_message_t &msg);
|
|
|
|
void handle_vision_position_estimate(const mavlink_message_t &msg);
|
|
|
|
void handle_global_vision_position_estimate(const mavlink_message_t &msg);
|
|
|
|
void handle_att_pos_mocap(const mavlink_message_t &msg);
|
2018-04-19 01:22:21 -03:00
|
|
|
void handle_common_vision_position_estimate_data(const uint64_t usec,
|
|
|
|
const float x,
|
|
|
|
const float y,
|
|
|
|
const float z,
|
|
|
|
const float roll,
|
|
|
|
const float pitch,
|
2018-05-15 21:43:09 -03:00
|
|
|
const float yaw,
|
|
|
|
const uint16_t payload_size);
|
2018-04-19 01:22:21 -03:00
|
|
|
void log_vision_position_estimate_data(const uint64_t usec,
|
2019-08-23 02:31:43 -03:00
|
|
|
const uint32_t corrected_msec,
|
2018-04-19 01:22:21 -03:00
|
|
|
const float x,
|
|
|
|
const float y,
|
|
|
|
const float z,
|
|
|
|
const float roll,
|
|
|
|
const float pitch,
|
|
|
|
const float yaw);
|
2017-07-18 22:36:49 -03:00
|
|
|
|
2019-07-11 05:31:45 -03:00
|
|
|
void lock_channel(const mavlink_channel_t chan, bool lock);
|
2015-04-22 20:11:43 -03:00
|
|
|
|
2018-05-14 00:09:12 -03:00
|
|
|
/*
|
|
|
|
correct an offboard timestamp in microseconds to a local time
|
|
|
|
since boot in milliseconds
|
|
|
|
*/
|
2018-05-15 21:43:09 -03:00
|
|
|
uint32_t correct_offboard_timestamp_usec_to_ms(uint64_t offboard_usec, uint16_t payload_size);
|
2018-05-14 00:09:12 -03:00
|
|
|
|
2016-01-21 02:55:50 -04:00
|
|
|
mavlink_signing_t signing;
|
|
|
|
static mavlink_signing_streams_t signing_streams;
|
2016-04-05 20:38:21 -03:00
|
|
|
static uint32_t last_signing_save_ms;
|
2016-01-21 02:55:50 -04:00
|
|
|
|
|
|
|
static StorageAccess _signing_storage;
|
2016-04-05 20:38:21 -03:00
|
|
|
static bool signing_key_save(const struct SigningKey &key);
|
|
|
|
static bool signing_key_load(struct SigningKey &key);
|
2016-01-20 22:49:06 -04:00
|
|
|
void load_signing_key(void);
|
2016-04-05 01:09:47 -03:00
|
|
|
bool signing_enabled(void) const;
|
2016-04-05 20:38:21 -03:00
|
|
|
static void save_signing_timestamp(bool force_save_now);
|
2018-03-25 07:45:48 -03:00
|
|
|
|
|
|
|
// alternative protocol handler support
|
|
|
|
struct {
|
|
|
|
GCS_MAVLINK::protocol_handler_fn_t handler;
|
|
|
|
uint32_t last_mavlink_ms;
|
|
|
|
uint32_t last_alternate_ms;
|
|
|
|
bool active;
|
|
|
|
} alternative;
|
2018-05-14 00:09:12 -03:00
|
|
|
|
2018-06-21 23:46:56 -03:00
|
|
|
JitterCorrection lag_correction;
|
|
|
|
|
2018-05-01 09:04:00 -03:00
|
|
|
// we cache the current location and send it even if the AHRS has
|
|
|
|
// no idea where we are:
|
|
|
|
struct Location global_position_current_loc;
|
|
|
|
|
2017-07-23 20:53:51 -03:00
|
|
|
void zero_rc_outputs();
|
2018-05-20 23:18:57 -03:00
|
|
|
|
2019-02-06 22:29:55 -04:00
|
|
|
uint8_t last_tx_seq;
|
|
|
|
uint16_t send_packet_count;
|
|
|
|
|
2018-12-06 19:14:23 -04:00
|
|
|
#if GCS_DEBUG_SEND_MESSAGE_TIMINGS
|
2018-05-20 23:18:57 -03:00
|
|
|
struct {
|
|
|
|
uint32_t longest_time_us;
|
|
|
|
ap_message longest_id;
|
|
|
|
uint32_t no_space_for_message;
|
|
|
|
uint16_t statustext_last_sent_ms;
|
|
|
|
uint32_t behind;
|
2019-08-20 19:19:23 -03:00
|
|
|
uint32_t out_of_time;
|
2018-05-20 23:18:57 -03:00
|
|
|
uint16_t fnbts_maxtime;
|
|
|
|
uint32_t max_retry_deferred_body_us;
|
|
|
|
uint8_t max_retry_deferred_body_type;
|
|
|
|
} try_send_message_stats;
|
2019-02-15 20:56:22 -04:00
|
|
|
uint16_t max_slowdown_ms;
|
2018-05-20 23:18:57 -03:00
|
|
|
#endif
|
|
|
|
|
2019-02-06 22:29:55 -04:00
|
|
|
uint32_t last_mavlink_stats_logged;
|
2011-02-17 05:36:33 -04:00
|
|
|
};
|
2016-05-30 07:05:00 -03:00
|
|
|
|
|
|
|
/// @class GCS
|
|
|
|
/// @brief global GCS object
|
|
|
|
class GCS
|
|
|
|
{
|
|
|
|
|
|
|
|
public:
|
|
|
|
|
|
|
|
GCS() {
|
|
|
|
if (_singleton == nullptr) {
|
|
|
|
_singleton = this;
|
|
|
|
} else {
|
|
|
|
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
|
|
|
|
// this is a serious problem, but we don't need to kill a
|
|
|
|
// real vehicle
|
|
|
|
AP_HAL::panic("GCS must be singleton");
|
|
|
|
#endif
|
|
|
|
}
|
|
|
|
};
|
|
|
|
|
2019-02-10 01:02:09 -04:00
|
|
|
static class GCS *get_singleton() {
|
2016-05-30 07:05:00 -03:00
|
|
|
return _singleton;
|
|
|
|
}
|
|
|
|
|
2019-02-12 07:54:24 -04:00
|
|
|
virtual uint32_t custom_mode() const = 0;
|
|
|
|
virtual MAV_TYPE frame_type() const = 0;
|
2019-03-01 02:07:45 -04:00
|
|
|
virtual const char* frame_string() const { return nullptr; }
|
2019-02-12 07:54:24 -04:00
|
|
|
|
2018-11-06 22:11:08 -04:00
|
|
|
void send_to_active_channels(uint32_t msgid, const char *pkt);
|
|
|
|
|
2019-07-23 05:33:56 -03:00
|
|
|
void send_text(MAV_SEVERITY severity, const char *fmt, ...) FMT_PRINTF(3, 4);
|
2017-08-01 02:13:34 -03:00
|
|
|
void send_textv(MAV_SEVERITY severity, const char *fmt, va_list arg_list);
|
2017-02-23 02:28:32 -04:00
|
|
|
virtual void send_statustext(MAV_SEVERITY severity, uint8_t dest_bitmask, const char *text);
|
2016-05-30 07:05:00 -03:00
|
|
|
void service_statustext(void);
|
2018-12-10 03:56:32 -04:00
|
|
|
virtual GCS_MAVLINK *chan(const uint8_t ofs) = 0;
|
|
|
|
virtual const GCS_MAVLINK *chan(const uint8_t ofs) const = 0;
|
|
|
|
// return the number of valid GCS objects
|
|
|
|
uint8_t num_gcs() const { return _num_gcs; };
|
2017-02-13 06:57:50 -04:00
|
|
|
void send_message(enum ap_message id);
|
|
|
|
void send_mission_item_reached_message(uint16_t mission_index);
|
2018-05-15 02:20:30 -03:00
|
|
|
void send_named_float(const char *name, float value) const;
|
2018-03-11 19:50:51 -03:00
|
|
|
|
|
|
|
void send_parameter_value(const char *param_name,
|
|
|
|
ap_var_type param_type,
|
|
|
|
float param_value);
|
|
|
|
|
2019-04-28 23:52:02 -03:00
|
|
|
static MissionItemProtocol_Waypoints *_missionitemprotocol_waypoints;
|
|
|
|
static MissionItemProtocol_Rally *_missionitemprotocol_rally;
|
2019-04-29 00:05:59 -03:00
|
|
|
static MissionItemProtocol_Fence *_missionitemprotocol_fence;
|
2019-04-28 23:52:02 -03:00
|
|
|
MissionItemProtocol *get_prot_for_mission_type(const MAV_MISSION_TYPE mission_type) const;
|
|
|
|
void try_send_queued_message_for_type(MAV_MISSION_TYPE type);
|
|
|
|
|
2018-05-20 23:18:57 -03:00
|
|
|
void update_send();
|
|
|
|
void update_receive();
|
2016-05-30 07:05:00 -03:00
|
|
|
|
2019-04-26 01:44:34 -03:00
|
|
|
// minimum amount of time (in microseconds) that must remain in
|
|
|
|
// the main scheduler loop before we are allowed to send any
|
|
|
|
// mavlink messages. We want to prioritise the main flight
|
|
|
|
// control loop over communications
|
|
|
|
virtual uint16_t min_loop_time_remaining_for_message_send_us() const {
|
|
|
|
return 200;
|
2017-08-20 23:32:25 -03:00
|
|
|
}
|
2018-12-10 03:56:32 -04:00
|
|
|
|
|
|
|
void setup_console();
|
|
|
|
void setup_uarts();
|
|
|
|
|
2019-04-26 01:44:34 -03:00
|
|
|
bool out_of_time() const;
|
2017-08-20 23:32:25 -03:00
|
|
|
|
2019-02-12 07:54:24 -04:00
|
|
|
// frsky backend
|
2019-06-18 04:32:48 -03:00
|
|
|
AP_Frsky_Telem *frsky;
|
2016-05-30 07:05:00 -03:00
|
|
|
|
2019-09-03 21:37:06 -03:00
|
|
|
#if !HAL_MINIMIZE_FEATURES
|
2019-02-13 19:11:52 -04:00
|
|
|
// Devo backend
|
|
|
|
AP_DEVO_Telem devo_telemetry;
|
2019-09-03 21:37:06 -03:00
|
|
|
#endif
|
2019-02-13 19:11:52 -04:00
|
|
|
|
2018-03-25 07:45:48 -03:00
|
|
|
// install an alternative protocol handler
|
|
|
|
bool install_alternative_protocol(mavlink_channel_t chan, GCS_MAVLINK::protocol_handler_fn_t handler);
|
2018-07-01 07:49:30 -03:00
|
|
|
|
|
|
|
// get the VFR_HUD throttle
|
2018-12-10 03:56:32 -04:00
|
|
|
int16_t get_hud_throttle(void) const { return num_gcs()>0?chan(0)->vfr_hud_throttle():0; }
|
2018-08-06 02:51:38 -03:00
|
|
|
|
2018-12-19 07:25:20 -04:00
|
|
|
// update uart pass-thru
|
|
|
|
void update_passthru();
|
|
|
|
|
2019-02-13 21:59:00 -04:00
|
|
|
void get_sensor_status_flags(uint32_t &present, uint32_t &enabled, uint32_t &health);
|
2019-03-01 07:50:05 -04:00
|
|
|
virtual bool vehicle_initialised() const { return true; }
|
2019-02-13 21:59:00 -04:00
|
|
|
|
2019-03-01 19:27:52 -04:00
|
|
|
virtual bool simple_input_active() const { return false; }
|
|
|
|
virtual bool supersimple_input_active() const { return false; }
|
|
|
|
|
2019-10-02 04:17:09 -03:00
|
|
|
// set message interval for a given serial port and message id
|
|
|
|
// this function is for use by lua scripts, most consumers should use the channel level function
|
2019-09-10 22:26:15 -03:00
|
|
|
MAV_RESULT set_message_interval(uint8_t port_num, uint32_t msg_id, int32_t interval_us);
|
2019-10-02 04:17:09 -03:00
|
|
|
|
2019-09-10 22:26:15 -03:00
|
|
|
uint8_t get_channel_from_port_number(uint8_t port_num);
|
|
|
|
|
2019-02-13 21:59:00 -04:00
|
|
|
protected:
|
|
|
|
|
2018-12-10 03:56:32 -04:00
|
|
|
virtual GCS_MAVLINK *new_gcs_mavlink_backend(GCS_MAVLINK_Parameters ¶ms,
|
|
|
|
AP_HAL::UARTDriver &uart) = 0;
|
|
|
|
|
2019-02-13 21:59:00 -04:00
|
|
|
uint32_t control_sensors_present;
|
|
|
|
uint32_t control_sensors_enabled;
|
|
|
|
uint32_t control_sensors_health;
|
2019-02-19 19:01:16 -04:00
|
|
|
virtual void update_vehicle_sensor_status_flags() {}
|
2019-02-13 20:39:32 -04:00
|
|
|
|
2018-12-10 03:56:32 -04:00
|
|
|
GCS_MAVLINK_Parameters chan_parameters[MAVLINK_COMM_NUM_BUFFERS];
|
|
|
|
uint8_t _num_gcs;
|
|
|
|
GCS_MAVLINK *_chan[MAVLINK_COMM_NUM_BUFFERS];
|
|
|
|
|
2016-05-30 07:05:00 -03:00
|
|
|
private:
|
|
|
|
|
|
|
|
static GCS *_singleton;
|
|
|
|
|
2018-12-10 03:56:32 -04:00
|
|
|
void create_gcs_mavlink_backend(GCS_MAVLINK_Parameters ¶ms,
|
|
|
|
AP_HAL::UARTDriver &uart);
|
|
|
|
|
2016-05-30 07:05:00 -03:00
|
|
|
struct statustext_t {
|
|
|
|
uint8_t bitmask;
|
|
|
|
mavlink_statustext_t msg;
|
|
|
|
};
|
|
|
|
|
2019-08-16 01:22:15 -03:00
|
|
|
void update_sensor_status_flags();
|
|
|
|
|
2019-09-29 20:08:44 -03:00
|
|
|
#if HAL_MEM_CLASS <= HAL_MEM_CLASS_192 || CONFIG_HAL_BOARD == HAL_BOARD_SITL
|
2016-05-30 07:05:00 -03:00
|
|
|
static const uint8_t _status_capacity = 5;
|
|
|
|
#else
|
|
|
|
static const uint8_t _status_capacity = 30;
|
|
|
|
#endif
|
|
|
|
|
2018-08-06 02:51:38 -03:00
|
|
|
// a lock for the statustext queue, to make it safe to use send_text()
|
|
|
|
// from multiple threads
|
|
|
|
HAL_Semaphore _statustext_sem;
|
|
|
|
|
|
|
|
// queue of outgoing statustext messages
|
2016-05-30 07:05:00 -03:00
|
|
|
ObjectArray<statustext_t> _statustext_queue{_status_capacity};
|
|
|
|
|
2019-04-28 23:52:02 -03:00
|
|
|
// true if we have already allocated protocol objects:
|
|
|
|
bool initialised_missionitemprotocol_objects;
|
|
|
|
|
2018-12-19 07:25:20 -04:00
|
|
|
// handle passthru between two UARTs
|
|
|
|
struct {
|
|
|
|
bool enabled;
|
|
|
|
bool timer_installed;
|
|
|
|
AP_HAL::UARTDriver *port1;
|
|
|
|
AP_HAL::UARTDriver *port2;
|
|
|
|
uint32_t start_ms;
|
|
|
|
uint32_t last_ms;
|
|
|
|
uint32_t last_port1_data_ms;
|
|
|
|
uint8_t timeout_s;
|
|
|
|
HAL_Semaphore sem;
|
|
|
|
} _passthru;
|
|
|
|
|
|
|
|
// timer called to implement pass-thru
|
|
|
|
void passthru_timer();
|
2020-02-25 00:23:52 -04:00
|
|
|
|
|
|
|
// this contains the index of the GCS_MAVLINK backend we will
|
|
|
|
// first call update_send on. It is incremented each time
|
|
|
|
// GCS::update_send is called so we don't starve later links of
|
|
|
|
// time in which they are permitted to send messages.
|
|
|
|
uint8_t first_backend_to_send;
|
2016-05-30 07:05:00 -03:00
|
|
|
};
|
|
|
|
|
|
|
|
GCS &gcs();
|
2020-05-11 00:59:17 -03:00
|
|
|
|
|
|
|
// send text when we do have a GCS
|
|
|
|
#define GCS_SEND_TEXT(severity, format, args...) gcs().send_text(severity, format, ##args)
|
|
|
|
|
|
|
|
#else // HAL_NO_GCS
|
|
|
|
// empty send text when we have no GCS
|
|
|
|
#define GCS_SEND_TEXT(severity, format, args...)
|
|
|
|
|
|
|
|
#endif // HAL_NO_GCS
|
|
|
|
|