Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-04 23:18:28 -04:00
Code Issues Packages Projects Releases Wiki Activity
14d1a075db
ardupilot/libraries/GCS_MAVLink/GCS.h
Peter Barker 14d1a075db GCS_MAVLink: remove internal error from chan(), correct bounds check 
It's been argued that callers should always be checking for nullptr anyway.  This is the method which is best qualified to work out whether the channel actually corresponds to a link, so it makes sense for the check to be here
2023-02-07 10:07:39 +11:00

1276 lines
50 KiB
C++
Raw Blame History

/// @file GCS.h
/// @brief Interface definition for the various Ground Control System
// protocols.
#pragma once
#include "GCS_config.h"
#if HAL_GCS_ENABLED
#include <AP_HAL/AP_HAL.h>
#include <AP_Common/AP_Common.h>
#include "GCS_MAVLink.h"
#include <AP_Mission/AP_Mission.h>
#include <stdint.h>
#include "MAVLink_routing.h"
#include <AP_RTC/JitterCorrection.h>
#include <AP_Common/Bitmask.h>
#include <AP_LTM_Telem/AP_LTM_Telem.h>
#include <AP_Devo_Telem/AP_Devo_Telem.h>
#include <AP_Filesystem/AP_Filesystem_config.h>
#include <AP_Frsky_Telem/AP_Frsky_config.h>
#include <AP_GPS/AP_GPS.h>
#include <AP_Mount/AP_Mount.h>
#include <AP_SerialManager/AP_SerialManager.h>
#include "ap_message.h"
#define GCS_DEBUG_SEND_MESSAGE_TIMINGS 0
#ifndef HAL_HIGH_LATENCY2_ENABLED
#define HAL_HIGH_LATENCY2_ENABLED 1
#endif
#ifndef HAL_MAVLINK_INTERVALS_FROM_FILES_ENABLED
#define HAL_MAVLINK_INTERVALS_FROM_FILES_ENABLED (HAVE_FILESYSTEM_SUPPORT && BOARD_FLASH_SIZE > 1024)
#endif
// macros used to determine if a message will fit in the space available.
void gcs_out_of_space_to_send(mavlink_channel_t chan);
bool check_payload_size(mavlink_channel_t chan, uint16_t max_payload_len);
// important note: despite the names, these messages do NOT check to
// see if the payload will fit in the buffer. They check to see if
// the packed message along with any channel overhead will fit.
// PAYLOAD_SIZE returns the amount of space required to send the
// mavlink message with id id on channel chan. Mavlink2 has higher
// overheads than mavlink1, for example.
// check if a message will fit in the payload space available
#define PAYLOAD_SIZE(chan, id) (unsigned(GCS_MAVLINK::packet_overhead_chan(chan)+MAVLINK_MSG_ID_ ## id ## _LEN))
// HAVE_PAYLOAD_SPACE evaluates to an expression that can be used
// anywhere in the code to determine if the mavlink message with ID id
// can currently fit in the output of _chan. Note the use of the ","
// operator here to increment a counter.
#define HAVE_PAYLOAD_SPACE(_chan, id) (comm_get_txspace(_chan) >= PAYLOAD_SIZE(_chan, id) ? true : (gcs_out_of_space_to_send(_chan), false))
// CHECK_PAYLOAD_SIZE - macro which may only be used within a
// GCS_MAVLink object's methods. It inserts code which will
// immediately return false from the current function if there is no
// room to fit the mavlink message with id id on the current object's
// output
#define CHECK_PAYLOAD_SIZE(id) if (!check_payload_size(MAVLINK_MSG_ID_ ## id ## _LEN)) return false
// CHECK_PAYLOAD_SIZE2 - macro which inserts code which will
// immediately return false from the current function if there is no
// room to fit the mavlink message with id id on the mavlink output
// channel "chan". It is expecting there to be a "chan" variable in
// scope.
#define CHECK_PAYLOAD_SIZE2(id) if (!HAVE_PAYLOAD_SPACE(chan, id)) return false
// CHECK_PAYLOAD_SIZE2_VOID - macro which inserts code which will
// immediately return from the current (void) function if there is no
// room to fit the mavlink message with id id on the mavlink output
// channel "chan".
#define CHECK_PAYLOAD_SIZE2_VOID(chan, id) if (!HAVE_PAYLOAD_SPACE(chan, id)) return
// 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) \
}
#define MAV_STREAM_TERMINATOR { (streams)0, nullptr, 0 }
// code generation; avoid each subclass duplicating these two methods
// and just changing the name. These methods allow retrieval of
// objects specific to the vehicle's subclass, which the vehicle can
// then call its own specific methods on
#define GCS_MAVLINK_CHAN_METHOD_DEFINITIONS(subclass_name) \
subclass_name *chan(const uint8_t ofs) override { \
if (ofs >= _num_gcs) { \
return nullptr; \
} \
return (subclass_name *)_chan[ofs]; \
} \
\
const subclass_name *chan(const uint8_t ofs) const override { \
if (ofs >= _num_gcs) { \
return nullptr; \
} \
return (subclass_name *)_chan[ofs]; \
}
#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];
};
#if HAL_MAVLINK_INTERVALS_FROM_FILES_ENABLED
class DefaultIntervalsFromFiles
{
public:
DefaultIntervalsFromFiles(uint16_t max_num);
~DefaultIntervalsFromFiles();
void set(ap_message id, uint16_t interval);
uint16_t num_intervals() const {
return _num_intervals;
}
bool get_interval_for_ap_message_id(ap_message id, uint16_t &interval) const;
ap_message id_at(uint8_t ofs) const;
uint16_t interval_at(uint8_t ofs) const;
private:
struct from_file_default_interval {
ap_message id;
uint16_t interval;
};
from_file_default_interval *_intervals;
uint16_t _num_intervals;
uint16_t _max_intervals;
};
#endif
///
/// @class GCS_MAVLINK
/// @brief MAVLink transport control class
///
class GCS_MAVLINK
{
public:
friend class GCS;
GCS_MAVLINK(GCS_MAVLINK_Parameters &parameters, AP_HAL::UARTDriver &uart);
virtual ~GCS_MAVLINK() {}
void update_receive(uint32_t max_time_us=1000);
void update_send();
bool init(uint8_t instance);
void send_message(enum ap_message id);
void send_text(MAV_SEVERITY severity, const char *fmt, ...) const FMT_PRINTF(3, 4);
void queued_param_send();
void queued_mission_request_send();
bool sending_mavlink1() const;
// returns true if we are requesting any items from the GCS:
bool requesting_mission_items() const;
/// Check for available transmit space
uint16_t txspace() const {
if (_locked) {
return 0;
}
// there were concerns over return a too-large value for
// txspace (in case we tried to do too much with the space in
// a single loop):
return MIN(_port->txspace(), 8192U);
}
bool check_payload_size(uint16_t max_payload_len);
// this is called when we discover we'd like to send something but can't:
void out_of_space_to_send() { out_of_space_to_send_count++; }
void send_mission_ack(const mavlink_message_t &msg,
MAV_MISSION_TYPE mission_type,
MAV_MISSION_RESULT result) const {
CHECK_PAYLOAD_SIZE2_VOID(chan, MISSION_ACK);
mavlink_msg_mission_ack_send(chan,
msg.sysid,
msg.compid,
result,
mission_type);
}
// packetReceived is called on any successful decode of a mavlink message
virtual void packetReceived(const mavlink_status_t &status,
const mavlink_message_t &msg);
// 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);
}
// accessor for uart
AP_HAL::UARTDriver *get_uart() { return _port; }
virtual uint8_t sysid_my_gcs() const = 0;
virtual bool sysid_enforce() const { return false; }
// NOTE: param_name here must point to a 16+1 byte buffer - so do
// NOT try to pass in a static-char-* unless it does have that
// length!
void send_parameter_value(const char *param_name,
ap_var_type param_type,
float param_value);
// NOTE! The streams enum below and the
// set of AP_Int16 stream rates _must_ be
// kept in the same order
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
};
// 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");
bool is_high_bandwidth() { return chan == MAVLINK_COMM_0; }
// return true if this channel has hardware flow control
bool have_flow_control();
bool is_active() const {
return GCS_MAVLINK::active_channel_mask() & (1 << (chan-MAVLINK_COMM_0));
}
bool is_streaming() const {
return sending_bucket_id != no_bucket_to_send;
}
mavlink_channel_t get_chan() const { return chan; }
uint32_t get_last_heartbeat_time() const { return last_heartbeat_time; };
uint32_t last_heartbeat_time; // milliseconds
static uint32_t last_radio_status_remrssi_ms() {
return last_radio_status.remrssi_ms;
}
static float telemetry_radio_rssi(); // 0==no signal, 1==full signal
// mission item index to be sent on queued msg, delayed or not
uint16_t mission_item_reached_index = AP_MISSION_CMD_INDEX_NONE;
// common send functions
void send_heartbeat(void) const;
void send_meminfo(void);
void send_fence_status() const;
void send_power_status(void);
#if HAL_WITH_MCU_MONITORING
void send_mcu_status(void);
#endif
void send_battery_status(const uint8_t instance) const;
bool send_battery_status();
void send_distance_sensor();
// send_rangefinder sends only if a downward-facing instance is
// found. Rover overrides this!
virtual void send_rangefinder() const;
void send_proximity();
virtual void send_nav_controller_output() const = 0;
virtual void send_pid_tuning() = 0;
void send_ahrs2();
void send_system_time() const;
void send_rc_channels() const;
void send_rc_channels_raw() const;
void send_raw_imu();
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, int16_t temperature_press_diff));
void send_scaled_pressure();
void send_scaled_pressure2();
virtual void send_scaled_pressure3(); // allow sub to override this
void send_simstate() const;
void send_sim_state() const;
void send_ahrs();
#if AP_MAVLINK_BATTERY2_ENABLED
void send_battery2();
#endif
void send_opticalflow();
virtual void send_attitude() const;
virtual void send_attitude_quaternion() const;
void send_autopilot_version() const;
void send_extended_sys_state() const;
void send_local_position() const;
void send_vfr_hud();
void send_vibration() const;
void send_gimbal_device_attitude_status() const;
void send_named_float(const char *name, float value) const;
void send_home_position() const;
void send_gps_global_origin() const;
virtual void send_attitude_target() {};
virtual void send_position_target_global_int() { };
virtual void send_position_target_local_ned() { };
void send_servo_output_raw();
void send_accelcal_vehicle_position(uint32_t position);
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));
void send_sys_status();
void send_set_position_target_global_int(uint8_t target_system, uint8_t target_component, const Location& loc);
void send_rpm() const;
void send_generator_status() const;
virtual void send_winch_status() const {};
void send_water_depth() const;
int8_t battery_remaining_pct(const uint8_t instance) const;
#if HAL_HIGH_LATENCY2_ENABLED
void send_high_latency2() const;
#endif // HAL_HIGH_LATENCY2_ENABLED
void send_uavionix_adsb_out_status() const;
void send_autopilot_state_for_gimbal_device() const;
// lock a channel, preventing use by MAVLink
void lock(bool _lock) {
_locked = _lock;
}
// returns true if this channel isn't available for MAVLink
bool locked() const {
return _locked;
}
// 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; }
// return a bitmap of streaming channels
static uint8_t streaming_channel_mask(void) { return chan_is_streaming; }
// return a bitmap of private channels
static uint8_t private_channel_mask(void) { return mavlink_private; }
// 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); }
#if HAL_HIGH_LATENCY2_ENABLED
// return true if the link should be sending. Will return false if is a high latency link AND is not active
bool should_send() { return is_high_latency_link ? high_latency_link_enabled : true; }
#endif
/*
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
*/
static void send_to_components(uint32_t msgid, const char *pkt, uint8_t pkt_len) { routing.send_to_components(msgid, pkt, pkt_len); }
/*
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)); }
/*
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); }
/*
search for the first vehicle or component in the routing table with given mav_type and component id and retrieve its sysid and channel
returns true if a match is found
*/
static bool find_by_mavtype_and_compid(uint8_t mav_type, uint8_t compid, uint8_t &sysid, mavlink_channel_t &channel) { return routing.find_by_mavtype_and_compid(mav_type, compid, sysid, channel); }
// update signing timestamp on GPS lock
static void update_signing_timestamp(uint64_t timestamp_usec);
// return current packet overhead for a channel
static uint8_t packet_overhead_chan(mavlink_channel_t chan);
// alternative protocol function handler
FUNCTOR_TYPEDEF(protocol_handler_fn_t, bool, uint8_t, AP_HAL::UARTDriver *);
struct stream_entries {
const streams stream_id;
const ap_message *ap_message_ids;
const uint8_t num_ap_message_ids;
};
// vehicle subclass cpp files should define this:
static const struct stream_entries all_stream_entries[];
virtual uint64_t capabilities() const;
uint16_t get_stream_slowdown_ms() const { return stream_slowdown_ms; }
uint8_t get_last_txbuf() const { return last_txbuf; }
MAV_RESULT set_message_interval(uint32_t msg_id, int32_t interval_us);
protected:
bool mavlink_coordinate_frame_to_location_alt_frame(MAV_FRAME coordinate_frame,
Location::AltFrame &frame);
// overridable method to check for packet acceptance. Allows for
// enforcement of GCS sysid
bool accept_packet(const mavlink_status_t &status, const mavlink_message_t &msg) const;
void set_ekf_origin(const Location& loc);
virtual MAV_MODE base_mode() const = 0;
MAV_STATE system_status() const;
virtual MAV_STATE vehicle_system_status() const = 0;
virtual MAV_VTOL_STATE vtol_state() const { return MAV_VTOL_STATE_UNDEFINED; }
virtual MAV_LANDED_STATE landed_state() const { return MAV_LANDED_STATE_UNDEFINED; }
// return a MAVLink parameter type given a AP_Param type
static MAV_PARAM_TYPE mav_param_type(enum ap_var_type t);
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
AP_Int16 *streamRates;
void handle_heartbeat(const mavlink_message_t &msg) const;
virtual bool persist_streamrates() const { return false; }
void handle_request_data_stream(const mavlink_message_t &msg);
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);
MAV_RESULT handle_command_int_do_set_home(const mavlink_command_int_t &packet);
virtual MAV_RESULT handle_command_int_packet(const mavlink_command_int_t &packet);
virtual bool set_home_to_current_location(bool lock) = 0;
virtual bool set_home(const Location& loc, bool lock) = 0;
virtual MAV_RESULT handle_command_component_arm_disarm(const mavlink_command_long_t &packet);
MAV_RESULT handle_command_do_set_home(const mavlink_command_long_t &packet);
MAV_RESULT handle_command_do_aux_function(const mavlink_command_long_t &packet);
void handle_mission_request_list(const mavlink_message_t &msg);
void handle_mission_request(const mavlink_message_t &msg) const;
void handle_mission_request_int(const mavlink_message_t &msg) const;
void handle_mission_clear_all(const mavlink_message_t &msg) const;
// Note that there exists a relatively new mavlink DO command,
// MAV_CMD_DO_SET_MISSION_CURRENT which provides an acknowledgement
// that the command has been received, rather than the GCS having to
// rely on getting back an identical sequence number as some currently
// do.
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_distance_sensor(const mavlink_message_t &msg);
void handle_obstacle_distance(const mavlink_message_t &msg);
void handle_obstacle_distance_3d(const mavlink_message_t &msg);
void handle_adsb_message(const mavlink_message_t &msg);
void handle_osd_param_config(const mavlink_message_t &msg) const;
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);
virtual bool params_ready() const { return true; }
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) const;
#if HAL_MOUNT_ENABLED
virtual void handle_mount_message(const mavlink_message_t &msg);
#endif
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) const;
virtual MAV_RESULT handle_preflight_reboot(const mavlink_command_long_t &packet, const mavlink_message_t &msg);
// reset a message interval via mavlink:
MAV_RESULT handle_command_set_message_interval(const mavlink_command_long_t &packet);
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;
MAV_RESULT handle_command_request_message(const mavlink_command_long_t &packet);
MAV_RESULT handle_rc_bind(const mavlink_command_long_t &packet);
virtual MAV_RESULT handle_flight_termination(const mavlink_command_long_t &packet);
void handle_send_autopilot_version(const mavlink_message_t &msg);
MAV_RESULT handle_command_request_autopilot_capabilities(const mavlink_command_long_t &packet);
virtual void send_banner();
// send a (textual) message to the GCS that a received message has
// been deprecated
void send_received_message_deprecation_warning(const char *message);
void handle_device_op_read(const mavlink_message_t &msg);
void handle_device_op_write(const mavlink_message_t &msg);
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;
void handle_timesync(const mavlink_message_t &msg);
struct {
int64_t sent_ts1;
uint32_t last_sent_ms;
const uint16_t interval_ms = 10000;
} _timesync_request;
void handle_statustext(const mavlink_message_t &msg) const;
void handle_named_value(const mavlink_message_t &msg) const;
bool telemetry_delayed() const;
virtual uint32_t telem_delay() const = 0;
MAV_RESULT handle_command_run_prearm_checks(const mavlink_command_long_t &packet);
MAV_RESULT handle_command_preflight_set_sensor_offsets(const mavlink_command_long_t &packet);
MAV_RESULT handle_command_flash_bootloader(const mavlink_command_long_t &packet);
// generally this should not be overridden; Plane overrides it to ensure
// failsafe isn't triggered during calibration
virtual MAV_RESULT handle_command_preflight_calibration(const mavlink_command_long_t &packet);
virtual MAV_RESULT _handle_command_preflight_calibration(const mavlink_command_long_t &packet);
virtual MAV_RESULT _handle_command_preflight_calibration_baro();
MAV_RESULT handle_command_preflight_can(const mavlink_command_long_t &packet);
virtual MAV_RESULT handle_command_do_set_mission_current(const mavlink_command_long_t &packet);
MAV_RESULT handle_command_battery_reset(const mavlink_command_long_t &packet);
void handle_command_long(const mavlink_message_t &msg);
MAV_RESULT handle_command_accelcal_vehicle_pos(const mavlink_command_long_t &packet);
MAV_RESULT handle_command_do_set_roi_sysid(const uint8_t sysid);
MAV_RESULT handle_command_do_set_roi_sysid(const mavlink_command_int_t &packet);
MAV_RESULT handle_command_do_set_roi_sysid(const mavlink_command_long_t &packet);
virtual MAV_RESULT handle_command_mount(const mavlink_command_long_t &packet);
MAV_RESULT handle_command_mag_cal(const mavlink_command_long_t &packet);
virtual MAV_RESULT handle_command_long_packet(const mavlink_command_long_t &packet);
MAV_RESULT handle_command_camera(const mavlink_command_long_t &packet);
MAV_RESULT handle_command_do_send_banner(const mavlink_command_long_t &packet);
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);
MAV_RESULT handle_command_do_gripper(const mavlink_command_long_t &packet);
MAV_RESULT handle_command_do_sprayer(const mavlink_command_long_t &packet);
MAV_RESULT handle_command_do_set_mode(const mavlink_command_long_t &packet);
MAV_RESULT handle_command_get_home_position(const mavlink_command_long_t &packet);
MAV_RESULT handle_command_do_fence_enable(const mavlink_command_long_t &packet);
MAV_RESULT handle_command_debug_trap(const mavlink_command_long_t &packet);
MAV_RESULT handle_command_set_ekf_source_set(const mavlink_command_long_t &packet);
/*
handle MAV_CMD_CAN_FORWARD and CAN_FRAME messages for CAN over MAVLink
*/
void can_frame_callback(uint8_t bus, const AP_HAL::CANFrame &);
MAV_RESULT handle_can_forward(const mavlink_command_long_t &packet, const mavlink_message_t &msg);
void handle_can_frame(const mavlink_message_t &msg) const;
void handle_optical_flow(const mavlink_message_t &msg);
MAV_RESULT handle_fixed_mag_cal_yaw(const mavlink_command_long_t &packet);
void handle_manual_control(const mavlink_message_t &msg);
// default empty handling of LANDING_TARGET
virtual void handle_landing_target(const mavlink_landing_target_t &packet, uint32_t timestamp_ms) { }
// vehicle-overridable message send function
virtual bool try_send_message(enum ap_message id);
virtual void send_global_position_int();
// message sending functions:
bool try_send_mission_message(enum ap_message id);
void send_hwstatus();
void handle_data_packet(const mavlink_message_t &msg);
// 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;
virtual float vfr_hud_climbrate() const;
virtual float vfr_hud_airspeed() const;
virtual int16_t vfr_hud_throttle() const { return 0; }
virtual float vfr_hud_alt() const;
#if HAL_HIGH_LATENCY2_ENABLED
virtual int16_t high_latency_target_altitude() const { return 0; }
virtual uint8_t high_latency_tgt_heading() const { return 0; }
virtual uint16_t high_latency_tgt_dist() const { return 0; }
virtual uint8_t high_latency_tgt_airspeed() const { return 0; }
virtual uint8_t high_latency_wind_speed() const { return 0; }
virtual uint8_t high_latency_wind_direction() const { return 0; }
int8_t high_latency_air_temperature() const;
MAV_RESULT handle_control_high_latency(const mavlink_command_long_t &packet);
// true if this is a high latency link
bool is_high_latency_link;
bool high_latency_link_enabled;
#endif // HAL_HIGH_LATENCY2_ENABLED
static constexpr const float magic_force_arm_value = 2989.0f;
static constexpr const float magic_force_disarm_value = 21196.0f;
void manual_override(class RC_Channel *c, int16_t value_in, uint16_t offset, float scaler, const uint32_t tnow, bool reversed = false);
uint8_t receiver_rssi() const;
/*
correct an offboard timestamp in microseconds to a local time
since boot in milliseconds
*/
uint32_t correct_offboard_timestamp_usec_to_ms(uint64_t offboard_usec, uint16_t payload_size);
// converts a COMMAND_LONG packet to a COMMAND_INT packet, where
// the command-long packet is assumed to be in the supplied frame.
// If location is not present in the command then just omit frame.
static void convert_COMMAND_LONG_to_COMMAND_INT(const mavlink_command_long_t &in, mavlink_command_int_t &out, MAV_FRAME frame = MAV_FRAME_GLOBAL_RELATIVE_ALT);
// methods to extract a Location object from a command_long or command_int
bool location_from_command_t(const mavlink_command_long_t &in, MAV_FRAME in_frame, Location &out);
bool location_from_command_t(const mavlink_command_int_t &in, Location &out);
private:
const AP_SerialManager::UARTState *uartstate;
// last time we got a non-zero RSSI from RADIO_STATUS
static struct LastRadioStatus {
uint32_t remrssi_ms;
uint8_t rssi;
uint32_t received_ms; // time RADIO_STATUS received
} last_radio_status;
enum class Flags {
USING_SIGNING = (1<<0),
ACTIVE = (1<<1),
STREAMING = (1<<2),
PRIVATE = (1<<3),
LOCKED = (1<<4),
};
void log_mavlink_stats();
uint32_t last_accel_cal_ms; // used to rate limit accel cals for bad links
MAV_RESULT _set_mode_common(const MAV_MODE base_mode, const uint32_t custom_mode);
// send a (textual) message to the GCS that a received message has
// been deprecated
uint32_t last_deprecation_warning_send_time_ms;
const char *last_deprecation_message;
void service_statustext(void);
virtual void handleMessage(const mavlink_message_t &msg) = 0;
MAV_RESULT handle_servorelay_message(const mavlink_command_long_t &packet);
static bool command_long_stores_location(const MAV_CMD command);
bool calibrate_gyros();
/// The stream we are communicating over
AP_HAL::UARTDriver *_port;
/// 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
uint32_t _queued_parameter_send_time_ms;
// number of extra ms to add to slow things down for the radio
uint16_t stream_slowdown_ms;
// last reported radio buffer percent available
uint8_t last_txbuf = 100;
// outbound ("deferred message") queue.
// "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.
struct deferred_message_t {
const ap_message id;
uint16_t interval_ms;
uint16_t last_sent_ms; // from AP_HAL::millis16()
} deferred_message[3] = {
{ MSG_HEARTBEAT, },
{ MSG_NEXT_PARAM, },
{ MSG_HIGH_LATENCY2, },
};
// 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(uint16_t now16_ms);
// cache of which deferred message should be sent next:
int8_t next_deferred_message_to_send_cache = -1;
struct deferred_message_bucket_t {
Bitmask<MSG_LAST> ap_message_ids;
uint16_t interval_ms;
uint16_t last_sent_ms; // from AP_HAL::millis16()
};
deferred_message_bucket_t deferred_message_bucket[10];
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;
Bitmask<MSG_LAST> bucket_message_ids_to_send;
ap_message next_deferred_bucket_message_to_send(uint16_t now16_ms);
void find_next_bucket_to_send(uint16_t now16_ms);
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)
Bitmask<MSG_LAST> pushed_ap_message_ids;
// 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
// 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;
#if HAL_MAVLINK_INTERVALS_FROM_FILES_ENABLED
// read configuration files from (e.g.) SD and ROMFS, set
// intervals from same
void initialise_message_intervals_from_config_files();
// read file, set message intervals from it:
void get_intervals_from_filepath(const char *path, DefaultIntervalsFromFiles &);
#endif
// 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);
// time when we missed sending a parameter for GCS
static uint32_t reserve_param_space_start_ms;
// bitmask of what mavlink channels are active
static uint8_t mavlink_active;
// bitmask of what mavlink channels are private
static uint8_t mavlink_private;
// bitmask of what mavlink channels are streaming
static uint8_t chan_is_streaming;
// mavlink routing object
static MAVLink_routing routing;
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);
uint8_t send_parameter_async_replies();
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;
// 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;
uint32_t last_send_ms;
uint8_t need_banner_send_mask;
};
static struct ftp_state ftp;
static void ftp_error(struct pending_ftp &response, FTP_ERROR error); // FTP helper method for packing a NAK
static int gen_dir_entry(char *dest, size_t space, const char * path, const struct dirent * entry); // FTP helper for emitting a dir response
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);
bool send_ftp_reply(const pending_ftp &reply);
void ftp_worker(void);
void ftp_push_replies(pending_ftp &reply);
void send_distance_sensor(const class AP_RangeFinder_Backend *sensor, const uint8_t instance) const;
virtual bool handle_guided_request(AP_Mission::Mission_Command &cmd) = 0;
virtual void handle_change_alt_request(AP_Mission::Mission_Command &cmd) {};
void handle_common_mission_message(const mavlink_message_t &msg);
virtual void handle_manual_control_axes(const mavlink_manual_control_t &packet, const uint32_t tnow) {};
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);
void handle_odometry(const mavlink_message_t &msg);
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,
const float yaw,
const float covariance[21],
const uint8_t reset_counter,
const uint16_t payload_size);
void handle_vision_speed_estimate(const mavlink_message_t &msg);
void handle_landing_target(const mavlink_message_t &msg);
void lock_channel(const mavlink_channel_t chan, bool lock);
mavlink_signing_t signing;
static mavlink_signing_streams_t signing_streams;
static uint32_t last_signing_save_ms;
static StorageAccess _signing_storage;
static bool signing_key_save(const struct SigningKey &key);
static bool signing_key_load(struct SigningKey &key);
void load_signing_key(void);
bool signing_enabled(void) const;
static void save_signing_timestamp(bool force_save_now);
#if HAL_MAVLINK_INTERVALS_FROM_FILES_ENABLED
// structure containing default intervals read from files for this
// link:
DefaultIntervalsFromFiles *default_intervals_from_files;
#endif
// 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;
JitterCorrection lag_correction;
// we cache the current location and send it even if the AHRS has
// no idea where we are:
Location global_position_current_loc;
uint8_t last_tx_seq;
uint16_t send_packet_count;
uint16_t out_of_space_to_send_count; // number of times HAVE_PAYLOAD_SPACE and friends have returned false
#if GCS_DEBUG_SEND_MESSAGE_TIMINGS
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;
uint32_t out_of_time;
uint16_t fnbts_maxtime;
uint32_t max_retry_deferred_body_us;
uint8_t max_retry_deferred_body_type;
} try_send_message_stats;
uint16_t max_slowdown_ms;
#endif
uint32_t last_mavlink_stats_logged;
uint8_t last_battery_status_idx;
// if we've ever sent a DISTANCE_SENSOR message out of an
// orientation we continue to send it out, even if it is not
// longer valid.
uint8_t proximity_ever_valid_bitmask;
// true if we should NOT do MAVLink on this port (usually because
// someone's doing SERIAL_CONTROL over mavlink)
bool _locked;
};
/// @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
}
};
static class GCS *get_singleton() {
return _singleton;
}
virtual uint32_t custom_mode() const = 0;
virtual MAV_TYPE frame_type() const = 0;
virtual const char* frame_string() const { return nullptr; }
struct statustext_t {
mavlink_statustext_t msg;
uint16_t entry_created_ms;
uint8_t bitmask;
};
class StatusTextQueue : public ObjectArray<statustext_t> {
public:
using ObjectArray::ObjectArray;
HAL_Semaphore &semaphore() { return _sem; }
void prune();
private:
// a lock for the statustext queue, to make it safe to use send_text()
// from multiple threads
HAL_Semaphore _sem;
uint32_t last_prune_ms;
};
StatusTextQueue &statustext_queue() {
return _statustext_queue;
}
// last time traffic was seen from my designated GCS. traffic
// includes heartbeats and some manual control messages.
uint32_t sysid_myggcs_last_seen_time_ms() const {
return _sysid_mygcs_last_seen_time_ms;
}
// called when valid traffic has been seen from our GCS
void sysid_myggcs_seen(uint32_t seen_time_ms) {
_sysid_mygcs_last_seen_time_ms = seen_time_ms;
}
void send_to_active_channels(uint32_t msgid, const char *pkt);
void send_text(MAV_SEVERITY severity, const char *fmt, ...) FMT_PRINTF(3, 4);
void send_textv(MAV_SEVERITY severity, const char *fmt, va_list arg_list);
virtual void send_textv(MAV_SEVERITY severity, const char *fmt, va_list arg_list, uint8_t mask);
uint8_t statustext_send_channel_mask() const;
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; };
void send_message(enum ap_message id);
void send_mission_item_reached_message(uint16_t mission_index);
void send_named_float(const char *name, float value) const;
void send_parameter_value(const char *param_name,
ap_var_type param_type,
float param_value);
// an array of objects used to handle each of the different
// protocol types we support. This is indexed by the enumeration
// MAV_MISSION_TYPE, taking advantage of the fact that fence,
// mission and rally have values 0, 1 and 2. Indexing should be via
// get_prot_for_mission_type to do bounds checking.
static class MissionItemProtocol *missionitemprotocols[3];
class MissionItemProtocol *get_prot_for_mission_type(const MAV_MISSION_TYPE mission_type) const;
void try_send_queued_message_for_type(MAV_MISSION_TYPE type) const;
void update_send();
void update_receive();
// 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;
}
void init();
void setup_console();
void setup_uarts();
bool out_of_time() const;
#if AP_FRSKY_TELEM_ENABLED
// frsky backend
class AP_Frsky_Telem *frsky;
#endif
#if AP_LTM_TELEM_ENABLED
// LTM backend
AP_LTM_Telem ltm_telemetry;
#endif
#if AP_DEVO_TELEM_ENABLED
// Devo backend
AP_DEVO_Telem devo_telemetry;
#endif
// install an alternative protocol handler
bool install_alternative_protocol(mavlink_channel_t chan, GCS_MAVLINK::protocol_handler_fn_t handler);
// get the VFR_HUD throttle
int16_t get_hud_throttle(void) const { return num_gcs()>0?chan(0)->vfr_hud_throttle():0; }
// update uart pass-thru
void update_passthru();
void get_sensor_status_flags(uint32_t &present, uint32_t &enabled, uint32_t &health);
virtual bool vehicle_initialised() const { return true; }
virtual bool simple_input_active() const { return false; }
virtual bool supersimple_input_active() const { return false; }
// 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
MAV_RESULT set_message_interval(uint8_t port_num, uint32_t msg_id, int32_t interval_us);
uint8_t get_channel_from_port_number(uint8_t port_num);
#if HAL_HIGH_LATENCY2_ENABLED
void enable_high_latency_connections(bool enabled);
#endif // HAL_HIGH_LATENCY2_ENABLED
virtual uint8_t sysid_this_mav() const = 0;
protected:
virtual GCS_MAVLINK *new_gcs_mavlink_backend(GCS_MAVLINK_Parameters &params,
AP_HAL::UARTDriver &uart) = 0;
uint32_t control_sensors_present;
uint32_t control_sensors_enabled;
uint32_t control_sensors_health;
virtual void update_vehicle_sensor_status_flags() {}
GCS_MAVLINK_Parameters chan_parameters[MAVLINK_COMM_NUM_BUFFERS];
uint8_t _num_gcs;
GCS_MAVLINK *_chan[MAVLINK_COMM_NUM_BUFFERS];
private:
static GCS *_singleton;
void create_gcs_mavlink_backend(GCS_MAVLINK_Parameters &params,
AP_HAL::UARTDriver &uart);
char statustext_printf_buffer[256+1];
virtual AP_GPS::GPS_Status min_status_for_gps_healthy() const {
// NO_FIX simply excludes NO_GPS
return AP_GPS::GPS_Status::NO_FIX;
}
void update_sensor_status_flags();
// time we last saw traffic from our GCS
uint32_t _sysid_mygcs_last_seen_time_ms;
void service_statustext(void);
#if HAL_MEM_CLASS <= HAL_MEM_CLASS_192 || CONFIG_HAL_BOARD == HAL_BOARD_SITL
static const uint8_t _status_capacity = 7;
#else
static const uint8_t _status_capacity = 30;
#endif
// queue of outgoing statustext messages. Each entry consumes 58
// bytes of RAM on stm32
StatusTextQueue _statustext_queue{_status_capacity};
// true if we have already allocated protocol objects:
bool initialised_missionitemprotocol_objects;
// true if update_send has ever been called:
bool update_send_has_been_called;
// 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;
uint32_t baud1;
uint32_t baud2;
uint8_t timeout_s;
HAL_Semaphore sem;
} _passthru;
// timer called to implement pass-thru
void passthru_timer();
// 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;
};
GCS &gcs();
// send text when we do have a GCS
#if !defined(HAL_BUILD_AP_PERIPH)
#define GCS_SEND_TEXT(severity, format, args...) gcs().send_text(severity, format, ##args)
#else
extern "C" {
void can_printf(const char *fmt, ...);
}
#define GCS_SEND_TEXT(severity, format, args...) (void)severity; can_printf(format, ##args)
#endif
#elif defined(HAL_BUILD_AP_PERIPH) && !defined(STM32F1)
// map send text to can_printf() on larger AP_Periph boards
extern "C" {
void can_printf(const char *fmt, ...);
}
#define GCS_SEND_TEXT(severity, format, args...) can_printf(format, ##args)
#else // HAL_GCS_ENABLED
// empty send text when we have no GCS
#define GCS_SEND_TEXT(severity, format, args...)
#endif // HAL_GCS_ENABLED
Reference in New Issue View Git Blame Copy Permalink