You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
218 lines
7.9 KiB
218 lines
7.9 KiB
/* |
|
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/>. |
|
|
|
MSP telemetry library backend base class |
|
*/ |
|
#pragma once |
|
|
|
#include <AP_RCTelemetry/AP_RCTelemetry.h> |
|
#include <AP_OSD/AP_OSD.h> |
|
|
|
#include "msp.h" |
|
|
|
#include <time.h> |
|
|
|
#if HAL_MSP_ENABLED |
|
|
|
#define MSP_TIME_SLOT_MAX 12 |
|
#define CELLFULL 4.35 |
|
#define MSP_TXT_BUFFER_SIZE 15U // 11 + 3 utf8 chars + terminator |
|
#define MSP_TXT_VISIBLE_CHARS 11U |
|
|
|
class AP_MSP; |
|
|
|
class AP_MSP_Telem_Backend : AP_RCTelemetry |
|
{ |
|
friend AP_MSP; |
|
public: |
|
AP_MSP_Telem_Backend(AP_HAL::UARTDriver *uart); |
|
|
|
typedef struct battery_state_s { |
|
float batt_current_a; |
|
float batt_consumed_mah; |
|
float batt_voltage_v; |
|
int32_t batt_capacity_mah; |
|
uint8_t batt_cellcount; |
|
MSP::battery_state_e batt_state; |
|
} battery_state_t; |
|
|
|
typedef struct PACKED gps_state_s { |
|
uint8_t fix_type; |
|
uint8_t num_sats; |
|
int32_t lat; |
|
int32_t lon; |
|
uint16_t alt_m; |
|
uint16_t speed_cms; |
|
int16_t ground_course_cd; |
|
} gps_state_t; |
|
|
|
typedef struct airspeed_state_s { |
|
float airspeed_estimate_ms; |
|
bool airspeed_have_estimate; |
|
} airspeed_state_t; |
|
|
|
typedef struct home_state_s { |
|
bool home_is_set; |
|
float home_bearing_cd; |
|
uint32_t home_distance_m; |
|
int32_t rel_altitude_cm; |
|
} home_state_t; |
|
|
|
// init - perform required initialisation |
|
virtual bool init() override; |
|
virtual bool init_uart(); |
|
virtual void enable_warnings(); |
|
virtual void hide_osd_items(void); |
|
|
|
// MSP tx/rx processors |
|
void process_incoming_data(); // incoming data |
|
void process_outgoing_data(); // push outgoing data |
|
|
|
#if HAL_WITH_MSP_DISPLAYPORT |
|
// displayport commands |
|
// betaflight/src/main/io/displayport_msp.c |
|
virtual void msp_displayport_heartbeat(); |
|
virtual void msp_displayport_grab(); |
|
virtual void msp_displayport_release(); |
|
virtual void msp_displayport_clear_screen(); |
|
virtual void msp_displayport_draw_screen(); |
|
virtual void msp_displayport_write_string(uint8_t col, uint8_t row, bool blink, const char *string); |
|
#endif |
|
protected: |
|
enum msp_packet_type : uint8_t { |
|
EMPTY_SLOT = 0, |
|
NAME, |
|
STATUS, |
|
CONFIG, |
|
RAW_GPS, |
|
COMP_GPS, |
|
ATTITUDE, |
|
ALTITUDE, |
|
ANALOG, |
|
BATTERY_STATE, |
|
#if HAL_WITH_ESC_TELEM |
|
ESC_SENSOR_DATA, |
|
#endif |
|
RTC_DATETIME, |
|
}; |
|
|
|
const uint16_t msp_packet_type_map[MSP_TIME_SLOT_MAX] = { |
|
0, |
|
MSP_NAME, |
|
MSP_STATUS, |
|
MSP_OSD_CONFIG, |
|
MSP_RAW_GPS, |
|
MSP_COMP_GPS, |
|
MSP_ATTITUDE, |
|
MSP_ALTITUDE, |
|
MSP_ANALOG, |
|
MSP_BATTERY_STATE, |
|
#if HAL_WITH_ESC_TELEM |
|
MSP_ESC_SENSOR_DATA, |
|
#endif |
|
MSP_RTC |
|
}; |
|
|
|
/* UTF-8 encodings |
|
U+2191 ↑ e2 86 91 UPWARDS ARROW |
|
U+2197 ↗ e2 86 97 NORTH EAST ARROW |
|
U+2192 → e2 86 92 RIGHTWARDS ARROW |
|
U+2198 ↘ e2 86 98 SOUTH EAST ARROW |
|
U+2193 ↓ e2 86 93 DOWNWARDS ARROW |
|
U+2199 ↙ e2 86 99 SOUTH WEST ARROW |
|
U+2190 ← e2 86 90 LEFTWARDS ARROW |
|
U+2196 ↖ e2 86 96 NORTH WEST ARROW |
|
*/ |
|
static constexpr uint8_t arrows[8] = {0x91, 0x97, 0x92, 0x98, 0x93, 0x99, 0x90, 0x96}; |
|
|
|
static const uint8_t message_scroll_time_ms = 200; |
|
static const uint8_t message_scroll_delay = 5; |
|
|
|
// each backend can hide/unhide items dynamically |
|
uint64_t osd_hidden_items_bitmask; |
|
|
|
// MSP decoder status |
|
MSP::msp_port_t _msp_port; |
|
|
|
// 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_wfq_scheduler(void) override; |
|
bool get_next_msg_chunk(void) override |
|
{ |
|
return true; |
|
} |
|
|
|
// telemetry helpers |
|
uint8_t calc_cell_count(float battery_voltage); |
|
virtual float get_vspeed_ms(void) const; |
|
virtual bool get_rssi(float &rssi) const; |
|
virtual void update_home_pos(home_state_t &home_state); |
|
virtual void update_battery_state(battery_state_t &_battery_state); |
|
virtual void update_gps_state(gps_state_t &gps_state); |
|
virtual void update_airspeed(airspeed_state_t &airspeed_state); |
|
virtual void update_flight_mode_str(char *flight_mode_str, uint8_t size, bool wind_enabled); |
|
|
|
// MSP parsing |
|
void msp_process_received_command(); |
|
MSP::MSPCommandResult msp_process_command(MSP::msp_packet_t *cmd, MSP::msp_packet_t *reply); |
|
MSP::MSPCommandResult msp_process_sensor_command(uint16_t cmd_msp, MSP::sbuf_t *src); |
|
MSP::MSPCommandResult msp_process_out_command(uint16_t cmd_msp, MSP::sbuf_t *dst); |
|
|
|
// MSP send |
|
void msp_send_packet(uint16_t cmd, MSP::msp_version_e msp_version, const void *p, uint16_t size, bool is_request); |
|
|
|
// MSP sensor command processing |
|
void msp_handle_opflow(const MSP::msp_opflow_data_message_t &pkt); |
|
void msp_handle_rangefinder(const MSP::msp_rangefinder_data_message_t &pkt); |
|
void msp_handle_gps(const MSP::msp_gps_data_message_t &pkt); |
|
void msp_handle_compass(const MSP::msp_compass_data_message_t &pkt); |
|
void msp_handle_baro(const MSP::msp_baro_data_message_t &pkt); |
|
void msp_handle_airspeed(const MSP::msp_airspeed_data_message_t &pkt); |
|
|
|
// implementation specific helpers |
|
// custom masks are needed for vendor specific settings |
|
virtual uint32_t get_osd_flight_mode_bitmask(void) |
|
{ |
|
return 0; |
|
} |
|
|
|
virtual bool is_scheduler_enabled() const = 0; // only osd backends should allow a push type telemetry |
|
virtual bool use_msp_thread() const {return true;}; // is this backend hanlded by the MSP thread? |
|
virtual AP_SerialManager::SerialProtocol get_serial_protocol() const = 0; |
|
virtual bool displaying_stats_screen() const; |
|
|
|
// implementation specific MSP out command processing |
|
virtual MSP::MSPCommandResult msp_process_out_api_version(MSP::sbuf_t *dst); |
|
virtual MSP::MSPCommandResult msp_process_out_fc_version(MSP::sbuf_t *dst); |
|
virtual MSP::MSPCommandResult msp_process_out_fc_variant(MSP::sbuf_t *dst); |
|
virtual MSP::MSPCommandResult msp_process_out_uid(MSP::sbuf_t *dst); |
|
virtual MSP::MSPCommandResult msp_process_out_board_info(MSP::sbuf_t *dst); |
|
virtual MSP::MSPCommandResult msp_process_out_build_info(MSP::sbuf_t *dst); |
|
virtual MSP::MSPCommandResult msp_process_out_name(MSP::sbuf_t *dst); |
|
virtual MSP::MSPCommandResult msp_process_out_status(MSP::sbuf_t *dst); |
|
virtual MSP::MSPCommandResult msp_process_out_osd_config(MSP::sbuf_t *dst); |
|
virtual MSP::MSPCommandResult msp_process_out_raw_gps(MSP::sbuf_t *dst); |
|
virtual MSP::MSPCommandResult msp_process_out_comp_gps(MSP::sbuf_t *dst); |
|
virtual MSP::MSPCommandResult msp_process_out_attitude(MSP::sbuf_t *dst); |
|
virtual MSP::MSPCommandResult msp_process_out_altitude(MSP::sbuf_t *dst); |
|
virtual MSP::MSPCommandResult msp_process_out_analog(MSP::sbuf_t *dst); |
|
virtual MSP::MSPCommandResult msp_process_out_battery_state(MSP::sbuf_t *dst); |
|
virtual MSP::MSPCommandResult msp_process_out_esc_sensor_data(MSP::sbuf_t *dst); |
|
virtual MSP::MSPCommandResult msp_process_out_rtc(MSP::sbuf_t *dst); |
|
virtual MSP::MSPCommandResult msp_process_out_rc(MSP::sbuf_t *dst); |
|
}; |
|
|
|
#endif //HAL_MSP_ENABLED
|
|
|