ardupilot/libraries/AP_RCTelemetry/AP_CRSF_Telem.h

375 lines
11 KiB
C++

/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#include <AP_HAL/AP_HAL_Boards.h>
#include <AP_OSD/AP_OSD.h>
#ifndef HAL_CRSF_TELEM_ENABLED
#define HAL_CRSF_TELEM_ENABLED !HAL_MINIMIZE_FEATURES
#endif
#ifndef HAL_CRSF_TELEM_TEXT_SELECTION_ENABLED
#define HAL_CRSF_TELEM_TEXT_SELECTION_ENABLED OSD_ENABLED && OSD_PARAM_ENABLED && HAL_CRSF_TELEM_ENABLED && BOARD_FLASH_SIZE > 1024
#endif
#if HAL_CRSF_TELEM_ENABLED
#include <AP_RCProtocol/AP_RCProtocol_CRSF.h>
#include "AP_RCTelemetry.h"
#include <AP_HAL/utility/sparse-endian.h>
class AP_OSD_ParamSetting;
class AP_CRSF_Telem : public AP_RCTelemetry {
public:
AP_CRSF_Telem();
~AP_CRSF_Telem() override;
/* Do not allow copies */
AP_CRSF_Telem(const AP_CRSF_Telem &other) = delete;
AP_CRSF_Telem &operator=(const AP_CRSF_Telem&) = delete;
// init - perform required initialisation
virtual bool init() override;
static AP_CRSF_Telem *get_singleton(void);
void queue_message(MAV_SEVERITY severity, const char *text) override;
static const uint8_t PASSTHROUGH_STATUS_TEXT_FRAME_MAX_SIZE = 50U;
static const uint8_t PASSTHROUGH_MULTI_PACKET_FRAME_MAX_SIZE = 9U;
static const uint8_t CRSF_RX_DEVICE_PING_MAX_RETRY = 50U;
// Broadcast frame definitions courtesy of TBS
struct PACKED GPSFrame { // curious fact, calling this GPS makes sizeof(GPS) return 1!
int32_t latitude; // ( degree / 10`000`000 )
int32_t longitude; // (degree / 10`000`000 )
uint16_t groundspeed; // ( km/h / 100 )
uint16_t gps_heading; // ( degree / 100 )
uint16_t altitude; // ( meter - 1000m offset )
uint8_t satellites; // in use ( counter )
};
struct HeartbeatFrame {
uint8_t origin; // Device addres
};
struct PACKED BatteryFrame {
uint16_t voltage; // ( mV * 100 )
uint16_t current; // ( mA * 100 )
uint8_t capacity[3]; // ( mAh )
uint8_t remaining; // ( percent )
};
struct PACKED VTXFrame {
#if __BYTE_ORDER != __LITTLE_ENDIAN
#error "Only supported on little-endian architectures"
#endif
uint8_t origin; // address
// status
uint8_t is_in_pitmode : 1;
uint8_t is_in_user_frequency_mode : 1;
uint8_t unused : 2;
uint8_t is_vtx_available : 1;
uint8_t smart_audio_ver : 3; // SmartAudio_V1 = 0, SmartAudio_V2 = 1
// band / channel
uint8_t channel : 3; // 1x-8x
uint8_t band : 5; // A, B, E, AirWave, Race
uint16_t user_frequency;
uint8_t power : 4; // 25mW = 0, 200mW = 1, 500mW = 2, 800mW = 3
uint8_t pitmode : 4; // off = 0, In_Band = 1, Out_Band = 2;
};
struct PACKED VTXTelemetryFrame {
uint8_t origin; // address
uint8_t power; // power in dBm
uint16_t frequency; // frequency in Mhz
uint8_t pitmode; // disable 0, enable 1
};
struct PACKED AttitudeFrame {
int16_t pitch_angle; // ( rad * 10000 )
int16_t roll_angle; // ( rad * 10000 )
int16_t yaw_angle; // ( rad * 10000 )
};
struct PACKED FlightModeFrame {
char flight_mode[16]; // ( Null-terminated string )
};
// CRSF_FRAMETYPE_COMMAND
struct PACKED CommandFrame {
uint8_t destination;
uint8_t origin;
uint8_t command_id;
uint8_t payload[9]; // 8 maximum for LED command + crc8
};
// CRSF_FRAMETYPE_PARAM_DEVICE_PING
struct PACKED ParameterPingFrame {
uint8_t destination;
uint8_t origin;
};
// CRSF_FRAMETYPE_PARAM_DEVICE_INFO
struct PACKED ParameterDeviceInfoFrame {
uint8_t destination;
uint8_t origin;
uint8_t payload[58]; // largest possible frame is 60
};
enum ParameterType : uint8_t
{
UINT8 = 0,
INT8 = 1,
UINT16 = 2,
INT16 = 3,
FLOAT = 8,
TEXT_SELECTION = 9,
STRING = 10,
FOLDER = 11,
INFO = 12,
COMMAND = 13,
OUT_OF_RANGE = 127
};
// CRSF_FRAMETYPE_PARAMETER_SETTINGS_ENTRY
struct PACKED ParameterSettingsEntryHeader {
uint8_t destination;
uint8_t origin;
uint8_t param_num;
uint8_t chunks_left;
};
// CRSF_FRAMETYPE_PARAMETER_SETTINGS_ENTRY
struct PACKED ParameterSettingsEntry {
ParameterSettingsEntryHeader header;
uint8_t payload[56]; // largest possible frame is 60
};
// CRSF_FRAMETYPE_PARAMETER_READ
struct PACKED ParameterSettingsReadFrame {
uint8_t destination;
uint8_t origin;
uint8_t param_num;
uint8_t param_chunk;
} _param_request;
// CRSF_FRAMETYPE_PARAMETER_WRITE
struct PACKED ParameterSettingsWriteFrame {
uint8_t destination;
uint8_t origin;
uint8_t param_num;
uint8_t payload[57]; // largest possible frame is 60
};
// Frame to hold passthrough telemetry
struct PACKED PassthroughSinglePacketFrame {
uint8_t sub_type;
uint16_t appid;
uint32_t data;
};
// Frame to hold passthrough telemetry
struct PACKED PassthroughMultiPacketFrame {
uint8_t sub_type;
uint8_t size;
struct PACKED PassthroughTelemetryPacket {
uint16_t appid;
uint32_t data;
} packets[PASSTHROUGH_MULTI_PACKET_FRAME_MAX_SIZE];
};
// Frame to hold status text message
struct PACKED StatusTextFrame {
uint8_t sub_type;
uint8_t severity;
char text[PASSTHROUGH_STATUS_TEXT_FRAME_MAX_SIZE]; // ( Null-terminated string )
};
// ardupilot frametype container
union PACKED APCustomTelemFrame {
PassthroughSinglePacketFrame single_packet_passthrough;
PassthroughMultiPacketFrame multi_packet_passthrough;
StatusTextFrame status_text;
};
union PACKED BroadcastFrame {
GPSFrame gps;
HeartbeatFrame heartbeat;
BatteryFrame battery;
VTXFrame vtx;
AttitudeFrame attitude;
FlightModeFrame flightmode;
APCustomTelemFrame custom_telem;
};
union PACKED ExtendedFrame {
CommandFrame command;
ParameterPingFrame ping;
ParameterDeviceInfoFrame info;
ParameterSettingsEntry param_entry;
ParameterSettingsReadFrame param_read;
ParameterSettingsWriteFrame param_write;
};
union PACKED TelemetryPayload {
BroadcastFrame bcast;
ExtendedFrame ext;
};
// get the protocol string
const char* get_protocol_string() const {
if (_crsf_version.is_elrs) {
return "ELRS";
} else {
const AP_RCProtocol_CRSF* crsf = AP::crsf();
if (crsf && crsf->is_crsf_v3_active()) {
return "CRSFv3";
}
return "CRSFv2";
}
};
// Process a frame from the CRSF protocol decoder
static bool process_frame(AP_RCProtocol_CRSF::FrameType frame_type, void* data);
// process any changed settings and schedule for transmission
void update();
// get next telemetry data for external consumers of SPort data
static bool get_telem_data(AP_RCProtocol_CRSF::Frame* frame);
private:
enum SensorType {
HEARTBEAT,
PARAMETERS,
ATTITUDE,
VTX_PARAMETERS,
BATTERY,
GPS,
FLIGHT_MODE,
PASSTHROUGH,
STATUS_TEXT,
GENERAL_COMMAND,
NUM_SENSORS
};
// passthrough WFQ scheduler
bool is_packet_ready(uint8_t idx, bool queue_empty) override;
void process_packet(uint8_t idx) override;
void adjust_packet_weight(bool queue_empty) override;
void setup_custom_telemetry();
void update_custom_telemetry_rates(const AP_RCProtocol_CRSF::RFMode rf_mode);
void calc_parameter_ping();
void calc_heartbeat();
void calc_battery();
void calc_gps();
void calc_attitude();
void calc_flight_mode();
void calc_device_info();
void calc_device_ping();
void calc_command_response();
void calc_parameter();
#if HAL_CRSF_TELEM_TEXT_SELECTION_ENABLED
void calc_text_selection( AP_OSD_ParamSetting* param, uint8_t chunk);
#endif
void update_params();
void update_vtx_params();
void get_single_packet_passthrough_telem_data();
void get_multi_packet_passthrough_telem_data(uint8_t size = PASSTHROUGH_MULTI_PACKET_FRAME_MAX_SIZE);
void calc_status_text();
void process_rf_mode_changes();
uint8_t get_custom_telem_frame_id() const;
AP_RCProtocol_CRSF::RFMode get_rf_mode() const;
uint16_t get_telemetry_rate() const;
bool is_high_speed_telemetry(const AP_RCProtocol_CRSF::RFMode rf_mode) const;
void process_vtx_frame(VTXFrame* vtx);
void process_vtx_telem_frame(VTXTelemetryFrame* vtx);
void process_ping_frame(ParameterPingFrame* ping);
void process_param_read_frame(ParameterSettingsReadFrame* read);
void process_param_write_frame(ParameterSettingsWriteFrame* write);
void process_device_info_frame(ParameterDeviceInfoFrame* info);
void process_command_frame(CommandFrame* command);
// setup ready for passthrough operation
void setup_wfq_scheduler(void) override;
// setup the scheduler for parameters download
void enter_scheduler_params_mode();
void exit_scheduler_params_mode();
// get next telemetry data for external consumers
bool _get_telem_data(AP_RCProtocol_CRSF::Frame* data);
bool _process_frame(AP_RCProtocol_CRSF::FrameType frame_type, void* data);
TelemetryPayload _telem;
uint8_t _telem_size;
uint8_t _telem_type;
AP_RCProtocol_CRSF::RFMode _telem_rf_mode;
// reporting telemetry rate
uint32_t _telem_last_report_ms;
uint16_t _telem_last_avg_rate;
bool _telem_is_high_speed;
bool _telem_pending;
bool _enable_telemetry;
struct {
uint8_t destination = AP_RCProtocol_CRSF::CRSF_ADDRESS_BROADCAST;
uint8_t frame_type;
} _pending_request;
struct {
uint8_t minor;
uint8_t major;
uint8_t retry_count;
bool use_rf_mode;
bool is_tracer;
bool pending = true;
bool is_elrs;
} _crsf_version;
struct {
bool init_done;
uint32_t params_mode_start_ms;
bool params_mode_active;
} _custom_telem;
struct {
bool pending;
bool valid;
uint8_t port_id;
} _baud_rate_request;
// vtx state
bool _vtx_freq_update; // update using the frequency method or not
bool _vtx_dbm_update; // update using the dbm method or not
bool _vtx_freq_change_pending; // a vtx command has been issued but not confirmed by a vtx broadcast frame
bool _vtx_power_change_pending;
bool _vtx_options_change_pending;
bool _noted_lq_as_rssi_active;
static AP_CRSF_Telem *singleton;
};
namespace AP {
AP_CRSF_Telem *crsf_telem();
};
#endif