mirror of https://github.com/ArduPilot/ardupilot
AP_MSP: added Multiwii Serial protocol (MSP) v1 and v2 support
This commit is contained in:
yaapu
Andrew Tridgell
parent
522c3e6281
commit
80eca32604
|
|
@ -0,0 +1,215 @@
|
|||
/*
|
||||
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/>.
|
||||
*/
|
||||
|
||||
#include <AP_AHRS/AP_AHRS.h>
|
||||
#include <AP_RSSI/AP_RSSI.h>
|
||||
|
||||
#include "AP_MSP.h"
|
||||
#include "AP_MSP_Telem_Generic.h"
|
||||
#include "AP_MSP_Telem_DJI.h"
|
||||
|
||||
#include <ctype.h>
|
||||
#include <stdio.h>
|
||||
|
||||
#if HAL_MSP_ENABLED
|
||||
|
||||
const uint16_t OSD_FLIGHT_MODE_FOCUS_TIME = 2000;
|
||||
extern const AP_HAL::HAL& hal;
|
||||
|
||||
const AP_Param::GroupInfo AP_MSP::var_info[] = {
|
||||
|
||||
// @Param: _OSD_NCELLS
|
||||
// @DisplayName: Cell count override
|
||||
// @Description: Used for average cell voltage calculation
|
||||
// @Values: 0:Auto, 1-12
|
||||
// @User: Standard
|
||||
AP_GROUPINFO("_OSD_NCELLS", 1, AP_MSP, _cellcount, 0),
|
||||
|
||||
// @Param: _OPTIONS
|
||||
// @DisplayName: MSP OSD Options
|
||||
// @Description: A bitmask to set some MSP specific options
|
||||
// @Bitmask: 0:EnableTelemetryMode
|
||||
// @User: Standard
|
||||
AP_GROUPINFO("_OPTIONS", 2, AP_MSP, _options, 0),
|
||||
|
||||
AP_GROUPEND
|
||||
};
|
||||
|
||||
AP_MSP *AP_MSP::_singleton;
|
||||
|
||||
AP_MSP::AP_MSP()
|
||||
{
|
||||
_singleton = this;
|
||||
AP_Param::setup_object_defaults(this, var_info);
|
||||
}
|
||||
|
||||
bool AP_MSP::init_backend(uint8_t backend_idx, AP_HAL::UARTDriver *uart, AP_SerialManager::SerialProtocol protocol)
|
||||
{
|
||||
if (protocol == AP_SerialManager::SerialProtocol_MSP) {
|
||||
_backends[backend_idx] = new AP_MSP_Telem_Generic(uart);
|
||||
} else if (protocol == AP_SerialManager::SerialProtocol_DJI_FPV) {
|
||||
_backends[backend_idx] = new AP_MSP_Telem_DJI(uart);
|
||||
} else {
|
||||
return false;
|
||||
}
|
||||
if (_backends[backend_idx] != nullptr) {
|
||||
_backends[backend_idx]->init();
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
/*
|
||||
* init - perform required initialisation
|
||||
*/
|
||||
void AP_MSP::init()
|
||||
{
|
||||
const AP_SerialManager &serial_manager = AP::serialmanager();
|
||||
uint8_t backend_instance = 0;
|
||||
AP_HAL::UARTDriver *uart = nullptr;
|
||||
|
||||
// DJI FPV backends
|
||||
for (uint8_t protocol_instance=0; protocol_instance<MSP_MAX_INSTANCES; protocol_instance++) {
|
||||
uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_DJI_FPV, backend_instance);
|
||||
if (uart != nullptr) {
|
||||
if (!init_backend(protocol_instance, uart, AP_SerialManager::SerialProtocol_DJI_FPV)) {
|
||||
break;
|
||||
}
|
||||
// initialize osd settings from OSD backend
|
||||
if (!_msp_status.osd_initialized) {
|
||||
init_osd();
|
||||
}
|
||||
backend_instance++;
|
||||
_msp_status.backend_count++;
|
||||
}
|
||||
}
|
||||
backend_instance = 0;
|
||||
// generic MSP backends
|
||||
for (uint8_t protocol_instance=0; protocol_instance<MSP_MAX_INSTANCES-_msp_status.backend_count; protocol_instance++) {
|
||||
uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_MSP, backend_instance);
|
||||
if (uart != nullptr) {
|
||||
if (!init_backend(protocol_instance, uart, AP_SerialManager::SerialProtocol_MSP)) {
|
||||
break;
|
||||
}
|
||||
backend_instance++;
|
||||
_msp_status.backend_count++;
|
||||
}
|
||||
}
|
||||
|
||||
if (_msp_status.backend_count > 0) {
|
||||
// we've found at least 1 msp backend, start protocol handler
|
||||
if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_MSP::loop, void),
|
||||
"MSP",
|
||||
1024, AP_HAL::Scheduler::PRIORITY_IO, 1)) {
|
||||
return;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void AP_MSP::init_osd()
|
||||
{
|
||||
#if OSD_ENABLED
|
||||
AP_OSD* osd = AP::osd();
|
||||
|
||||
if (osd == nullptr) {
|
||||
return;
|
||||
}
|
||||
|
||||
_osd_item_settings[OSD_RSSI_VALUE] = &osd->screen[0].rssi;
|
||||
_osd_item_settings[OSD_MAIN_BATT_VOLTAGE] = &osd->screen[0].bat_volt;
|
||||
_osd_item_settings[OSD_CROSSHAIRS] = &osd->screen[0].crosshair;
|
||||
_osd_item_settings[OSD_ARTIFICIAL_HORIZON] = &osd->screen[0].horizon;
|
||||
_osd_item_settings[OSD_HORIZON_SIDEBARS] = &osd->screen[0].sidebars;
|
||||
_osd_item_settings[OSD_CRAFT_NAME] = &osd->screen[0].message;
|
||||
_osd_item_settings[OSD_FLYMODE] = &osd->screen[0].fltmode;
|
||||
_osd_item_settings[OSD_CURRENT_DRAW] = &osd->screen[0].current;
|
||||
_osd_item_settings[OSD_MAH_DRAWN] = &osd->screen[0].batused;
|
||||
_osd_item_settings[OSD_GPS_SPEED] = &osd->screen[0].gspeed;
|
||||
_osd_item_settings[OSD_GPS_SATS] = &osd->screen[0].sats;
|
||||
_osd_item_settings[OSD_ALTITUDE] = &osd->screen[0].altitude;
|
||||
_osd_item_settings[OSD_POWER] = &osd->screen[0].power;
|
||||
_osd_item_settings[OSD_AVG_CELL_VOLTAGE] = &osd->screen[0].cell_volt;
|
||||
_osd_item_settings[OSD_GPS_LON] = &osd->screen[0].gps_longitude;
|
||||
_osd_item_settings[OSD_GPS_LAT] = &osd->screen[0].gps_latitude;
|
||||
_osd_item_settings[OSD_PITCH_ANGLE] = &osd->screen[0].pitch_angle;
|
||||
_osd_item_settings[OSD_ROLL_ANGLE] = &osd->screen[0].roll_angle;
|
||||
_osd_item_settings[OSD_MAIN_BATT_USAGE] = &osd->screen[0].batt_bar;
|
||||
_osd_item_settings[OSD_DISARMED] = &osd->screen[0].arming;
|
||||
_osd_item_settings[OSD_HOME_DIR] = &osd->screen[0].home_dir;
|
||||
_osd_item_settings[OSD_HOME_DIST] = &osd->screen[0].home_dist;
|
||||
_osd_item_settings[OSD_NUMERICAL_HEADING] = &osd->screen[0].heading;
|
||||
_osd_item_settings[OSD_NUMERICAL_VARIO] = &osd->screen[0].vspeed;
|
||||
#ifdef HAVE_AP_BLHELI_SUPPORT
|
||||
_osd_item_settings[OSD_ESC_TMP] = &osd->screen[0].blh_temp;
|
||||
#endif
|
||||
_osd_item_settings[OSD_RTC_DATETIME] = &osd->screen[0].clk;
|
||||
#endif // OSD_ENABLED
|
||||
_msp_status.osd_initialized = true;
|
||||
}
|
||||
|
||||
void AP_MSP::loop(void)
|
||||
{
|
||||
for (uint8_t i=0; i<_msp_status.backend_count; i++) {
|
||||
// one time uart init
|
||||
if (_backends[i] != nullptr) {
|
||||
_backends[i]->init_uart();
|
||||
}
|
||||
}
|
||||
|
||||
while (true) {
|
||||
hal.scheduler->delay(10); // 115200 baud, 18 MSP packets @4Hz, 100Hz should be OK
|
||||
|
||||
const uint32_t now = AP_HAL::millis();
|
||||
// toggle flashing every 0.7 seconds
|
||||
if (((now / 700) & 0x01) != _msp_status.flashing_on) {
|
||||
_msp_status.flashing_on = !_msp_status.flashing_on;
|
||||
}
|
||||
|
||||
// detect flight mode changes and steal focus from text messages
|
||||
if (AP::notify().flags.flight_mode != _msp_status.last_flight_mode) {
|
||||
_msp_status.flight_mode_focus = true;
|
||||
_msp_status.last_flight_mode = AP::notify().flags.flight_mode;
|
||||
_msp_status.last_flight_mode_change_ms = AP_HAL::millis();
|
||||
} else if (now - _msp_status.last_flight_mode_change_ms > OSD_FLIGHT_MODE_FOCUS_TIME) {
|
||||
_msp_status.flight_mode_focus = false;
|
||||
}
|
||||
|
||||
for (uint8_t i=0; i< _msp_status.backend_count; i++) {
|
||||
if (_backends[i] != nullptr) {
|
||||
// dynamically hide/unhide
|
||||
_backends[i]->hide_osd_items();
|
||||
// process incoming MSP frames (and reply if needed)
|
||||
_backends[i]->process_incoming_data();
|
||||
// push outgoing telemetry frames
|
||||
_backends[i]->process_outgoing_data();
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool AP_MSP::check_option(msp_option_e option)
|
||||
{
|
||||
return (_options & option) != 0;
|
||||
}
|
||||
|
||||
namespace AP
|
||||
{
|
||||
AP_MSP *msp()
|
||||
{
|
||||
return AP_MSP::get_singleton();
|
||||
}
|
||||
};
|
||||
|
||||
#endif //HAL_MSP_ENABLED
|
||||
|
|
@ -0,0 +1,99 @@
|
|||
/*
|
||||
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 version 1 and 2 protocol library, based on betaflight/iNav implementations
|
||||
code by Alex Apostoli
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <AP_OSD/AP_OSD.h>
|
||||
|
||||
#include "AP_MSP_Telem_Backend.h"
|
||||
|
||||
#if HAL_MSP_ENABLED
|
||||
|
||||
#define MSP_MAX_INSTANCES 3
|
||||
#define MSP_OSD_START 2048
|
||||
#define MSP_OSD_STEP_X 1
|
||||
#define MSP_OSD_STEP_Y 32
|
||||
#define MSP_OSD_POS(osd_setting) (MSP_OSD_START + osd_setting->xpos*MSP_OSD_STEP_X + osd_setting->ypos*MSP_OSD_STEP_Y)
|
||||
|
||||
using namespace MSP;
|
||||
|
||||
class AP_MSP
|
||||
{
|
||||
friend class AP_MSP_Telem_Generic;
|
||||
friend class AP_MSP_Telem_DJI;
|
||||
friend class AP_MSP_Telem_Backend;
|
||||
|
||||
public:
|
||||
AP_MSP();
|
||||
|
||||
/* Do not allow copies */
|
||||
AP_MSP(const AP_MSP &other) = delete;
|
||||
AP_MSP &operator=(const AP_MSP&) = delete;
|
||||
|
||||
// User settable parameters
|
||||
static const struct AP_Param::GroupInfo var_info[];
|
||||
|
||||
// init - perform required initialisation
|
||||
void init();
|
||||
|
||||
static AP_MSP *get_singleton(void)
|
||||
{
|
||||
return _singleton;
|
||||
}
|
||||
|
||||
private:
|
||||
|
||||
enum msp_option_e : uint8_t{
|
||||
OPTION_TELEMETRY_MODE = 1U<<0,
|
||||
};
|
||||
|
||||
AP_MSP_Telem_Backend *_backends[MSP_MAX_INSTANCES];
|
||||
|
||||
AP_Int8 _options;
|
||||
AP_Int8 _units;
|
||||
AP_Int8 _msgtime_s;
|
||||
AP_Int8 _cellcount;
|
||||
|
||||
// these are the osd items we support for MSP OSD
|
||||
AP_OSD_Setting* _osd_item_settings[OSD_ITEM_COUNT];
|
||||
osd_config_t _osd_config;
|
||||
|
||||
struct {
|
||||
bool flashing_on; // OSD item flashing support
|
||||
uint8_t last_flight_mode = 255;
|
||||
uint32_t last_flight_mode_change_ms;
|
||||
bool flight_mode_focus; // do we need to steal focus from text messages
|
||||
bool osd_initialized; // for one time osd initialization
|
||||
uint8_t backend_count; // actual count of active bacends
|
||||
} _msp_status;
|
||||
|
||||
bool init_backend(uint8_t backend_idx, AP_HAL::UARTDriver *uart, AP_SerialManager::SerialProtocol protocol);
|
||||
void init_osd();
|
||||
void loop(void);
|
||||
bool check_option(const msp_option_e option);
|
||||
|
||||
static AP_MSP *_singleton;
|
||||
};
|
||||
|
||||
namespace AP
|
||||
{
|
||||
AP_MSP *msp();
|
||||
};
|
||||
|
||||
#endif //HAL_MSP_ENABLED
|
||||
|
|
@ -0,0 +1,945 @@
|
|||
/*
|
||||
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/>.
|
||||
*/
|
||||
|
||||
#include <AP_AHRS/AP_AHRS.h>
|
||||
#include <AP_Baro/AP_Baro.h>
|
||||
#include <AP_BattMonitor/AP_BattMonitor.h>
|
||||
#include <AP_BLHeli/AP_BLHeli.h>
|
||||
#include <RC_Channel/RC_Channel.h>
|
||||
#include <AP_Common/AP_FWVersion.h>
|
||||
#include <AP_GPS/AP_GPS.h>
|
||||
#include <AP_OpticalFlow/AP_OpticalFlow.h>
|
||||
#include <AP_RangeFinder/AP_RangeFinder.h>
|
||||
#include <AP_RCMapper/AP_RCMapper.h>
|
||||
#include <AP_RSSI/AP_RSSI.h>
|
||||
#include <AP_RTC/AP_RTC.h>
|
||||
#include <GCS_MAVLink/GCS.h>
|
||||
|
||||
#include "AP_MSP.h"
|
||||
#include "AP_MSP_Telem_Backend.h"
|
||||
|
||||
#include <ctype.h>
|
||||
#include <stdio.h>
|
||||
|
||||
#if HAL_MSP_ENABLED
|
||||
|
||||
extern const AP_HAL::HAL& hal;
|
||||
constexpr uint8_t AP_MSP_Telem_Backend::arrows[8];
|
||||
|
||||
AP_MSP_Telem_Backend::AP_MSP_Telem_Backend(AP_HAL::UARTDriver *uart) : AP_RCTelemetry(MSP_TIME_SLOT_MAX)
|
||||
{
|
||||
_msp_port.uart = uart;
|
||||
}
|
||||
|
||||
/*
|
||||
Scheduler helper
|
||||
*/
|
||||
void AP_MSP_Telem_Backend::setup_wfq_scheduler(void)
|
||||
{
|
||||
// initialize packet weights for the WFQ scheduler
|
||||
// priority[i] = 1/_scheduler.packet_weight[i]
|
||||
// rate[i] = LinkRate * ( priority[i] / (sum(priority[1-n])) )
|
||||
|
||||
set_scheduler_entry(EMPTY_SLOT, 50, 50); // nothing to send
|
||||
set_scheduler_entry(NAME, 200, 200); // 5Hz 12 chars string used for general purpose text messages
|
||||
set_scheduler_entry(STATUS, 500, 500); // 2Hz flightmode
|
||||
set_scheduler_entry(CONFIG, 200, 200); // 5Hz OSD item positions
|
||||
set_scheduler_entry(RAW_GPS, 250, 250); // 4Hz GPS lat/lon
|
||||
set_scheduler_entry(COMP_GPS, 250, 250); // 4Hz home direction and distance
|
||||
set_scheduler_entry(ATTITUDE, 200, 200); // 5Hz attitude
|
||||
set_scheduler_entry(ALTITUDE, 250, 250); // 4Hz altitude(cm) and velocity(cm/s)
|
||||
set_scheduler_entry(ANALOG, 250, 250); // 4Hz rssi + batt
|
||||
set_scheduler_entry(BATTERY_STATE, 500, 500); // 2Hz battery
|
||||
set_scheduler_entry(ESC_SENSOR_DATA, 500, 500); // 2Hz ESC telemetry
|
||||
set_scheduler_entry(RTC_DATETIME, 1000, 1000); // 1Hz RTC
|
||||
}
|
||||
|
||||
/*
|
||||
* init - perform required initialisation
|
||||
*/
|
||||
bool AP_MSP_Telem_Backend::init()
|
||||
{
|
||||
enable_warnings();
|
||||
return AP_RCTelemetry::init();
|
||||
}
|
||||
|
||||
bool AP_MSP_Telem_Backend::init_uart()
|
||||
{
|
||||
if (_msp_port.uart != nullptr) {
|
||||
_msp_port.uart->set_flow_control(AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE);
|
||||
_msp_port.uart->begin(AP_SERIALMANAGER_MSP_BAUD, AP_SERIALMANAGER_MSP_BUFSIZE_RX, AP_SERIALMANAGER_MSP_BUFSIZE_TX);
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
void AP_MSP_Telem_Backend::process_outgoing_data()
|
||||
{
|
||||
if (is_scheduler_enabled()) {
|
||||
AP_RCTelemetry::run_wfq_scheduler();
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
Scheduler helper
|
||||
*/
|
||||
bool AP_MSP_Telem_Backend::is_packet_ready(uint8_t idx, bool queue_empty)
|
||||
{
|
||||
switch (idx) {
|
||||
case EMPTY_SLOT: // empty slot
|
||||
case NAME: // used for status_text messages
|
||||
case STATUS: // flightmode
|
||||
case CONFIG: // OSD config
|
||||
case RAW_GPS: // lat,lon, speed
|
||||
case COMP_GPS: // home dir,dist
|
||||
case ATTITUDE: // Attitude
|
||||
case ALTITUDE: // Altitude and Vario
|
||||
case ANALOG: // Rssi, Battery, mAh, Current
|
||||
case BATTERY_STATE: // voltage, capacity, current, mAh
|
||||
case ESC_SENSOR_DATA: // esc temp + rpm
|
||||
case RTC_DATETIME: // RTC
|
||||
return true;
|
||||
default:
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
Invoked at each scheduler step
|
||||
*/
|
||||
void AP_MSP_Telem_Backend::process_packet(uint8_t idx)
|
||||
{
|
||||
if (idx == EMPTY_SLOT) {
|
||||
return;
|
||||
}
|
||||
|
||||
uint8_t out_buf[MSP_PORT_OUTBUF_SIZE] {};
|
||||
|
||||
msp_packet_t reply = {
|
||||
.buf = { .ptr = out_buf, .end = MSP_ARRAYEND(out_buf), },
|
||||
.cmd = (int16_t)msp_packet_type_map[idx],
|
||||
.flags = 0,
|
||||
.result = 0,
|
||||
};
|
||||
uint8_t *out_buf_head = reply.buf.ptr;
|
||||
|
||||
msp_process_out_command(msp_packet_type_map[idx], &reply.buf);
|
||||
sbuf_switch_to_reader(&reply.buf, out_buf_head); // change streambuf direction
|
||||
msp_serial_encode(&_msp_port, &reply, _msp_port.msp_version);
|
||||
|
||||
_msp_port.c_state = MSP_IDLE;
|
||||
}
|
||||
|
||||
uint8_t AP_MSP_Telem_Backend::calc_cell_count(const float battery_voltage)
|
||||
{
|
||||
return floorf((battery_voltage / CELLFULL) + 1);
|
||||
}
|
||||
|
||||
float AP_MSP_Telem_Backend::get_vspeed_ms(void)
|
||||
{
|
||||
{
|
||||
// release semaphore as soon as possible
|
||||
AP_AHRS &_ahrs = AP::ahrs();
|
||||
Vector3f v {};
|
||||
WITH_SEMAPHORE(_ahrs.get_semaphore());
|
||||
if (_ahrs.get_velocity_NED(v)) {
|
||||
return -v.z;
|
||||
}
|
||||
}
|
||||
AP_Baro &_baro = AP::baro();
|
||||
WITH_SEMAPHORE(_baro.get_semaphore());
|
||||
return _baro.get_climb_rate();
|
||||
}
|
||||
|
||||
void AP_MSP_Telem_Backend::update_home_pos(home_state_t &home_state)
|
||||
{
|
||||
AP_AHRS &_ahrs = AP::ahrs();
|
||||
WITH_SEMAPHORE(_ahrs.get_semaphore());
|
||||
Location loc;
|
||||
float alt;
|
||||
if (_ahrs.get_position(loc) && _ahrs.home_is_set()) {
|
||||
const Location &home_loc = _ahrs.get_home();
|
||||
home_state.home_distance_m = home_loc.get_distance(loc);
|
||||
home_state.home_bearing_cd = loc.get_bearing_to(home_loc);
|
||||
} else {
|
||||
home_state.home_distance_m = 0;
|
||||
home_state.home_bearing_cd = 0;
|
||||
}
|
||||
_ahrs.get_relative_position_D_home(alt);
|
||||
home_state.rel_altitude_cm = -alt * 100;
|
||||
home_state.home_is_set = _ahrs.home_is_set();
|
||||
}
|
||||
|
||||
void AP_MSP_Telem_Backend::update_gps_state(gps_state_t &gps_state)
|
||||
{
|
||||
AP_GPS& gps = AP::gps();
|
||||
WITH_SEMAPHORE(gps.get_semaphore());
|
||||
gps_state.gps_fix_type = gps.status();
|
||||
|
||||
if (gps_state.gps_fix_type >= AP_GPS::GPS_Status::GPS_OK_FIX_2D) {
|
||||
gps_state.gps_num_sats = gps.num_sats();
|
||||
} else {
|
||||
gps_state.gps_num_sats = 0;
|
||||
}
|
||||
|
||||
if (gps_state.gps_fix_type >= AP_GPS::GPS_Status::GPS_OK_FIX_3D) {
|
||||
const Location &loc = AP::gps().location(); //get gps instance 0
|
||||
gps_state.gps_latitude = loc.lat;
|
||||
gps_state.gps_longitude = loc.lng;
|
||||
gps_state.gps_altitude_cm = loc.alt;
|
||||
gps_state.gps_speed_ms = gps.ground_speed();
|
||||
gps_state.gps_ground_course_cd = gps.ground_course_cd();
|
||||
} else {
|
||||
gps_state.gps_latitude = 0;
|
||||
gps_state.gps_longitude = 0;
|
||||
gps_state.gps_altitude_cm = 0;
|
||||
gps_state.gps_speed_ms = 0;
|
||||
gps_state.gps_ground_course_cd = 0;
|
||||
}
|
||||
}
|
||||
|
||||
void AP_MSP_Telem_Backend::update_battery_state(battery_state_t &battery_state)
|
||||
{
|
||||
const AP_BattMonitor &_battery = AP::battery();
|
||||
if (!_battery.current_amps(battery_state.batt_current_a)) {
|
||||
battery_state.batt_current_a = 0;
|
||||
}
|
||||
if (!_battery.consumed_mah(battery_state.batt_consumed_mah)) {
|
||||
battery_state.batt_consumed_mah = 0;
|
||||
}
|
||||
battery_state.batt_voltage_v =_battery.voltage();
|
||||
battery_state.batt_capacity_mah = _battery.pack_capacity_mah();
|
||||
|
||||
const AP_Notify& notify = AP::notify();
|
||||
if (notify.flags.failsafe_battery) {
|
||||
battery_state.batt_state = MSP_BATTERY_CRITICAL;
|
||||
} else {
|
||||
battery_state.batt_state = MSP_BATTERY_OK;
|
||||
}
|
||||
// detect cellcount and update only if we get a higher values, we do not want to update it while discharging
|
||||
uint8_t cc = calc_cell_count(battery_state.batt_voltage_v);
|
||||
if (cc > battery_state.batt_cellcount) {
|
||||
battery_state.batt_cellcount = cc;
|
||||
}
|
||||
}
|
||||
|
||||
void AP_MSP_Telem_Backend::update_airspeed(airspeed_state_t &airspeed_state)
|
||||
{
|
||||
AP_AHRS &ahrs = AP::ahrs();
|
||||
WITH_SEMAPHORE(ahrs.get_semaphore());
|
||||
airspeed_state.airspeed_have_estimate = ahrs.airspeed_estimate(airspeed_state.airspeed_estimate_ms);
|
||||
if (!airspeed_state.airspeed_have_estimate) {
|
||||
airspeed_state.airspeed_estimate_ms = 0.0;
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
MSP OSDs can display up to MSP_TXT_VISIBLE_CHARS chars (UTF8 characters are supported)
|
||||
We display the flight mode string either with or without wind state
|
||||
*/
|
||||
void AP_MSP_Telem_Backend::update_flight_mode_str(char *flight_mode_str, bool wind_enabled)
|
||||
{
|
||||
#if OSD_ENABLED
|
||||
AP_OSD *osd = AP::osd();
|
||||
if (osd == nullptr) {
|
||||
return;
|
||||
}
|
||||
#endif
|
||||
AP_Notify *notify = AP_Notify::get_singleton();
|
||||
if (notify == nullptr) {
|
||||
return;
|
||||
}
|
||||
// clear
|
||||
memset(flight_mode_str, 0, MSP_TXT_BUFFER_SIZE);
|
||||
|
||||
if (wind_enabled) {
|
||||
/*
|
||||
Wind is rendered next to the current flight mode, for the direction we use an UTF8 arrow (bytes 0xE286[nn])
|
||||
example: MANU 4m/s ↗
|
||||
*/
|
||||
AP_AHRS &ahrs = AP::ahrs();
|
||||
Vector3f v;
|
||||
{
|
||||
WITH_SEMAPHORE(ahrs.get_semaphore());
|
||||
v = ahrs.wind_estimate();
|
||||
}
|
||||
bool invert_wind = false;
|
||||
#if OSD_ENABLED
|
||||
invert_wind = osd->screen[0].check_option(AP_OSD::OPTION_INVERTED_WIND);
|
||||
#endif
|
||||
if (invert_wind) {
|
||||
v = -v;
|
||||
}
|
||||
uint8_t units = OSD_UNIT_METRIC;
|
||||
#if OSD_ENABLED
|
||||
units = osd->units == AP_OSD::UNITS_IMPERIAL ? OSD_UNIT_IMPERIAL : OSD_UNIT_METRIC;
|
||||
#endif
|
||||
// if needed convert m/s to ft/s
|
||||
const float v_length = (units == OSD_UNIT_METRIC) ? v.length() : v.length() * 3.28084;
|
||||
const char* unit = (units == OSD_UNIT_METRIC) ? "m/s" : "f/s";
|
||||
|
||||
if (v_length > 1.0f) {
|
||||
const int32_t angle = wrap_360_cd(DEGX100 * atan2f(v.y, v.x) - ahrs.yaw_sensor);
|
||||
const int32_t interval = 36000 / ARRAY_SIZE(arrows);
|
||||
uint8_t arrow = arrows[((angle + interval / 2) / interval) % ARRAY_SIZE(arrows)];
|
||||
snprintf(flight_mode_str, MSP_TXT_BUFFER_SIZE, "%s %d%s%c%c%c", notify->get_flight_mode_str(), (uint8_t)roundf(v_length), unit, 0xE2, 0x86, arrow);
|
||||
} else {
|
||||
snprintf(flight_mode_str, MSP_TXT_BUFFER_SIZE, "%s ---%s", notify->get_flight_mode_str(), unit);
|
||||
}
|
||||
} else {
|
||||
/*
|
||||
Flight mode is rendered with simple mode flags
|
||||
examples:
|
||||
MANU
|
||||
MANU [S]
|
||||
MANU [SS]
|
||||
*/
|
||||
const bool simple_mode = gcs().simple_input_active();
|
||||
const bool supersimple_mode = gcs().supersimple_input_active();
|
||||
const char* simple_mode_str = simple_mode ? " [S]" : (supersimple_mode ? " [SS]" : "");
|
||||
|
||||
char buffer[MSP_TXT_BUFFER_SIZE] {};
|
||||
// flightmode
|
||||
const uint8_t used = snprintf(buffer, ARRAY_SIZE(buffer), "%s%s", notify->get_flight_mode_str(), simple_mode_str);
|
||||
// left pad
|
||||
uint8_t left_padded_len = MSP_TXT_VISIBLE_CHARS - (MSP_TXT_VISIBLE_CHARS - used)/2;
|
||||
snprintf(flight_mode_str, MSP_TXT_BUFFER_SIZE, "%*s", left_padded_len, buffer);
|
||||
}
|
||||
}
|
||||
|
||||
void AP_MSP_Telem_Backend::enable_warnings()
|
||||
{
|
||||
AP_MSP *msp = AP::msp();
|
||||
if (msp == nullptr) {
|
||||
return;
|
||||
}
|
||||
BIT_SET(msp->_osd_config.enabled_warnings, OSD_WARNING_FAIL_SAFE);
|
||||
BIT_SET(msp->_osd_config.enabled_warnings, OSD_WARNING_BATTERY_CRITICAL);
|
||||
}
|
||||
|
||||
void AP_MSP_Telem_Backend::process_incoming_data()
|
||||
{
|
||||
if (_msp_port.uart == nullptr) {
|
||||
return;
|
||||
}
|
||||
|
||||
uint32_t numc = MIN(_msp_port.uart->available(), 1024U);
|
||||
|
||||
if (numc > 0) {
|
||||
// Process incoming bytes
|
||||
while (numc-- > 0) {
|
||||
const uint8_t c = _msp_port.uart->read();
|
||||
msp_parse_received_data(&_msp_port, c);
|
||||
|
||||
if (_msp_port.c_state == MSP_COMMAND_RECEIVED) {
|
||||
msp_process_received_command();
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
ported from betaflight/src/main/msp/msp_serial.c
|
||||
*/
|
||||
void AP_MSP_Telem_Backend::msp_process_received_command()
|
||||
{
|
||||
uint8_t out_buf[MSP_PORT_OUTBUF_SIZE];
|
||||
|
||||
msp_packet_t reply = {
|
||||
.buf = { .ptr = out_buf, .end = MSP_ARRAYEND(out_buf), },
|
||||
.cmd = -1,
|
||||
.flags = 0,
|
||||
.result = 0,
|
||||
};
|
||||
uint8_t *out_buf_head = reply.buf.ptr;
|
||||
|
||||
msp_packet_t command = {
|
||||
.buf = { .ptr = _msp_port.in_buf, .end = _msp_port.in_buf + _msp_port.data_size, },
|
||||
.cmd = (int16_t)_msp_port.cmd_msp,
|
||||
.flags = _msp_port.cmd_flags,
|
||||
.result = 0,
|
||||
};
|
||||
|
||||
const MSPCommandResult status = msp_process_command(&command, &reply);
|
||||
|
||||
if (status != MSP_RESULT_NO_REPLY) {
|
||||
sbuf_switch_to_reader(&reply.buf, out_buf_head); // change streambuf direction
|
||||
msp_serial_encode(&_msp_port, &reply, _msp_port.msp_version);
|
||||
}
|
||||
|
||||
_msp_port.c_state = MSP_IDLE;
|
||||
}
|
||||
|
||||
/*
|
||||
ported from inav/src/main/fc/fc_msp.c
|
||||
*/
|
||||
MSPCommandResult AP_MSP_Telem_Backend::msp_process_command(msp_packet_t *cmd, msp_packet_t *reply)
|
||||
{
|
||||
MSPCommandResult ret = MSP_RESULT_ACK;
|
||||
sbuf_t *dst = &reply->buf;
|
||||
sbuf_t *src = &cmd->buf;
|
||||
const uint16_t cmd_msp = cmd->cmd;
|
||||
// initialize reply by default
|
||||
reply->cmd = cmd->cmd;
|
||||
|
||||
if (MSP2_IS_SENSOR_MESSAGE(cmd_msp)) {
|
||||
ret = msp_process_sensor_command(cmd_msp, src);
|
||||
} else {
|
||||
ret = msp_process_out_command(cmd_msp, dst);
|
||||
}
|
||||
|
||||
// Process DONT_REPLY flag
|
||||
if (cmd->flags & MSP_FLAG_DONT_REPLY) {
|
||||
ret = MSP_RESULT_NO_REPLY;
|
||||
}
|
||||
|
||||
reply->result = ret;
|
||||
return ret;
|
||||
}
|
||||
|
||||
MSPCommandResult AP_MSP_Telem_Backend::msp_process_out_command(uint16_t cmd_msp, sbuf_t *dst)
|
||||
{
|
||||
switch (cmd_msp) {
|
||||
case MSP_API_VERSION:
|
||||
return msp_process_out_api_version(dst);
|
||||
case MSP_FC_VARIANT:
|
||||
return msp_process_out_fc_variant(dst);
|
||||
case MSP_FC_VERSION:
|
||||
return msp_process_out_fc_version(dst);
|
||||
case MSP_BOARD_INFO:
|
||||
return msp_process_out_board_info(dst);
|
||||
case MSP_BUILD_INFO:
|
||||
return msp_process_out_build_info(dst);
|
||||
case MSP_NAME:
|
||||
return msp_process_out_name(dst);
|
||||
case MSP_OSD_CONFIG:
|
||||
return msp_process_out_osd_config(dst);
|
||||
case MSP_STATUS:
|
||||
case MSP_STATUS_EX:
|
||||
return msp_process_out_status(dst);
|
||||
case MSP_RAW_GPS:
|
||||
return msp_process_out_raw_gps(dst);
|
||||
case MSP_COMP_GPS:
|
||||
return msp_process_out_comp_gps(dst);
|
||||
case MSP_ATTITUDE:
|
||||
return msp_process_out_attitude(dst);
|
||||
case MSP_ALTITUDE:
|
||||
return msp_process_out_altitude(dst);
|
||||
case MSP_ANALOG:
|
||||
return msp_process_out_analog(dst);
|
||||
case MSP_BATTERY_STATE:
|
||||
return msp_process_out_battery_state(dst);
|
||||
case MSP_UID:
|
||||
return msp_process_out_uid(dst);
|
||||
#ifdef HAVE_AP_BLHELI_SUPPORT
|
||||
case MSP_ESC_SENSOR_DATA:
|
||||
return msp_process_out_esc_sensor_data(dst);
|
||||
#endif
|
||||
case MSP_RTC:
|
||||
return msp_process_out_rtc(dst);
|
||||
case MSP_RC:
|
||||
return msp_process_out_rc(dst);
|
||||
default:
|
||||
return MSP_RESULT_ERROR;
|
||||
}
|
||||
}
|
||||
|
||||
MSPCommandResult AP_MSP_Telem_Backend::msp_process_sensor_command(uint16_t cmd_msp, sbuf_t *src)
|
||||
{
|
||||
MSP_UNUSED(src);
|
||||
|
||||
switch (cmd_msp) {
|
||||
case MSP2_SENSOR_RANGEFINDER: {
|
||||
const msp_rangefinder_sensor_t pkt = *(const msp_rangefinder_sensor_t *)src->ptr;
|
||||
msp_handle_rangefinder(pkt);
|
||||
}
|
||||
break;
|
||||
case MSP2_SENSOR_OPTIC_FLOW: {
|
||||
const msp_opflow_sensor_t pkt = *(const msp_opflow_sensor_t *)src->ptr;
|
||||
msp_handle_opflow(pkt);
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
return MSP_RESULT_NO_REPLY;
|
||||
}
|
||||
|
||||
void AP_MSP_Telem_Backend::msp_handle_opflow(const msp_opflow_sensor_t &pkt)
|
||||
{
|
||||
OpticalFlow *optflow = AP::opticalflow();
|
||||
if (optflow == nullptr) {
|
||||
return;
|
||||
}
|
||||
optflow->handle_msp(pkt);
|
||||
}
|
||||
|
||||
void AP_MSP_Telem_Backend::msp_handle_rangefinder(const msp_rangefinder_sensor_t &pkt)
|
||||
{
|
||||
RangeFinder *rangefinder = AP::rangefinder();
|
||||
if (rangefinder == nullptr) {
|
||||
return;
|
||||
}
|
||||
rangefinder->handle_msp(pkt);
|
||||
}
|
||||
|
||||
MSPCommandResult AP_MSP_Telem_Backend::msp_process_out_raw_gps(sbuf_t *dst)
|
||||
{
|
||||
#if OSD_ENABLED
|
||||
AP_OSD *osd = AP::osd();
|
||||
if (osd == nullptr) {
|
||||
return MSP_RESULT_ERROR;
|
||||
}
|
||||
#endif
|
||||
gps_state_t gps_state;
|
||||
update_gps_state(gps_state);
|
||||
|
||||
sbuf_write_u8(dst, gps_state.gps_fix_type >= 3 ? 2 : 0); // bitmask 1 << 1 is GPS FIX
|
||||
sbuf_write_u8(dst, gps_state.gps_num_sats);
|
||||
sbuf_write_u32(dst, gps_state.gps_latitude);
|
||||
sbuf_write_u32(dst, gps_state.gps_longitude);
|
||||
sbuf_write_u16(dst, (uint16_t)constrain_int32(gps_state.gps_altitude_cm / 100, 0, UINT16_MAX)); // alt changed from 1m to 0.01m per lsb since MSP API 1.39 by RTH. To maintain backwards compatibility compensate to 1m per lsb in MSP again.
|
||||
// handle airspeed override
|
||||
bool airspeed_en = false;
|
||||
#if OSD_ENABLED
|
||||
airspeed_en = osd->screen[0].aspeed.enabled;
|
||||
#endif
|
||||
if (airspeed_en) {
|
||||
airspeed_state_t airspeed_state;
|
||||
update_airspeed(airspeed_state);
|
||||
sbuf_write_u16(dst, airspeed_state.airspeed_estimate_ms * 100); // airspeed in cm/s
|
||||
} else {
|
||||
sbuf_write_u16(dst, (uint16_t)roundf(gps_state.gps_speed_ms * 100)); // speed in cm/s
|
||||
}
|
||||
sbuf_write_u16(dst, gps_state.gps_ground_course_cd * 1000); // degrees * 10 == centideg * 1000
|
||||
return MSP_RESULT_ACK;
|
||||
}
|
||||
|
||||
MSPCommandResult AP_MSP_Telem_Backend::msp_process_out_comp_gps(sbuf_t *dst)
|
||||
{
|
||||
home_state_t home_state;
|
||||
update_home_pos(home_state);
|
||||
|
||||
// no need to apply yaw compensation, the DJI air unit will do it for us :-)
|
||||
int32_t home_angle_deg = home_state.home_bearing_cd * 0.01;
|
||||
if (home_state.home_distance_m < 2) {
|
||||
//avoid fast rotating arrow at small distances
|
||||
home_angle_deg = 0;
|
||||
}
|
||||
|
||||
sbuf_write_u16(dst, home_state.home_distance_m);
|
||||
sbuf_write_u16(dst, (uint16_t)home_angle_deg); //deg
|
||||
sbuf_write_u8(dst, 1); // 1 is toggle GPS position update
|
||||
return MSP_RESULT_ACK;
|
||||
}
|
||||
|
||||
// Autoscroll message is the same as in minimosd-extra.
|
||||
// Thanks to night-ghost for the approach.
|
||||
MSPCommandResult AP_MSP_Telem_Backend::msp_process_out_name(sbuf_t *dst)
|
||||
{
|
||||
#if OSD_ENABLED
|
||||
AP_OSD *osd = AP::osd();
|
||||
if (osd == nullptr) {
|
||||
return MSP_RESULT_ERROR;
|
||||
}
|
||||
#endif
|
||||
AP_MSP *msp = AP::msp();
|
||||
if (msp == nullptr) {
|
||||
return MSP_RESULT_ERROR;
|
||||
}
|
||||
AP_Notify * notify = AP_Notify::get_singleton();
|
||||
if (notify) {
|
||||
uint16_t msgtime_ms = 10000; //default is 10 secs
|
||||
#if OSD_ENABLED
|
||||
msgtime_ms = AP::osd()->msgtime_s * 1000;
|
||||
#endif
|
||||
// text message is visible for _msp.msgtime_s but only if
|
||||
// a flight mode change did not steal focus
|
||||
const uint32_t visible_time_ms = AP_HAL::millis() - notify->get_text_updated_millis();
|
||||
if (visible_time_ms < msgtime_ms && !msp->_msp_status.flight_mode_focus) {
|
||||
char buffer[NOTIFY_TEXT_BUFFER_SIZE];
|
||||
strncpy(buffer, notify->get_text(), ARRAY_SIZE(buffer));
|
||||
const uint8_t len = strnlen(buffer, ARRAY_SIZE(buffer));
|
||||
|
||||
for (uint8_t i=0; i<len; i++) {
|
||||
//normalize whitespace
|
||||
if (isspace(buffer[i])) {
|
||||
buffer[i] = ' ';
|
||||
} else {
|
||||
//converted to uppercase,
|
||||
buffer[i] = toupper(buffer[i]);
|
||||
}
|
||||
}
|
||||
|
||||
int8_t start_position = 0;
|
||||
//scroll if required
|
||||
//scroll pattern: wait, scroll to the left, wait, scroll to the right
|
||||
if (len > MSP_TXT_VISIBLE_CHARS) {
|
||||
const uint8_t chars_to_scroll = len - MSP_TXT_VISIBLE_CHARS;
|
||||
const uint8_t total_cycles = 2*message_scroll_delay + 2*chars_to_scroll;
|
||||
const uint8_t current_cycle = (visible_time_ms / message_scroll_time_ms) % total_cycles;
|
||||
|
||||
//calculate scroll start_position
|
||||
if (current_cycle < total_cycles/2) {
|
||||
//move to the left
|
||||
start_position = current_cycle - message_scroll_delay;
|
||||
} else {
|
||||
//move to the right
|
||||
start_position = total_cycles - current_cycle;
|
||||
}
|
||||
start_position = constrain_int16(start_position, 0, chars_to_scroll);
|
||||
uint8_t end_position = start_position + MSP_TXT_VISIBLE_CHARS;
|
||||
|
||||
//ensure array boundaries
|
||||
start_position = MIN(start_position, int8_t(ARRAY_SIZE(buffer)-1));
|
||||
end_position = MIN(end_position, int8_t(ARRAY_SIZE(buffer)-1));
|
||||
|
||||
//trim invisible part
|
||||
buffer[end_position] = 0;
|
||||
}
|
||||
|
||||
sbuf_write_data(dst, buffer + start_position, strlen(buffer + start_position)); // max MSP_TXT_VISIBLE_CHARS chars general text...
|
||||
} else {
|
||||
bool wind_en = false;
|
||||
char flight_mode_str[MSP_TXT_BUFFER_SIZE];
|
||||
#if OSD_ENABLED
|
||||
wind_en = osd->screen[0].wind.enabled;
|
||||
#endif
|
||||
update_flight_mode_str(flight_mode_str, wind_en);
|
||||
sbuf_write_data(dst, flight_mode_str, ARRAY_SIZE(flight_mode_str)); // rendered as up to MSP_TXT_VISIBLE_CHARS chars with UTF8 support
|
||||
}
|
||||
}
|
||||
return MSP_RESULT_ACK;
|
||||
}
|
||||
|
||||
MSPCommandResult AP_MSP_Telem_Backend::msp_process_out_status(sbuf_t *dst)
|
||||
{
|
||||
const uint32_t mode_bitmask = get_osd_flight_mode_bitmask();
|
||||
sbuf_write_u16(dst, 0); // task delta time
|
||||
sbuf_write_u16(dst, 0); // I2C error count
|
||||
sbuf_write_u16(dst, 0); // sensor status
|
||||
sbuf_write_data(dst, &mode_bitmask, 4); // unconditional part of flags, first 32 bits
|
||||
sbuf_write_u8(dst, 0);
|
||||
|
||||
sbuf_write_u16(dst, constrain_int16(0, 0, 100)); //system load
|
||||
sbuf_write_u16(dst, 0); // gyro cycle time
|
||||
|
||||
// Cap BoxModeFlags to 32 bits
|
||||
sbuf_write_u8(dst, 0);
|
||||
|
||||
// Write arming disable flags
|
||||
sbuf_write_u8(dst, 1);
|
||||
sbuf_write_u32(dst, !AP::notify().flags.armed);
|
||||
|
||||
// Extra flags
|
||||
sbuf_write_u8(dst, 0);
|
||||
return MSP_RESULT_ACK;
|
||||
}
|
||||
|
||||
MSPCommandResult AP_MSP_Telem_Backend::msp_process_out_osd_config(sbuf_t *dst)
|
||||
{
|
||||
#if OSD_ENABLED
|
||||
AP_OSD *osd = AP::osd();
|
||||
if (osd == nullptr) {
|
||||
return MSP_RESULT_ERROR;
|
||||
}
|
||||
#endif
|
||||
const AP_MSP *msp = AP::msp();
|
||||
if (msp == nullptr) {
|
||||
return MSP_RESULT_ERROR;
|
||||
}
|
||||
sbuf_write_u8(dst, OSD_FLAGS_OSD_FEATURE); // flags
|
||||
sbuf_write_u8(dst, 0); // video system
|
||||
// Configuration
|
||||
uint8_t units = OSD_UNIT_METRIC;
|
||||
#if OSD_ENABLED
|
||||
units = osd->units == AP_OSD::UNITS_METRIC ? OSD_UNIT_METRIC : OSD_UNIT_IMPERIAL;
|
||||
#endif
|
||||
|
||||
sbuf_write_u8(dst, units); // units
|
||||
// Alarms
|
||||
sbuf_write_u8(dst, msp->_osd_config.rssi_alarm); // rssi alarm
|
||||
sbuf_write_u16(dst, msp->_osd_config.cap_alarm); // capacity alarm
|
||||
// Reuse old timer alarm (U16) as OSD_ITEM_COUNT
|
||||
sbuf_write_u8(dst, 0);
|
||||
sbuf_write_u8(dst, OSD_ITEM_COUNT); // osd items count
|
||||
|
||||
sbuf_write_u16(dst, msp->_osd_config.alt_alarm); // altitude alarm
|
||||
|
||||
// element position and visibility
|
||||
uint16_t pos = 0; // default is hide this element
|
||||
for (uint8_t i = 0; i < OSD_ITEM_COUNT; i++) {
|
||||
pos = 0; // 0 is hide this item
|
||||
if (msp->_osd_item_settings[i] != nullptr) { // ok supported
|
||||
if (msp->_osd_item_settings[i]->enabled) { // ok enabled
|
||||
// let's check if we need to hide this dynamically
|
||||
if (!BIT_IS_SET(osd_hidden_items_bitmask, i)) {
|
||||
pos = MSP_OSD_POS(msp->_osd_item_settings[i]);
|
||||
}
|
||||
}
|
||||
}
|
||||
sbuf_write_u16(dst, pos);
|
||||
}
|
||||
|
||||
// post flight statistics
|
||||
sbuf_write_u8(dst, OSD_STAT_COUNT); // stats items count
|
||||
for (uint8_t i = 0; i < OSD_STAT_COUNT; i++ ) {
|
||||
sbuf_write_u16(dst, 0); // stats not supported
|
||||
}
|
||||
|
||||
// timers
|
||||
sbuf_write_u8(dst, OSD_TIMER_COUNT); // timers
|
||||
for (uint8_t i = 0; i < OSD_TIMER_COUNT; i++) {
|
||||
// no timer support
|
||||
sbuf_write_u16(dst, 0);
|
||||
}
|
||||
|
||||
// Enabled warnings
|
||||
// API < 1.41
|
||||
// Send low word first for backwards compatibility
|
||||
sbuf_write_u16(dst, (uint16_t)(msp->_osd_config.enabled_warnings & 0xFFFF)); // Enabled warnings
|
||||
// API >= 1.41
|
||||
// Send the warnings count and 32bit enabled warnings flags.
|
||||
// Add currently active OSD profile (0 indicates OSD profiles not available).
|
||||
// Add OSD stick overlay mode (0 indicates OSD stick overlay not available).
|
||||
sbuf_write_u8(dst, OSD_WARNING_COUNT); // warning count
|
||||
sbuf_write_u32(dst, msp->_osd_config.enabled_warnings); // enabled warning
|
||||
|
||||
// If the feature is not available there is only 1 profile and it's always selected
|
||||
sbuf_write_u8(dst, 1); // available profiles
|
||||
sbuf_write_u8(dst, 1); // selected profile
|
||||
|
||||
sbuf_write_u8(dst, 0); // OSD stick overlay
|
||||
|
||||
// API >= 1.43
|
||||
// Add the camera frame element width/height
|
||||
//sbuf_write_u8(dst, osdConfig()->camera_frame_width);
|
||||
//sbuf_write_u8(dst, osdConfig()->camera_frame_height);
|
||||
return MSP_RESULT_ACK;
|
||||
}
|
||||
|
||||
MSPCommandResult AP_MSP_Telem_Backend::msp_process_out_attitude(sbuf_t *dst)
|
||||
{
|
||||
AP_AHRS &ahrs = AP::ahrs();
|
||||
WITH_SEMAPHORE(ahrs.get_semaphore());
|
||||
sbuf_write_u16(dst, (int16_t)(ahrs.roll_sensor * 0.1)); // centidegress to decidegrees
|
||||
sbuf_write_u16(dst, (int16_t)(ahrs.pitch_sensor * 0.1)); // centidegress to decidegrees
|
||||
sbuf_write_u16(dst, (int16_t)ahrs.yaw_sensor * 0.01); // centidegrees to degrees
|
||||
return MSP_RESULT_ACK;
|
||||
}
|
||||
|
||||
MSPCommandResult AP_MSP_Telem_Backend::msp_process_out_altitude(sbuf_t *dst)
|
||||
{
|
||||
home_state_t home_state;
|
||||
update_home_pos(home_state);
|
||||
|
||||
sbuf_write_u32(dst, home_state.rel_altitude_cm); // relative altitude cm
|
||||
sbuf_write_u16(dst, (int16_t)get_vspeed_ms() * 100); // climb rate cm/s
|
||||
return MSP_RESULT_ACK;
|
||||
}
|
||||
|
||||
MSPCommandResult AP_MSP_Telem_Backend::msp_process_out_analog(sbuf_t *dst)
|
||||
{
|
||||
AP_RSSI* rssi = AP::rssi();
|
||||
if (rssi == nullptr) {
|
||||
return MSP_RESULT_ERROR;
|
||||
}
|
||||
battery_state_t battery_state;
|
||||
update_battery_state(battery_state);
|
||||
sbuf_write_u8(dst, (uint8_t)constrain_int16(battery_state.batt_voltage_v * 10, 0, 255)); // battery voltage V to dV
|
||||
sbuf_write_u16(dst, constrain_int16(battery_state.batt_consumed_mah, 0, 0xFFFF)); // milliamp hours drawn from battery
|
||||
sbuf_write_u16(dst, rssi->enabled() ? rssi->read_receiver_rssi() * 1023 : 0); // rssi 0-1 to 0-1023
|
||||
sbuf_write_u16(dst, constrain_int16(battery_state.batt_current_a * 100, -0x8000, 0x7FFF)); // current A to cA (0.01 steps, range is -320A to 320A)
|
||||
sbuf_write_u16(dst, constrain_int16(battery_state.batt_voltage_v * 100,0,0xFFFF)); // battery voltage in 0.01V steps
|
||||
|
||||
return MSP_RESULT_ACK;
|
||||
}
|
||||
|
||||
MSPCommandResult AP_MSP_Telem_Backend::msp_process_out_battery_state(sbuf_t *dst)
|
||||
{
|
||||
const AP_MSP *msp = AP::msp();
|
||||
if (msp == nullptr) {
|
||||
return MSP_RESULT_ERROR;
|
||||
}
|
||||
battery_state_t battery_state;
|
||||
update_battery_state(battery_state);
|
||||
|
||||
// battery characteristics
|
||||
sbuf_write_u8(dst, (uint8_t)constrain_int16((msp->_cellcount > 0 ? msp->_cellcount : battery_state.batt_cellcount), 0, 255)); // cell count 0 indicates battery not detected.
|
||||
sbuf_write_u16(dst, battery_state.batt_capacity_mah); // in mAh
|
||||
|
||||
// battery state
|
||||
sbuf_write_u8(dst, (uint8_t)constrain_int16(battery_state.batt_voltage_v * 10, 0, 255)); // battery voltage V to dV
|
||||
sbuf_write_u16(dst, (uint16_t)MIN(battery_state.batt_consumed_mah, 0xFFFF)); // milliamp hours drawn from battery
|
||||
sbuf_write_u16(dst, constrain_int16(battery_state.batt_current_a * 100, -0x8000, 0x7FFF)); // current A to cA (0.01 steps, range is -320A to 320A)
|
||||
|
||||
// battery alerts
|
||||
sbuf_write_u8(dst, battery_state.batt_state); // BATTERY: OK=0, CRITICAL=2
|
||||
|
||||
sbuf_write_u16(dst, constrain_int16(battery_state.batt_voltage_v * 100, 0, 0x7FFF)); // battery voltage in 0.01V steps
|
||||
return MSP_RESULT_ACK;
|
||||
}
|
||||
|
||||
MSPCommandResult AP_MSP_Telem_Backend::msp_process_out_esc_sensor_data(sbuf_t *dst)
|
||||
{
|
||||
#ifdef HAVE_AP_BLHELI_SUPPORT
|
||||
AP_BLHeli *blheli = AP_BLHeli::get_singleton();
|
||||
if (blheli) {
|
||||
AP_BLHeli::telem_data td;
|
||||
sbuf_write_u8(dst, blheli->get_num_motors());
|
||||
for (uint8_t i = 0; i < blheli->get_num_motors(); i++) {
|
||||
if (blheli->get_telem_data(i, td)) {
|
||||
sbuf_write_u8(dst, td.temperature); // deg
|
||||
sbuf_write_u16(dst, td.rpm * 0.01); // eRpm to 0.01 eRpm
|
||||
}
|
||||
}
|
||||
}
|
||||
#endif
|
||||
return MSP_RESULT_ACK;
|
||||
}
|
||||
|
||||
MSPCommandResult AP_MSP_Telem_Backend::msp_process_out_rtc(sbuf_t *dst)
|
||||
{
|
||||
tm localtime_tm {}; // year is relative to 1900
|
||||
uint64_t time_usec;
|
||||
if (AP::rtc().get_utc_usec(time_usec)) { // may fail, leaving time_unix at 0
|
||||
const time_t time_sec = time_usec / 1000000;
|
||||
localtime_tm = *gmtime(&time_sec);
|
||||
}
|
||||
sbuf_write_u16(dst, localtime_tm.tm_year + 1900); // tm_year is relative to year 1900
|
||||
sbuf_write_u8(dst, localtime_tm.tm_mon + 1); // MSP requires [1-12] months
|
||||
sbuf_write_u8(dst, localtime_tm.tm_mday);
|
||||
sbuf_write_u8(dst, localtime_tm.tm_hour);
|
||||
sbuf_write_u8(dst, localtime_tm.tm_min);
|
||||
sbuf_write_u8(dst, localtime_tm.tm_sec);
|
||||
sbuf_write_u16(dst, 0);
|
||||
return MSP_RESULT_ACK;
|
||||
}
|
||||
|
||||
MSPCommandResult AP_MSP_Telem_Backend::msp_process_out_rc(sbuf_t *dst)
|
||||
{
|
||||
const RCMapper* rcmap = AP::rcmap();
|
||||
if (rcmap == nullptr) {
|
||||
return MSP_RESULT_ERROR;
|
||||
}
|
||||
uint16_t values[16] = {};
|
||||
rc().get_radio_in(values, ARRAY_SIZE(values));
|
||||
|
||||
// send only 4 channels, MSP order is AERT
|
||||
sbuf_write_u16(dst, values[rcmap->roll()]); // A
|
||||
sbuf_write_u16(dst, values[rcmap->pitch()]); // E
|
||||
sbuf_write_u16(dst, values[rcmap->yaw()]); // R
|
||||
sbuf_write_u16(dst, values[rcmap->throttle()]); // T
|
||||
|
||||
return MSP_RESULT_ACK;
|
||||
}
|
||||
|
||||
MSPCommandResult AP_MSP_Telem_Backend::msp_process_out_board_info(sbuf_t *dst)
|
||||
{
|
||||
const AP_FWVersion &fwver = AP::fwversion();
|
||||
|
||||
sbuf_write_data(dst, "ARDU", BOARD_IDENTIFIER_LENGTH);
|
||||
sbuf_write_u16(dst, 0);
|
||||
sbuf_write_u8(dst, 0);
|
||||
sbuf_write_u8(dst, 0);
|
||||
sbuf_write_u8(dst, strlen(fwver.fw_string));
|
||||
sbuf_write_data(dst, fwver.fw_string, strlen(fwver.fw_string));
|
||||
return MSP_RESULT_ACK;
|
||||
}
|
||||
|
||||
MSPCommandResult AP_MSP_Telem_Backend::msp_process_out_build_info(sbuf_t *dst)
|
||||
{
|
||||
const AP_FWVersion &fwver = AP::fwversion();
|
||||
|
||||
sbuf_write_data(dst, __DATE__, BUILD_DATE_LENGTH);
|
||||
sbuf_write_data(dst, __TIME__, BUILD_TIME_LENGTH);
|
||||
sbuf_write_data(dst, fwver.fw_hash_str, GIT_SHORT_REVISION_LENGTH);
|
||||
return MSP_RESULT_ACK;
|
||||
}
|
||||
|
||||
MSPCommandResult AP_MSP_Telem_Backend::msp_process_out_uid(sbuf_t *dst)
|
||||
{
|
||||
sbuf_write_u32(dst, 0xAABBCCDD);
|
||||
sbuf_write_u32(dst, 0xAABBCCDD);
|
||||
sbuf_write_u32(dst, 0xAABBCCDD);
|
||||
return MSP_RESULT_ACK;
|
||||
}
|
||||
|
||||
void AP_MSP_Telem_Backend::hide_osd_items(void)
|
||||
{
|
||||
#if OSD_ENABLED
|
||||
AP_OSD *osd = AP::osd();
|
||||
if (osd == nullptr) {
|
||||
return;
|
||||
}
|
||||
#endif
|
||||
AP_MSP *msp = AP::msp();
|
||||
if (msp == nullptr) {
|
||||
return;
|
||||
}
|
||||
const AP_Notify ¬ify = AP::notify();
|
||||
// clear all and only set the flashing ones
|
||||
BIT_CLEAR(osd_hidden_items_bitmask, OSD_GPS_SATS);
|
||||
BIT_CLEAR(osd_hidden_items_bitmask, OSD_HOME_DIR);
|
||||
BIT_CLEAR(osd_hidden_items_bitmask, OSD_HOME_DIST);
|
||||
BIT_CLEAR(osd_hidden_items_bitmask, OSD_GPS_SPEED);
|
||||
BIT_CLEAR(osd_hidden_items_bitmask, OSD_CRAFT_NAME);
|
||||
BIT_CLEAR(osd_hidden_items_bitmask, OSD_AVG_CELL_VOLTAGE);
|
||||
BIT_CLEAR(osd_hidden_items_bitmask, OSD_MAIN_BATT_VOLTAGE);
|
||||
BIT_CLEAR(osd_hidden_items_bitmask, OSD_RTC_DATETIME);
|
||||
|
||||
if (msp->_msp_status.flashing_on) {
|
||||
// flash satcount when no 3D Fix
|
||||
gps_state_t gps_state;
|
||||
update_gps_state(gps_state);
|
||||
if (gps_state.gps_fix_type < AP_GPS::GPS_Status::GPS_OK_FIX_3D) {
|
||||
BIT_SET(osd_hidden_items_bitmask, OSD_GPS_SATS);
|
||||
}
|
||||
// flash home dir and distance if home is not set
|
||||
home_state_t home_state;
|
||||
update_home_pos(home_state);
|
||||
if (!home_state.home_is_set) {
|
||||
BIT_SET(osd_hidden_items_bitmask, OSD_HOME_DIR);
|
||||
BIT_SET(osd_hidden_items_bitmask, OSD_HOME_DIST);
|
||||
}
|
||||
// flash airspeed if there's no estimate
|
||||
bool airspeed_en = false;
|
||||
#if OSD_ENABLED
|
||||
airspeed_en = osd->screen[0].aspeed.enabled;
|
||||
#endif
|
||||
if (airspeed_en) {
|
||||
airspeed_state_t airspeed_state;
|
||||
update_airspeed(airspeed_state);
|
||||
if (!airspeed_state.airspeed_have_estimate) {
|
||||
BIT_SET(osd_hidden_items_bitmask, OSD_GPS_SPEED);
|
||||
}
|
||||
}
|
||||
// flash text flightmode for 3secs after each change
|
||||
if (msp->_msp_status.flight_mode_focus) {
|
||||
BIT_SET(osd_hidden_items_bitmask, OSD_CRAFT_NAME);
|
||||
}
|
||||
// flash battery on failsafe
|
||||
if (notify.flags.failsafe_battery) {
|
||||
BIT_SET(osd_hidden_items_bitmask, OSD_AVG_CELL_VOLTAGE);
|
||||
BIT_SET(osd_hidden_items_bitmask, OSD_MAIN_BATT_VOLTAGE);
|
||||
}
|
||||
// flash rtc if no time available
|
||||
uint64_t time_usec;
|
||||
if (!AP::rtc().get_utc_usec(time_usec)) {
|
||||
BIT_SET(osd_hidden_items_bitmask, OSD_RTC_DATETIME);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#endif //HAL_MSP_ENABLED
|
||||
|
|
@ -0,0 +1,191 @@
|
|||
/*
|
||||
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 "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
|
||||
|
||||
using namespace MSP;
|
||||
|
||||
class AP_MSP;
|
||||
|
||||
class AP_MSP_Telem_Backend : AP_RCTelemetry
|
||||
{
|
||||
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;
|
||||
battery_state_e batt_state;
|
||||
} battery_state_t;
|
||||
|
||||
typedef struct gps_state_s {
|
||||
int32_t gps_latitude;
|
||||
int32_t gps_longitude;
|
||||
uint8_t gps_num_sats;
|
||||
int32_t gps_altitude_cm;
|
||||
float gps_speed_ms;
|
||||
uint16_t gps_ground_course_cd;
|
||||
uint8_t gps_fix_type;
|
||||
} 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
|
||||
|
||||
protected:
|
||||
enum msp_packet_type : uint8_t {
|
||||
EMPTY_SLOT = 0,
|
||||
NAME,
|
||||
STATUS,
|
||||
CONFIG,
|
||||
RAW_GPS,
|
||||
COMP_GPS,
|
||||
ATTITUDE,
|
||||
ALTITUDE,
|
||||
ANALOG,
|
||||
BATTERY_STATE,
|
||||
ESC_SENSOR_DATA,
|
||||
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,
|
||||
MSP_ESC_SENSOR_DATA,
|
||||
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_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);
|
||||
float get_vspeed_ms(void);
|
||||
void update_home_pos(home_state_t &home_state);
|
||||
void update_battery_state(battery_state_t &_battery_state);
|
||||
void update_gps_state(gps_state_t &gps_state);
|
||||
void update_airspeed(airspeed_state_t &airspeed_state);
|
||||
void update_flight_mode_str(char *flight_mode_str, bool wind_enabled);
|
||||
|
||||
// MSP parsing
|
||||
void msp_process_received_command();
|
||||
MSPCommandResult msp_process_command(msp_packet_t *cmd, msp_packet_t *reply);
|
||||
MSPCommandResult msp_process_sensor_command(uint16_t cmd_msp, sbuf_t *src);
|
||||
MSPCommandResult msp_process_out_command(uint16_t cmd_msp, sbuf_t *dst);
|
||||
|
||||
// MSP sensor command processing
|
||||
void msp_handle_opflow(const msp_opflow_sensor_t &pkt);
|
||||
void msp_handle_rangefinder(const msp_rangefinder_sensor_t &pkt);
|
||||
|
||||
// implementation specific helpers
|
||||
virtual uint32_t get_osd_flight_mode_bitmask(void)
|
||||
{
|
||||
return 0;
|
||||
}; // custom masks are needed for vendor specific settings
|
||||
virtual bool is_scheduler_enabled() = 0; // only osd backends should allow a push type telemetry
|
||||
|
||||
// implementation specific MSP out command processing
|
||||
virtual MSPCommandResult msp_process_out_api_version(sbuf_t *dst) = 0;
|
||||
virtual MSPCommandResult msp_process_out_fc_version(sbuf_t *dst) = 0;
|
||||
virtual MSPCommandResult msp_process_out_fc_variant(sbuf_t *dst) = 0;
|
||||
virtual MSPCommandResult msp_process_out_uid(sbuf_t *dst);
|
||||
virtual MSPCommandResult msp_process_out_board_info(sbuf_t *dst);
|
||||
virtual MSPCommandResult msp_process_out_build_info(sbuf_t *dst);
|
||||
virtual MSPCommandResult msp_process_out_name(sbuf_t *dst);
|
||||
virtual MSPCommandResult msp_process_out_status(sbuf_t *dst);
|
||||
virtual MSPCommandResult msp_process_out_osd_config(sbuf_t *dst);
|
||||
virtual MSPCommandResult msp_process_out_raw_gps(sbuf_t *dst);
|
||||
virtual MSPCommandResult msp_process_out_comp_gps(sbuf_t *dst);
|
||||
virtual MSPCommandResult msp_process_out_attitude(sbuf_t *dst);
|
||||
virtual MSPCommandResult msp_process_out_altitude(sbuf_t *dst);
|
||||
virtual MSPCommandResult msp_process_out_analog(sbuf_t *dst);
|
||||
virtual MSPCommandResult msp_process_out_battery_state(sbuf_t *dst);
|
||||
virtual MSPCommandResult msp_process_out_esc_sensor_data(sbuf_t *dst);
|
||||
virtual MSPCommandResult msp_process_out_rtc(sbuf_t *dst);
|
||||
virtual MSPCommandResult msp_process_out_rc(sbuf_t *dst);
|
||||
};
|
||||
|
||||
#endif //HAL_MSP_ENABLED
|
||||
|
|
@ -0,0 +1,105 @@
|
|||
/*
|
||||
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/>.
|
||||
*/
|
||||
|
||||
#include <AP_Common/AP_FWVersion.h>
|
||||
#include <AP_Vehicle/AP_Vehicle_Type.h>
|
||||
|
||||
#include "AP_MSP.h"
|
||||
#include "AP_MSP_Telem_DJI.h"
|
||||
|
||||
#if HAL_MSP_ENABLED
|
||||
extern const AP_HAL::HAL& hal;
|
||||
|
||||
bool AP_MSP_Telem_DJI::init_uart()
|
||||
{
|
||||
if (_msp_port.uart != nullptr) {
|
||||
_msp_port.uart->set_flow_control(AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE);
|
||||
_msp_port.uart->begin(AP_SERIALMANAGER_MSP_BAUD, AP_SERIALMANAGER_MSP_BUFSIZE_RX, AP_SERIALMANAGER_MSP_BUFSIZE_TX);
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
bool AP_MSP_Telem_DJI::is_scheduler_enabled()
|
||||
{
|
||||
AP_MSP *msp = AP::msp();
|
||||
if (msp == nullptr) {
|
||||
return false;
|
||||
}
|
||||
return msp->check_option(AP_MSP::OPTION_TELEMETRY_MODE);
|
||||
}
|
||||
|
||||
void AP_MSP_Telem_DJI::hide_osd_items(void)
|
||||
{
|
||||
AP_MSP *msp = AP::msp();
|
||||
if (msp == nullptr) {
|
||||
return;
|
||||
}
|
||||
// apply base class defaults
|
||||
AP_MSP_Telem_Backend::hide_osd_items();
|
||||
|
||||
// apply DJI OSD specific rules
|
||||
const AP_Notify& notify = AP::notify();
|
||||
// default is hide the DJI flightmode widget
|
||||
BIT_SET(osd_hidden_items_bitmask, OSD_FLYMODE);
|
||||
|
||||
if (msp->_msp_status.flashing_on) {
|
||||
// flash flightmode on failsafe
|
||||
if (notify.flags.failsafe_battery || notify.flags.failsafe_gcs || notify.flags.failsafe_radio || notify.flags.ekf_bad) {
|
||||
BIT_CLEAR(osd_hidden_items_bitmask, OSD_FLYMODE);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
uint32_t AP_MSP_Telem_DJI::get_osd_flight_mode_bitmask(void)
|
||||
{
|
||||
uint32_t mode_mask = 0;
|
||||
const AP_Notify& notify = AP::notify();
|
||||
|
||||
// set arming status
|
||||
if (notify.flags.armed) {
|
||||
BIT_SET(mode_mask, DJI_FLAG_ARM);
|
||||
}
|
||||
|
||||
// check failsafe
|
||||
if (notify.flags.failsafe_battery || notify.flags.failsafe_gcs || notify.flags.failsafe_radio || notify.flags.ekf_bad ) {
|
||||
BIT_SET(mode_mask, DJI_FLAG_FS);
|
||||
}
|
||||
return mode_mask;
|
||||
}
|
||||
|
||||
MSPCommandResult AP_MSP_Telem_DJI::msp_process_out_api_version(sbuf_t *dst)
|
||||
{
|
||||
sbuf_write_u8(dst, MSP_PROTOCOL_VERSION);
|
||||
sbuf_write_u8(dst, API_VERSION_MAJOR);
|
||||
sbuf_write_u8(dst, API_VERSION_MINOR);
|
||||
return MSP_RESULT_ACK;
|
||||
}
|
||||
|
||||
MSPCommandResult AP_MSP_Telem_DJI::msp_process_out_fc_version(sbuf_t *dst)
|
||||
{
|
||||
sbuf_write_u8(dst, FC_VERSION_MAJOR);
|
||||
sbuf_write_u8(dst, FC_VERSION_MINOR);
|
||||
sbuf_write_u8(dst, FC_VERSION_PATCH_LEVEL);
|
||||
return MSP_RESULT_ACK;
|
||||
}
|
||||
|
||||
MSPCommandResult AP_MSP_Telem_DJI::msp_process_out_fc_variant(sbuf_t *dst)
|
||||
{
|
||||
sbuf_write_data(dst, "BTFL", FLIGHT_CONTROLLER_IDENTIFIER_LENGTH);
|
||||
return MSP_RESULT_ACK;
|
||||
}
|
||||
|
||||
#endif //HAL_MSP_ENABLED
|
||||
|
|
@ -0,0 +1,71 @@
|
|||
/*
|
||||
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/>.
|
||||
|
||||
DJI FPV MSP telemetry library backend
|
||||
|
||||
The DJI Air Unit polls the FC for the following MSP messages at around 4Hz.
|
||||
Note: messages are polled in ascending hex id order.
|
||||
|
||||
Hex|Dec|Name
|
||||
---------------------------
|
||||
03 03 MSP_FC_VERSION
|
||||
0a 10 MSP_NAME
|
||||
54 84 MSP_OSD_CONFIG
|
||||
5c 92 MSP_FILTER_CONFIG
|
||||
5e 94 MSP_PID_ADVANCED
|
||||
65 101 MSP_STATUS
|
||||
69 105 MSP_RC
|
||||
6a 106 MSP_RAW_GPS
|
||||
6b 107 MSP_COMP_GPS
|
||||
6c 108 MSP_ATTITUDE
|
||||
6d 109 MSP_ALTITUDE
|
||||
6e 110 MSP_ANALOG
|
||||
6f 111 MSP_RC_TUNING
|
||||
70 112 MSP_PID
|
||||
82 130 MSP_BATTERY_STATE
|
||||
86 134 MSP_ESC_SENSOR_DATA
|
||||
96 150 MSP_STATUS_EX
|
||||
f7 247 MSP_RTC
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "AP_MSP_Telem_Backend.h"
|
||||
|
||||
#if HAL_MSP_ENABLED
|
||||
|
||||
class AP_MSP_Telem_DJI : public AP_MSP_Telem_Backend
|
||||
{
|
||||
using AP_MSP_Telem_Backend::AP_MSP_Telem_Backend;
|
||||
public:
|
||||
bool init_uart() override;
|
||||
// implementation specific helpers
|
||||
bool is_scheduler_enabled() override;
|
||||
uint32_t get_osd_flight_mode_bitmask(void) override;
|
||||
void hide_osd_items(void) override;
|
||||
MSPCommandResult msp_process_out_api_version(sbuf_t *dst) override;
|
||||
MSPCommandResult msp_process_out_fc_version(sbuf_t *dst) override;
|
||||
MSPCommandResult msp_process_out_fc_variant(sbuf_t *dst) override;
|
||||
|
||||
enum : uint8_t {
|
||||
DJI_FLAG_ARM = 0,
|
||||
DJI_FLAG_STAB,
|
||||
DJI_FLAG_HOR,
|
||||
DJI_FLAG_HEAD,
|
||||
DJI_FLAG_FS,
|
||||
DJI_FLAG_RESC,
|
||||
};
|
||||
};
|
||||
|
||||
#endif //HAL_MSP_ENABLED
|
||||
|
|
@ -0,0 +1,48 @@
|
|||
/*
|
||||
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/>.
|
||||
*/
|
||||
|
||||
#include <AP_Common/AP_FWVersion.h>
|
||||
#include <AP_Vehicle/AP_Vehicle_Type.h>
|
||||
|
||||
#include "AP_MSP.h"
|
||||
#include "AP_MSP_Telem_Generic.h"
|
||||
|
||||
#if HAL_MSP_ENABLED
|
||||
|
||||
extern const AP_HAL::HAL& hal;
|
||||
|
||||
MSPCommandResult AP_MSP_Telem_Generic::msp_process_out_api_version(sbuf_t *dst)
|
||||
{
|
||||
sbuf_write_u8(dst, MSP_PROTOCOL_VERSION);
|
||||
sbuf_write_u8(dst, API_VERSION_MAJOR);
|
||||
sbuf_write_u8(dst, API_VERSION_MINOR);
|
||||
return MSP_RESULT_ACK;
|
||||
}
|
||||
|
||||
MSPCommandResult AP_MSP_Telem_Generic::msp_process_out_fc_version(sbuf_t *dst)
|
||||
{
|
||||
sbuf_write_u8(dst, FC_VERSION_MAJOR);
|
||||
sbuf_write_u8(dst, FC_VERSION_MINOR);
|
||||
sbuf_write_u8(dst, FC_VERSION_PATCH_LEVEL);
|
||||
return MSP_RESULT_ACK;
|
||||
}
|
||||
|
||||
MSPCommandResult AP_MSP_Telem_Generic::msp_process_out_fc_variant(sbuf_t *dst)
|
||||
{
|
||||
sbuf_write_data(dst, "ARDU", FLIGHT_CONTROLLER_IDENTIFIER_LENGTH);
|
||||
return MSP_RESULT_ACK;
|
||||
}
|
||||
|
||||
#endif //HAL_MSP_ENABLED
|
||||
|
|
@ -0,0 +1,36 @@
|
|||
/*
|
||||
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 generic telemetry library backend
|
||||
*/
|
||||
#pragma once
|
||||
|
||||
#include "AP_MSP_Telem_Backend.h"
|
||||
|
||||
#if HAL_MSP_ENABLED
|
||||
|
||||
class AP_MSP_Telem_Generic : public AP_MSP_Telem_Backend
|
||||
{
|
||||
using AP_MSP_Telem_Backend::AP_MSP_Telem_Backend;
|
||||
public:
|
||||
bool is_scheduler_enabled() override
|
||||
{
|
||||
return false;
|
||||
};
|
||||
MSPCommandResult msp_process_out_api_version(sbuf_t *dst) override;
|
||||
MSPCommandResult msp_process_out_fc_version(sbuf_t *dst) override;
|
||||
MSPCommandResult msp_process_out_fc_variant(sbuf_t *dst) override;
|
||||
};
|
||||
|
||||
#endif //HAL_MSP_ENABLED
|
||||
|
|
@ -0,0 +1,313 @@
|
|||
#include <AP_Math/AP_Math.h>
|
||||
#include <AP_SerialManager/AP_SerialManager.h>
|
||||
|
||||
#include "msp.h"
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
#if HAL_MSP_ENABLED
|
||||
|
||||
/*
|
||||
ported from betaflight/src/main/msp/msp_serial.c
|
||||
*/
|
||||
uint8_t MSP::msp_serial_checksum_buf(uint8_t checksum, const uint8_t *data, uint32_t len)
|
||||
{
|
||||
while (len-- > 0) {
|
||||
checksum ^= *data++;
|
||||
}
|
||||
return checksum;
|
||||
}
|
||||
|
||||
/*
|
||||
ported from betaflight/src/main/msp/msp_serial.c
|
||||
*/
|
||||
uint32_t MSP::msp_serial_send_frame(msp_port_t *msp, const uint8_t * hdr, uint32_t hdr_len, const uint8_t * data, uint32_t data_len, const uint8_t * crc, uint32_t crc_len)
|
||||
{
|
||||
if (msp->uart == nullptr) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
// We are allowed to send out the response if
|
||||
// a) TX buffer is completely empty (we are talking to well-behaving party that follows request-response scheduling;
|
||||
// this allows us to transmit jumbo frames bigger than TX buffer (serialWriteBuf will block, but for jumbo frames we don't care)
|
||||
// b) Response fits into TX buffer
|
||||
const uint32_t total_frame_length = hdr_len + data_len + crc_len;
|
||||
|
||||
if (!msp->uart->tx_pending() && (msp->uart->txspace() < total_frame_length)) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
// Transmit frame
|
||||
msp->uart->write(hdr, hdr_len);
|
||||
msp->uart->write(data, data_len);
|
||||
msp->uart->write(crc, crc_len);
|
||||
|
||||
return total_frame_length;
|
||||
}
|
||||
|
||||
/*
|
||||
ported from betaflight/src/main/msp/msp_serial.c
|
||||
*/
|
||||
uint32_t MSP::msp_serial_encode(msp_port_t *msp, msp_packet_t *packet, msp_version_e msp_version)
|
||||
{
|
||||
static const uint8_t msp_magic[MSP_VERSION_COUNT] = MSP_VERSION_MAGIC_INITIALIZER;
|
||||
/*
|
||||
Note: after calling sbuf_switch_to_reader() sbuf_bytes_remaining() returns the size of the packet
|
||||
*/
|
||||
const uint16_t data_len = sbuf_bytes_remaining(&packet->buf);
|
||||
const uint8_t hdr_buf[16] = { '$', msp_magic[msp_version], static_cast<uint8_t>(packet->result == MSP_RESULT_ERROR ? '!' : '>')};
|
||||
uint8_t crc_buf[2];
|
||||
uint32_t hdr_len = 3;
|
||||
uint32_t crc_len = 0;
|
||||
|
||||
#define V1_CHECKSUM_STARTPOS 3
|
||||
if (msp_version == MSP_V1) {
|
||||
msp_header_v1_t * hdr_v1 = (msp_header_v1_t *)&hdr_buf[hdr_len];
|
||||
hdr_len += sizeof(msp_header_v1_t);
|
||||
hdr_v1->cmd = packet->cmd;
|
||||
|
||||
// Add JUMBO-frame header if necessary
|
||||
if (data_len >= JUMBO_FRAME_SIZE_LIMIT) {
|
||||
msp_header_jumbo_t * hdr_jumbo = (msp_header_jumbo_t *)&hdr_buf[hdr_len];
|
||||
hdr_len += sizeof(msp_header_jumbo_t);
|
||||
|
||||
hdr_v1->size = JUMBO_FRAME_SIZE_LIMIT;
|
||||
hdr_jumbo->size = data_len;
|
||||
} else {
|
||||
hdr_v1->size = data_len;
|
||||
}
|
||||
|
||||
// Pre-calculate CRC
|
||||
crc_buf[crc_len] = msp_serial_checksum_buf(0, hdr_buf + V1_CHECKSUM_STARTPOS, hdr_len - V1_CHECKSUM_STARTPOS);
|
||||
crc_buf[crc_len] = msp_serial_checksum_buf(crc_buf[crc_len], sbuf_ptr(&packet->buf), data_len);
|
||||
crc_len++;
|
||||
} else if (msp_version == MSP_V2_OVER_V1) {
|
||||
msp_header_v1_t * hdr_v1 = (msp_header_v1_t *)&hdr_buf[hdr_len];
|
||||
|
||||
hdr_len += sizeof(msp_header_v1_t);
|
||||
|
||||
msp_header_v2_t * hdr_v2 = (msp_header_v2_t *)&hdr_buf[hdr_len];
|
||||
hdr_len += sizeof(msp_header_v2_t);
|
||||
|
||||
const uint32_t v1_payload_size = sizeof(msp_header_v2_t) + data_len + 1; // MSPv2 header + data payload + MSPv2 checksum
|
||||
hdr_v1->cmd = MSP_V2_FRAME_ID;
|
||||
|
||||
// Add JUMBO-frame header if necessary
|
||||
if (v1_payload_size >= JUMBO_FRAME_SIZE_LIMIT) {
|
||||
msp_header_jumbo_t * hdr_jumbo = (msp_header_jumbo_t *)&hdr_buf[hdr_len];
|
||||
hdr_len += sizeof(msp_header_jumbo_t);
|
||||
|
||||
hdr_v1->size = JUMBO_FRAME_SIZE_LIMIT;
|
||||
hdr_jumbo->size = v1_payload_size;
|
||||
} else {
|
||||
hdr_v1->size = v1_payload_size;
|
||||
}
|
||||
|
||||
// Fill V2 header
|
||||
hdr_v2->flags = packet->flags;
|
||||
hdr_v2->cmd = packet->cmd;
|
||||
hdr_v2->size = data_len;
|
||||
|
||||
// V2 CRC: only V2 header + data payload
|
||||
crc_buf[crc_len] = crc8_dvb_s2_update(0, (uint8_t *)hdr_v2, sizeof(msp_header_v2_t));
|
||||
crc_buf[crc_len] = crc8_dvb_s2_update(crc_buf[crc_len], sbuf_ptr(&packet->buf), data_len);
|
||||
crc_len++;
|
||||
|
||||
// V1 CRC: All headers + data payload + V2 CRC byte
|
||||
crc_buf[crc_len] = msp_serial_checksum_buf(0, hdr_buf + V1_CHECKSUM_STARTPOS, hdr_len - V1_CHECKSUM_STARTPOS);
|
||||
crc_buf[crc_len] = msp_serial_checksum_buf(crc_buf[crc_len], sbuf_ptr(&packet->buf), data_len);
|
||||
crc_buf[crc_len] = msp_serial_checksum_buf(crc_buf[crc_len], crc_buf, crc_len);
|
||||
crc_len++;
|
||||
} else if (msp_version == MSP_V2_NATIVE) {
|
||||
msp_header_v2_t * hdr_v2 = (msp_header_v2_t *)&hdr_buf[hdr_len];
|
||||
hdr_len += sizeof(msp_header_v2_t);
|
||||
|
||||
hdr_v2->flags = packet->flags;
|
||||
hdr_v2->cmd = packet->cmd;
|
||||
hdr_v2->size = data_len;
|
||||
|
||||
crc_buf[crc_len] = crc8_dvb_s2_update(0, (uint8_t *)hdr_v2, sizeof(msp_header_v2_t));
|
||||
crc_buf[crc_len] = crc8_dvb_s2_update(crc_buf[crc_len], sbuf_ptr(&packet->buf), data_len);
|
||||
crc_len++;
|
||||
} else {
|
||||
// Shouldn't get here
|
||||
return 0;
|
||||
}
|
||||
|
||||
// Send the frame
|
||||
return msp_serial_send_frame(msp, hdr_buf, hdr_len, sbuf_ptr(&packet->buf), data_len, crc_buf, crc_len);
|
||||
}
|
||||
|
||||
/*
|
||||
ported from betaflight/src/main/msp/msp_serial.c
|
||||
*/
|
||||
bool MSP::msp_parse_received_data(msp_port_t *msp, uint8_t c)
|
||||
{
|
||||
switch (msp->c_state) {
|
||||
default:
|
||||
case MSP_IDLE: // Waiting for '$' character
|
||||
if (c == '$') {
|
||||
msp->msp_version = MSP_V1;
|
||||
msp->c_state = MSP_HEADER_START;
|
||||
} else {
|
||||
return false;
|
||||
}
|
||||
break;
|
||||
|
||||
case MSP_HEADER_START: // Waiting for 'M' (MSPv1 / MSPv2_over_v1) or 'X' (MSPv2 native)
|
||||
switch (c) {
|
||||
case 'M':
|
||||
msp->c_state = MSP_HEADER_M;
|
||||
break;
|
||||
case 'X':
|
||||
msp->c_state = MSP_HEADER_X;
|
||||
break;
|
||||
default:
|
||||
msp->c_state = MSP_IDLE;
|
||||
break;
|
||||
}
|
||||
break;
|
||||
|
||||
case MSP_HEADER_M: // Waiting for '<'
|
||||
if (c == '<') {
|
||||
msp->offset = 0;
|
||||
msp->checksum1 = 0;
|
||||
msp->checksum2 = 0;
|
||||
msp->c_state = MSP_HEADER_V1;
|
||||
} else {
|
||||
msp->c_state = MSP_IDLE;
|
||||
}
|
||||
break;
|
||||
|
||||
case MSP_HEADER_X:
|
||||
if (c == '<') {
|
||||
msp->offset = 0;
|
||||
msp->checksum2 = 0;
|
||||
msp->msp_version = MSP_V2_NATIVE;
|
||||
msp->c_state = MSP_HEADER_V2_NATIVE;
|
||||
} else {
|
||||
msp->c_state = MSP_IDLE;
|
||||
}
|
||||
break;
|
||||
|
||||
case MSP_HEADER_V1: // Now receive v1 header (size/cmd), this is already checksummable
|
||||
msp->in_buf[msp->offset++] = c;
|
||||
msp->checksum1 ^= c;
|
||||
if (msp->offset == sizeof(msp_header_v1_t)) {
|
||||
msp_header_v1_t * hdr = (msp_header_v1_t *)&msp->in_buf[0];
|
||||
// Check incoming buffer size limit
|
||||
if (hdr->size > MSP_PORT_INBUF_SIZE) {
|
||||
msp->c_state = MSP_IDLE;
|
||||
} else if (hdr->cmd == MSP_V2_FRAME_ID) {
|
||||
// MSPv1 payload must be big enough to hold V2 header + extra checksum
|
||||
if (hdr->size >= sizeof(msp_header_v2_t) + 1) {
|
||||
msp->msp_version = MSP_V2_OVER_V1;
|
||||
msp->c_state = MSP_HEADER_V2_OVER_V1;
|
||||
} else {
|
||||
msp->c_state = MSP_IDLE;
|
||||
}
|
||||
} else {
|
||||
msp->data_size = hdr->size;
|
||||
msp->cmd_msp = hdr->cmd;
|
||||
msp->cmd_flags = 0;
|
||||
msp->offset = 0; // re-use buffer
|
||||
msp->c_state = msp->data_size > 0 ? MSP_PAYLOAD_V1 : MSP_CHECKSUM_V1; // If no payload - jump to checksum byte
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
||||
case MSP_PAYLOAD_V1:
|
||||
msp->in_buf[msp->offset++] = c;
|
||||
msp->checksum1 ^= c;
|
||||
if (msp->offset == msp->data_size) {
|
||||
msp->c_state = MSP_CHECKSUM_V1;
|
||||
}
|
||||
break;
|
||||
|
||||
case MSP_CHECKSUM_V1:
|
||||
if (msp->checksum1 == c) {
|
||||
msp->c_state = MSP_COMMAND_RECEIVED;
|
||||
} else {
|
||||
msp->c_state = MSP_IDLE;
|
||||
}
|
||||
break;
|
||||
|
||||
case MSP_HEADER_V2_OVER_V1: // V2 header is part of V1 payload - we need to calculate both checksums now
|
||||
msp->in_buf[msp->offset++] = c;
|
||||
msp->checksum1 ^= c;
|
||||
msp->checksum2 = crc8_dvb_s2(msp->checksum2, c);
|
||||
if (msp->offset == (sizeof(msp_header_v2_t) + sizeof(msp_header_v1_t))) {
|
||||
msp_header_v2_t * hdrv2 = (msp_header_v2_t *)&msp->in_buf[sizeof(msp_header_v1_t)];
|
||||
msp->data_size = hdrv2->size;
|
||||
|
||||
// Check for potential buffer overflow
|
||||
if (hdrv2->size > MSP_PORT_INBUF_SIZE) {
|
||||
msp->c_state = MSP_IDLE;
|
||||
} else {
|
||||
msp->cmd_msp = hdrv2->cmd;
|
||||
msp->cmd_flags = hdrv2->flags;
|
||||
msp->offset = 0; // re-use buffer
|
||||
msp->c_state = msp->data_size > 0 ? MSP_PAYLOAD_V2_OVER_V1 : MSP_CHECKSUM_V2_OVER_V1;
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
||||
case MSP_PAYLOAD_V2_OVER_V1:
|
||||
msp->checksum2 = crc8_dvb_s2(msp->checksum2, c);
|
||||
msp->checksum1 ^= c;
|
||||
msp->in_buf[msp->offset++] = c;
|
||||
|
||||
if (msp->offset == msp->data_size) {
|
||||
msp->c_state = MSP_CHECKSUM_V2_OVER_V1;
|
||||
}
|
||||
break;
|
||||
|
||||
case MSP_CHECKSUM_V2_OVER_V1:
|
||||
msp->checksum1 ^= c;
|
||||
if (msp->checksum2 == c) {
|
||||
msp->c_state = MSP_CHECKSUM_V1; // Checksum 2 correct - verify v1 checksum
|
||||
} else {
|
||||
msp->c_state = MSP_IDLE;
|
||||
}
|
||||
break;
|
||||
|
||||
case MSP_HEADER_V2_NATIVE:
|
||||
msp->in_buf[msp->offset++] = c;
|
||||
msp->checksum2 = crc8_dvb_s2(msp->checksum2, c);
|
||||
if (msp->offset == sizeof(msp_header_v2_t)) {
|
||||
msp_header_v2_t * hdrv2 = (msp_header_v2_t *)&msp->in_buf[0];
|
||||
|
||||
// Check for potential buffer overflow
|
||||
if (hdrv2->size > MSP_PORT_INBUF_SIZE) {
|
||||
msp->c_state = MSP_IDLE;
|
||||
} else {
|
||||
msp->data_size = hdrv2->size;
|
||||
msp->cmd_msp = hdrv2->cmd;
|
||||
msp->cmd_flags = hdrv2->flags;
|
||||
msp->offset = 0; // re-use buffer
|
||||
msp->c_state = msp->data_size > 0 ? MSP_PAYLOAD_V2_NATIVE : MSP_CHECKSUM_V2_NATIVE;
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
||||
case MSP_PAYLOAD_V2_NATIVE:
|
||||
msp->checksum2 = crc8_dvb_s2(msp->checksum2, c);
|
||||
msp->in_buf[msp->offset++] = c;
|
||||
|
||||
if (msp->offset == msp->data_size) {
|
||||
msp->c_state = MSP_CHECKSUM_V2_NATIVE;
|
||||
}
|
||||
break;
|
||||
|
||||
case MSP_CHECKSUM_V2_NATIVE:
|
||||
if (msp->checksum2 == c) {
|
||||
msp->c_state = MSP_COMMAND_RECEIVED;
|
||||
} else {
|
||||
msp->c_state = MSP_IDLE;
|
||||
}
|
||||
break;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
#endif //HAL_MSP_ENABLED
|
||||
|
|
@ -0,0 +1,137 @@
|
|||
#pragma once
|
||||
|
||||
#ifndef HAL_MSP_ENABLED
|
||||
#define HAL_MSP_ENABLED !HAL_MINIMIZE_FEATURES
|
||||
#endif
|
||||
|
||||
#include <AP_HAL/UARTDriver.h>
|
||||
#include <AP_SerialManager/AP_SerialManager.h>
|
||||
|
||||
#include "msp_osd.h"
|
||||
#include "msp_protocol.h"
|
||||
#include "msp_sbuf.h"
|
||||
#include "msp_version.h"
|
||||
|
||||
#if HAL_MSP_ENABLED
|
||||
|
||||
// betaflight/src/main/common/utils.h
|
||||
#define MSP_ARRAYEND(x) (&(x)[ARRAY_SIZE(x)])
|
||||
#define MSP_UNUSED(x) (void)(x)
|
||||
// betaflight/src/main/msp/msp_serial.c
|
||||
#define JUMBO_FRAME_SIZE_LIMIT 255
|
||||
// betaflight/src/main/msp/msp.h
|
||||
#define MSP_V2_FRAME_ID 255
|
||||
#define MSP_VERSION_MAGIC_INITIALIZER { 'M', 'M', 'X' }
|
||||
#define MSP_PORT_INBUF_SIZE 192
|
||||
#define MSP_PORT_OUTBUF_SIZE 512
|
||||
#define MSP_MAX_HEADER_SIZE 9
|
||||
// betaflight/src/main/msp/msp_protocol_v2_sensor.h
|
||||
#define MSP2_IS_SENSOR_MESSAGE(x) ((x) >= 0x1F00U && (x) <= 0x1FFFU)
|
||||
#define MSP2_SENSOR_RANGEFINDER 0x1F01
|
||||
#define MSP2_SENSOR_OPTIC_FLOW 0x1F02
|
||||
|
||||
class AP_MSP_Telem_Backend;
|
||||
|
||||
namespace MSP
|
||||
{
|
||||
typedef enum {
|
||||
MSP_V1 = 0,
|
||||
MSP_V2_OVER_V1 = 1,
|
||||
MSP_V2_NATIVE = 2,
|
||||
MSP_VERSION_COUNT
|
||||
} msp_version_e;
|
||||
|
||||
// return positive for ACK, negative on error, zero for no reply
|
||||
typedef enum {
|
||||
MSP_RESULT_ACK = 1,
|
||||
MSP_RESULT_ERROR = -1,
|
||||
MSP_RESULT_NO_REPLY = 0
|
||||
} MSPCommandResult;
|
||||
|
||||
typedef struct msp_packet_s {
|
||||
sbuf_t buf;
|
||||
int16_t cmd;
|
||||
uint8_t flags;
|
||||
int16_t result;
|
||||
} msp_packet_t;
|
||||
|
||||
typedef enum {
|
||||
MSP_FLAG_DONT_REPLY = (1 << 0),
|
||||
} msp_flags_e;
|
||||
|
||||
typedef enum {
|
||||
MSP_IDLE,
|
||||
MSP_HEADER_START,
|
||||
MSP_HEADER_M,
|
||||
MSP_HEADER_X,
|
||||
|
||||
MSP_HEADER_V1,
|
||||
MSP_PAYLOAD_V1,
|
||||
MSP_CHECKSUM_V1,
|
||||
|
||||
MSP_HEADER_V2_OVER_V1,
|
||||
MSP_PAYLOAD_V2_OVER_V1,
|
||||
MSP_CHECKSUM_V2_OVER_V1,
|
||||
|
||||
MSP_HEADER_V2_NATIVE,
|
||||
MSP_PAYLOAD_V2_NATIVE,
|
||||
MSP_CHECKSUM_V2_NATIVE,
|
||||
|
||||
MSP_COMMAND_RECEIVED
|
||||
} msp_state_e;
|
||||
|
||||
typedef struct PACKED {
|
||||
uint8_t size;
|
||||
uint8_t cmd;
|
||||
} msp_header_v1_t;
|
||||
|
||||
typedef struct PACKED {
|
||||
uint16_t size;
|
||||
} msp_header_jumbo_t;
|
||||
|
||||
typedef struct PACKED {
|
||||
uint8_t flags;
|
||||
uint16_t cmd;
|
||||
uint16_t size;
|
||||
} msp_header_v2_t;
|
||||
|
||||
typedef struct msp_port_s {
|
||||
AP_HAL::UARTDriver *uart;
|
||||
msp_state_e c_state;
|
||||
uint8_t in_buf[MSP_PORT_INBUF_SIZE];
|
||||
uint_fast16_t offset;
|
||||
uint_fast16_t data_size;
|
||||
msp_version_e msp_version;
|
||||
uint8_t cmd_flags;
|
||||
uint16_t cmd_msp;
|
||||
uint8_t checksum1;
|
||||
uint8_t checksum2;
|
||||
} msp_port_t;
|
||||
|
||||
typedef struct PACKED {
|
||||
uint8_t quality; // [0;255]
|
||||
int32_t distance_mm; // Negative value for out of range
|
||||
} msp_rangefinder_sensor_t;
|
||||
|
||||
typedef struct PACKED {
|
||||
uint8_t quality; // [0;255]
|
||||
int32_t motion_x;
|
||||
int32_t motion_y;
|
||||
} msp_opflow_sensor_t;
|
||||
|
||||
// betaflight/src/main/sensors/battery.h
|
||||
typedef enum : uint8_t {
|
||||
MSP_BATTERY_OK = 0,
|
||||
MSP_BATTERY_WARNING,
|
||||
MSP_BATTERY_CRITICAL,
|
||||
MSP_BATTERY_NOT_PRESENT,
|
||||
MSP_BATTERY_INIT
|
||||
} battery_state_e;
|
||||
|
||||
uint8_t msp_serial_checksum_buf(uint8_t checksum, const uint8_t *data, uint32_t len);
|
||||
uint32_t msp_serial_send_frame(msp_port_t *msp, const uint8_t * hdr, uint32_t hdr_len, const uint8_t * data, uint32_t data_len, const uint8_t * crc, uint32_t crc_len);
|
||||
uint32_t msp_serial_encode(msp_port_t *msp, msp_packet_t *packet, msp_version_e msp_version);
|
||||
bool msp_parse_received_data(msp_port_t *msp, uint8_t c);
|
||||
}
|
||||
|
||||
#endif //HAL_MSP_ENABLED
|
||||
|
|
@ -0,0 +1,145 @@
|
|||
#pragma once
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
#if HAL_MSP_ENABLED
|
||||
|
||||
#define OSD_FLAGS_OSD_FEATURE (1U << 0)
|
||||
|
||||
namespace MSP
|
||||
{
|
||||
typedef enum {
|
||||
OSD_RSSI_VALUE,
|
||||
OSD_MAIN_BATT_VOLTAGE,
|
||||
OSD_CROSSHAIRS,
|
||||
OSD_ARTIFICIAL_HORIZON,
|
||||
OSD_HORIZON_SIDEBARS,
|
||||
OSD_ITEM_TIMER_1,
|
||||
OSD_ITEM_TIMER_2,
|
||||
OSD_FLYMODE,
|
||||
OSD_CRAFT_NAME,
|
||||
OSD_THROTTLE_POS,
|
||||
OSD_VTX_CHANNEL,
|
||||
OSD_CURRENT_DRAW,
|
||||
OSD_MAH_DRAWN,
|
||||
OSD_GPS_SPEED,
|
||||
OSD_GPS_SATS,
|
||||
OSD_ALTITUDE,
|
||||
OSD_ROLL_PIDS,
|
||||
OSD_PITCH_PIDS,
|
||||
OSD_YAW_PIDS,
|
||||
OSD_POWER,
|
||||
OSD_PIDRATE_PROFILE,
|
||||
OSD_WARNINGS,
|
||||
OSD_AVG_CELL_VOLTAGE,
|
||||
OSD_GPS_LON,
|
||||
OSD_GPS_LAT,
|
||||
OSD_DEBUG,
|
||||
OSD_PITCH_ANGLE,
|
||||
OSD_ROLL_ANGLE,
|
||||
OSD_MAIN_BATT_USAGE,
|
||||
OSD_DISARMED,
|
||||
OSD_HOME_DIR,
|
||||
OSD_HOME_DIST,
|
||||
OSD_NUMERICAL_HEADING,
|
||||
OSD_NUMERICAL_VARIO,
|
||||
OSD_COMPASS_BAR,
|
||||
OSD_ESC_TMP,
|
||||
OSD_ESC_RPM,
|
||||
OSD_REMAINING_TIME_ESTIMATE,
|
||||
OSD_RTC_DATETIME,
|
||||
OSD_ADJUSTMENT_RANGE,
|
||||
OSD_CORE_TEMPERATURE,
|
||||
OSD_ANTI_GRAVITY,
|
||||
OSD_G_FORCE,
|
||||
OSD_MOTOR_DIAG,
|
||||
OSD_LOG_STATUS,
|
||||
OSD_FLIP_ARROW,
|
||||
OSD_LINK_QUALITY,
|
||||
OSD_FLIGHT_DIST,
|
||||
OSD_STICK_OVERLAY_LEFT,
|
||||
OSD_STICK_OVERLAY_RIGHT,
|
||||
OSD_DISPLAY_NAME,
|
||||
OSD_ESC_RPM_FREQ,
|
||||
OSD_RATE_PROFILE_NAME,
|
||||
OSD_PID_PROFILE_NAME,
|
||||
OSD_PROFILE_NAME,
|
||||
OSD_RSSI_DBM_VALUE,
|
||||
OSD_RC_CHANNELS,
|
||||
OSD_CAMERA_FRAME,
|
||||
OSD_ITEM_COUNT // MUST BE LAST
|
||||
} osd_items_e;
|
||||
|
||||
static_assert(OSD_ITEM_COUNT == 58, "OSD_ITEM_COUNT != 58");
|
||||
|
||||
typedef enum {
|
||||
OSD_STAT_RTC_DATE_TIME,
|
||||
OSD_STAT_TIMER_1,
|
||||
OSD_STAT_TIMER_2,
|
||||
OSD_STAT_MAX_SPEED,
|
||||
OSD_STAT_MAX_DISTANCE,
|
||||
OSD_STAT_MIN_BATTERY,
|
||||
OSD_STAT_END_BATTERY,
|
||||
OSD_STAT_BATTERY,
|
||||
OSD_STAT_MIN_RSSI,
|
||||
OSD_STAT_MAX_CURRENT,
|
||||
OSD_STAT_USED_MAH,
|
||||
OSD_STAT_MAX_ALTITUDE,
|
||||
OSD_STAT_BLACKBOX,
|
||||
OSD_STAT_BLACKBOX_NUMBER,
|
||||
OSD_STAT_MAX_G_FORCE,
|
||||
OSD_STAT_MAX_ESC_TEMP,
|
||||
OSD_STAT_MAX_ESC_RPM,
|
||||
OSD_STAT_MIN_LINK_QUALITY,
|
||||
OSD_STAT_FLIGHT_DISTANCE,
|
||||
OSD_STAT_MAX_FFT,
|
||||
OSD_STAT_TOTAL_FLIGHTS,
|
||||
OSD_STAT_TOTAL_TIME,
|
||||
OSD_STAT_TOTAL_DIST,
|
||||
OSD_STAT_MIN_RSSI_DBM,
|
||||
OSD_STAT_COUNT // MUST BE LAST
|
||||
} osd_stats_e;
|
||||
|
||||
typedef enum : uint8_t {
|
||||
OSD_UNIT_IMPERIAL,
|
||||
OSD_UNIT_METRIC
|
||||
} osd_unit_e;
|
||||
|
||||
typedef enum {
|
||||
OSD_TIMER_1,
|
||||
OSD_TIMER_2,
|
||||
OSD_TIMER_COUNT
|
||||
} osd_timer_e;
|
||||
|
||||
typedef enum {
|
||||
OSD_WARNING_ARMING_DISABLE,
|
||||
OSD_WARNING_BATTERY_NOT_FULL,
|
||||
OSD_WARNING_BATTERY_WARNING,
|
||||
OSD_WARNING_BATTERY_CRITICAL,
|
||||
OSD_WARNING_VISUAL_BEEPER,
|
||||
OSD_WARNING_CRASH_FLIP,
|
||||
OSD_WARNING_ESC_FAIL,
|
||||
OSD_WARNING_CORE_TEMPERATURE,
|
||||
OSD_WARNING_RC_SMOOTHING,
|
||||
OSD_WARNING_FAIL_SAFE,
|
||||
OSD_WARNING_LAUNCH_CONTROL,
|
||||
OSD_WARNING_GPS_RESCUE_UNAVAILABLE,
|
||||
OSD_WARNING_GPS_RESCUE_DISABLED,
|
||||
OSD_WARNING_RSSI,
|
||||
OSD_WARNING_LINK_QUALITY,
|
||||
OSD_WARNING_RSSI_DBM,
|
||||
OSD_WARNING_COUNT // MUST BE LAST
|
||||
} osd_warnings_flags_e;
|
||||
|
||||
typedef struct osd_config_s {
|
||||
osd_unit_e units;
|
||||
uint8_t rssi_alarm;
|
||||
uint16_t cap_alarm;
|
||||
uint16_t alt_alarm;
|
||||
uint16_t timers[OSD_TIMER_COUNT];
|
||||
uint32_t enabled_stats;
|
||||
uint32_t enabled_warnings;
|
||||
} osd_config_t;
|
||||
}
|
||||
|
||||
#endif //HAL_MSP_ENABLED
|
||||
|
|
@ -0,0 +1,344 @@
|
|||
/*
|
||||
* This file is part of Cleanflight and Betaflight.
|
||||
*
|
||||
* Cleanflight and Betaflight are free software. You can redistribute
|
||||
* this software and/or modify this software 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.
|
||||
*
|
||||
* Cleanflight and Betaflight are distributed in the hope that they
|
||||
* 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 software.
|
||||
*
|
||||
* If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
/**
|
||||
* MSP Guidelines, emphasis is used to clarify.
|
||||
*
|
||||
* Each FlightController (FC, Server) MUST change the API version when any MSP command is added, deleted, or changed.
|
||||
*
|
||||
* If you fork the FC source code and release your own version, you MUST change the Flight Controller Identifier.
|
||||
*
|
||||
* NEVER release a modified copy of this code that shares the same Flight controller IDENT and API version
|
||||
* if the API doesn't match EXACTLY.
|
||||
*
|
||||
* Consumers of the API (API clients) SHOULD first attempt to get a response from the MSP_API_VERSION command.
|
||||
* If no response is obtained then client MAY try the legacy MSP_IDENT command.
|
||||
*
|
||||
* API consumers should ALWAYS handle communication failures gracefully and attempt to continue
|
||||
* without the information if possible. Clients MAY log/display a suitable message.
|
||||
*
|
||||
* API clients should NOT attempt any communication if they can't handle the returned API MAJOR VERSION.
|
||||
*
|
||||
* API clients SHOULD attempt communication if the API MINOR VERSION has increased from the time
|
||||
* the API client was written and handle command failures gracefully. Clients MAY disable
|
||||
* functionality that depends on the commands while still leaving other functionality intact.
|
||||
* that the newer API version may cause problems before using API commands that change FC state.
|
||||
*
|
||||
* It is for this reason that each MSP command should be specific as possible, such that changes
|
||||
* to commands break as little functionality as possible.
|
||||
*
|
||||
* API client authors MAY use a compatibility matrix/table when determining if they can support
|
||||
* a given command from a given flight controller at a given api version level.
|
||||
*
|
||||
* Developers MUST NOT create new MSP commands that do more than one thing.
|
||||
*
|
||||
* Failure to follow these guidelines will likely invoke the wrath of developers trying to write tools
|
||||
* that use the API and the users of those tools.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
/* Protocol numbers used both by the wire format, config system, and
|
||||
field setters.
|
||||
*/
|
||||
|
||||
#define MSP_PROTOCOL_VERSION 0
|
||||
|
||||
#define API_VERSION_MAJOR 1
|
||||
#define API_VERSION_MINOR 42 // for compatibility with DJI OSD
|
||||
|
||||
#define API_VERSION_LENGTH 2
|
||||
|
||||
#define MULTIWII_IDENTIFIER "MWII";
|
||||
#define BASEFLIGHT_IDENTIFIER "BAFL";
|
||||
#define BETAFLIGHT_IDENTIFIER "BTFL"
|
||||
#define CLEANFLIGHT_IDENTIFIER "CLFL"
|
||||
#define INAV_IDENTIFIER "INAV"
|
||||
#define RACEFLIGHT_IDENTIFIER "RCFL"
|
||||
#define ARDUPILOT_IDENTIFIER "ARDU"
|
||||
|
||||
#define FLIGHT_CONTROLLER_IDENTIFIER_LENGTH 4
|
||||
#define FLIGHT_CONTROLLER_VERSION_LENGTH 3
|
||||
#define FLIGHT_CONTROLLER_VERSION_MASK 0xFFF
|
||||
|
||||
#define BOARD_IDENTIFIER_LENGTH 4 // 4 UPPER CASE alpha numeric characters that identify the board being used.
|
||||
#define BOARD_HARDWARE_REVISION_LENGTH 2
|
||||
|
||||
// These are baseflight specific flags but they are useless now since MW 2.3 uses the upper 4 bits for the navigation version.
|
||||
#define CAP_PLATFORM_32BIT ((uint32_t)1 << 31)
|
||||
#define CAP_BASEFLIGHT_CONFIG ((uint32_t)1 << 30)
|
||||
|
||||
// MW 2.3 stores NAVI_VERSION in the top 4 bits of the capability mask.
|
||||
#define CAP_NAVI_VERSION_BIT_4_MSB ((uint32_t)1 << 31)
|
||||
#define CAP_NAVI_VERSION_BIT_3 ((uint32_t)1 << 30)
|
||||
#define CAP_NAVI_VERSION_BIT_2 ((uint32_t)1 << 29)
|
||||
#define CAP_NAVI_VERSION_BIT_1_LSB ((uint32_t)1 << 28)
|
||||
|
||||
#define CAP_DYNBALANCE ((uint32_t)1 << 2)
|
||||
#define CAP_FLAPS ((uint32_t)1 << 3)
|
||||
#define CAP_NAVCAP ((uint32_t)1 << 4)
|
||||
#define CAP_EXTAUX ((uint32_t)1 << 5)
|
||||
|
||||
#define MSP_API_VERSION 1 //out message
|
||||
#define MSP_FC_VARIANT 2 //out message
|
||||
#define MSP_FC_VERSION 3 //out message
|
||||
#define MSP_BOARD_INFO 4 //out message
|
||||
#define MSP_BUILD_INFO 5 //out message
|
||||
|
||||
#define MSP_NAME 10 //out message Returns user set board name - betaflight
|
||||
#define MSP_SET_NAME 11 //in message Sets board name - betaflight
|
||||
|
||||
//
|
||||
// MSP commands for Cleanflight original features
|
||||
//
|
||||
#define MSP_BATTERY_CONFIG 32
|
||||
#define MSP_SET_BATTERY_CONFIG 33
|
||||
|
||||
#define MSP_MODE_RANGES 34 //out message Returns all mode ranges
|
||||
#define MSP_SET_MODE_RANGE 35 //in message Sets a single mode range
|
||||
|
||||
#define MSP_FEATURE_CONFIG 36
|
||||
#define MSP_SET_FEATURE_CONFIG 37
|
||||
|
||||
#define MSP_BOARD_ALIGNMENT_CONFIG 38
|
||||
#define MSP_SET_BOARD_ALIGNMENT_CONFIG 39
|
||||
|
||||
#define MSP_CURRENT_METER_CONFIG 40
|
||||
#define MSP_SET_CURRENT_METER_CONFIG 41
|
||||
|
||||
#define MSP_MIXER_CONFIG 42
|
||||
#define MSP_SET_MIXER_CONFIG 43
|
||||
|
||||
#define MSP_RX_CONFIG 44
|
||||
#define MSP_SET_RX_CONFIG 45
|
||||
|
||||
#define MSP_LED_COLORS 46
|
||||
#define MSP_SET_LED_COLORS 47
|
||||
|
||||
#define MSP_LED_STRIP_CONFIG 48
|
||||
#define MSP_SET_LED_STRIP_CONFIG 49
|
||||
|
||||
#define MSP_RSSI_CONFIG 50
|
||||
#define MSP_SET_RSSI_CONFIG 51
|
||||
|
||||
#define MSP_ADJUSTMENT_RANGES 52
|
||||
#define MSP_SET_ADJUSTMENT_RANGE 53
|
||||
|
||||
// private - only to be used by the configurator, the commands are likely to change
|
||||
#define MSP_CF_SERIAL_CONFIG 54
|
||||
#define MSP_SET_CF_SERIAL_CONFIG 55
|
||||
|
||||
#define MSP_VOLTAGE_METER_CONFIG 56
|
||||
#define MSP_SET_VOLTAGE_METER_CONFIG 57
|
||||
|
||||
#define MSP_SONAR_ALTITUDE 58 //out message get sonar altitude [cm]
|
||||
|
||||
#define MSP_PID_CONTROLLER 59
|
||||
#define MSP_SET_PID_CONTROLLER 60
|
||||
|
||||
#define MSP_ARMING_CONFIG 61
|
||||
#define MSP_SET_ARMING_CONFIG 62
|
||||
|
||||
//
|
||||
// Baseflight MSP commands (if enabled they exist in Cleanflight)
|
||||
//
|
||||
#define MSP_RX_MAP 64 //out message get channel map (also returns number of channels total)
|
||||
#define MSP_SET_RX_MAP 65 //in message set rx map, numchannels to set comes from MSP_RX_MAP
|
||||
|
||||
// DEPRECATED - DO NOT USE "MSP_BF_CONFIG" and MSP_SET_BF_CONFIG. In Cleanflight, isolated commands already exist and should be used instead.
|
||||
// DEPRECATED - #define MSP_BF_CONFIG 66 //out message baseflight-specific settings that aren't covered elsewhere
|
||||
// DEPRECATED - #define MSP_SET_BF_CONFIG 67 //in message baseflight-specific settings save
|
||||
|
||||
#define MSP_REBOOT 68 //in message reboot settings
|
||||
|
||||
// Use MSP_BUILD_INFO instead
|
||||
// DEPRECATED - #define MSP_BF_BUILD_INFO 69 //out message build date as well as some space for future expansion
|
||||
|
||||
#define MSP_DATAFLASH_SUMMARY 70 //out message - get description of dataflash chip
|
||||
#define MSP_DATAFLASH_READ 71 //out message - get content of dataflash chip
|
||||
#define MSP_DATAFLASH_ERASE 72 //in message - erase dataflash chip
|
||||
|
||||
// No-longer needed
|
||||
// DEPRECATED - #define MSP_LOOP_TIME 73 //out message Returns FC cycle time i.e looptime parameter // DEPRECATED
|
||||
// DEPRECATED - #define MSP_SET_LOOP_TIME 74 //in message Sets FC cycle time i.e looptime parameter // DEPRECATED
|
||||
|
||||
#define MSP_FAILSAFE_CONFIG 75 //out message Returns FC Fail-Safe settings
|
||||
#define MSP_SET_FAILSAFE_CONFIG 76 //in message Sets FC Fail-Safe settings
|
||||
|
||||
#define MSP_RXFAIL_CONFIG 77 //out message Returns RXFAIL settings
|
||||
#define MSP_SET_RXFAIL_CONFIG 78 //in message Sets RXFAIL settings
|
||||
|
||||
#define MSP_SDCARD_SUMMARY 79 //out message Get the state of the SD card
|
||||
|
||||
#define MSP_BLACKBOX_CONFIG 80 //out message Get blackbox settings
|
||||
#define MSP_SET_BLACKBOX_CONFIG 81 //in message Set blackbox settings
|
||||
|
||||
#define MSP_TRANSPONDER_CONFIG 82 //out message Get transponder settings
|
||||
#define MSP_SET_TRANSPONDER_CONFIG 83 //in message Set transponder settings
|
||||
|
||||
#define MSP_OSD_CONFIG 84 //out message Get osd settings - betaflight
|
||||
#define MSP_SET_OSD_CONFIG 85 //in message Set osd settings - betaflight
|
||||
|
||||
#define MSP_OSD_CHAR_READ 86 //out message Get osd settings - betaflight
|
||||
#define MSP_OSD_CHAR_WRITE 87 //in message Set osd settings - betaflight
|
||||
|
||||
#define MSP_VTX_CONFIG 88 //out message Get vtx settings - betaflight
|
||||
#define MSP_SET_VTX_CONFIG 89 //in message Set vtx settings - betaflight
|
||||
|
||||
// Betaflight Additional Commands
|
||||
#define MSP_ADVANCED_CONFIG 90
|
||||
#define MSP_SET_ADVANCED_CONFIG 91
|
||||
|
||||
#define MSP_FILTER_CONFIG 92
|
||||
#define MSP_SET_FILTER_CONFIG 93
|
||||
|
||||
#define MSP_PID_ADVANCED 94
|
||||
#define MSP_SET_PID_ADVANCED 95
|
||||
|
||||
#define MSP_SENSOR_CONFIG 96
|
||||
#define MSP_SET_SENSOR_CONFIG 97
|
||||
|
||||
#define MSP_CAMERA_CONTROL 98
|
||||
|
||||
#define MSP_SET_ARMING_DISABLED 99
|
||||
|
||||
//
|
||||
// OSD specific
|
||||
//
|
||||
#define MSP_OSD_VIDEO_CONFIG 180
|
||||
#define MSP_SET_OSD_VIDEO_CONFIG 181
|
||||
|
||||
// External OSD displayport mode messages
|
||||
#define MSP_DISPLAYPORT 182
|
||||
|
||||
#define MSP_COPY_PROFILE 183
|
||||
|
||||
#define MSP_BEEPER_CONFIG 184
|
||||
#define MSP_SET_BEEPER_CONFIG 185
|
||||
|
||||
#define MSP_SET_TX_INFO 186 // in message Used to send runtime information from TX lua scripts to the firmware
|
||||
#define MSP_TX_INFO 187 // out message Used by TX lua scripts to read information from the firmware
|
||||
|
||||
//
|
||||
// Multwii original MSP commands
|
||||
//
|
||||
|
||||
// See MSP_API_VERSION and MSP_MIXER_CONFIG
|
||||
//DEPRECATED - #define MSP_IDENT 100 //out message mixerMode + multiwii version + protocol version + capability variable
|
||||
|
||||
|
||||
#define MSP_STATUS 101 //out message cycletime & errors_count & sensor present & box activation & current setting number
|
||||
#define MSP_RAW_IMU 102 //out message 9 DOF
|
||||
#define MSP_SERVO 103 //out message servos
|
||||
#define MSP_MOTOR 104 //out message motors
|
||||
#define MSP_RC 105 //out message rc channels and more
|
||||
#define MSP_RAW_GPS 106 //out message fix, numsat, lat, lon, alt, speed, ground course
|
||||
#define MSP_COMP_GPS 107 //out message distance home, direction home
|
||||
#define MSP_ATTITUDE 108 //out message 2 angles 1 heading
|
||||
#define MSP_ALTITUDE 109 //out message altitude, variometer
|
||||
#define MSP_ANALOG 110 //out message vbat, powermetersum, rssi if available on RX
|
||||
#define MSP_RC_TUNING 111 //out message rc rate, rc expo, rollpitch rate, yaw rate, dyn throttle PID
|
||||
#define MSP_PID 112 //out message P I D coeff (9 are used currently)
|
||||
// Legacy Multiicommand that was never used.
|
||||
//DEPRECATED - #define MSP_BOX 113 //out message BOX setup (number is dependant of your setup)
|
||||
// Legacy command that was under constant change due to the naming vagueness, avoid at all costs - use more specific commands instead.
|
||||
//DEPRECATED - #define MSP_MISC 114 //out message powermeter trig
|
||||
// Legacy Multiicommand that was never used and always wrong
|
||||
//DEPRECATED - #define MSP_MOTOR_PINS 115 //out message which pins are in use for motors & servos, for GUI
|
||||
#define MSP_BOXNAMES 116 //out message the aux switch names
|
||||
#define MSP_PIDNAMES 117 //out message the PID names
|
||||
#define MSP_WP 118 //out message get a WP, WP# is in the payload, returns (WP#, lat, lon, alt, flags) WP#0-home, WP#16-poshold
|
||||
#define MSP_BOXIDS 119 //out message get the permanent IDs associated to BOXes
|
||||
#define MSP_SERVO_CONFIGURATIONS 120 //out message All servo configurations.
|
||||
#define MSP_NAV_STATUS 121 //out message Returns navigation status
|
||||
#define MSP_NAV_CONFIG 122 //out message Returns navigation parameters
|
||||
#define MSP_MOTOR_3D_CONFIG 124 //out message Settings needed for reversible ESCs
|
||||
#define MSP_RC_DEADBAND 125 //out message deadbands for yaw alt pitch roll
|
||||
#define MSP_SENSOR_ALIGNMENT 126 //out message orientation of acc,gyro,mag
|
||||
#define MSP_LED_STRIP_MODECOLOR 127 //out message Get LED strip mode_color settings
|
||||
#define MSP_VOLTAGE_METERS 128 //out message Voltage (per meter)
|
||||
#define MSP_CURRENT_METERS 129 //out message Amperage (per meter)
|
||||
#define MSP_BATTERY_STATE 130 //out message Connected/Disconnected, Voltage, Current Used
|
||||
#define MSP_MOTOR_CONFIG 131 //out message Motor configuration (min/max throttle, etc)
|
||||
#define MSP_GPS_CONFIG 132 //out message GPS configuration
|
||||
#define MSP_COMPASS_CONFIG 133 //out message Compass configuration
|
||||
#define MSP_ESC_SENSOR_DATA 134 //out message Extra ESC data from 32-Bit ESCs (Temperature, RPM)
|
||||
#define MSP_GPS_RESCUE 135 //out message GPS Rescues's angle, initialAltitude, descentDistance, rescueGroundSpeed, sanityChecks and minSats
|
||||
#define MSP_GPS_RESCUE_PIDS 136 //out message GPS Rescues's throttleP and velocity PIDS + yaw P
|
||||
#define MSP_VTXTABLE_BAND 137 //out message vtxTable band/channel data
|
||||
#define MSP_VTXTABLE_POWERLEVEL 138 //out message vtxTable powerLevel data
|
||||
#define MSP_MOTOR_TELEMETRY 139 //out message Per-motor telemetry data (RPM, packet stats, ESC temp, etc.)
|
||||
|
||||
#define MSP_SET_RAW_RC 200 //in message 8 rc chan
|
||||
#define MSP_SET_RAW_GPS 201 //in message fix, numsat, lat, lon, alt, speed
|
||||
#define MSP_SET_PID 202 //in message P I D coeff (9 are used currently)
|
||||
// Legacy multiiwii command that was never used.
|
||||
//DEPRECATED - #define MSP_SET_BOX 203 //in message BOX setup (number is dependant of your setup)
|
||||
#define MSP_SET_RC_TUNING 204 //in message rc rate, rc expo, rollpitch rate, yaw rate, dyn throttle PID, yaw expo
|
||||
#define MSP_ACC_CALIBRATION 205 //in message no param
|
||||
#define MSP_MAG_CALIBRATION 206 //in message no param
|
||||
// Legacy command that was under constant change due to the naming vagueness, avoid at all costs - use more specific commands instead.
|
||||
//DEPRECATED - #define MSP_SET_MISC 207 //in message powermeter trig + 8 free for future use
|
||||
#define MSP_RESET_CONF 208 //in message no param
|
||||
#define MSP_SET_WP 209 //in message sets a given WP (WP#,lat, lon, alt, flags)
|
||||
#define MSP_SELECT_SETTING 210 //in message Select Setting Number (0-2)
|
||||
#define MSP_SET_HEADING 211 //in message define a new heading hold direction
|
||||
#define MSP_SET_SERVO_CONFIGURATION 212 //in message Servo settings
|
||||
#define MSP_SET_MOTOR 214 //in message PropBalance function
|
||||
#define MSP_SET_NAV_CONFIG 215 //in message Sets nav config parameters - write to the eeprom
|
||||
#define MSP_SET_MOTOR_3D_CONFIG 217 //in message Settings needed for reversible ESCs
|
||||
#define MSP_SET_RC_DEADBAND 218 //in message deadbands for yaw alt pitch roll
|
||||
#define MSP_SET_RESET_CURR_PID 219 //in message resetting the current pid profile to defaults
|
||||
#define MSP_SET_SENSOR_ALIGNMENT 220 //in message set the orientation of the acc,gyro,mag
|
||||
#define MSP_SET_LED_STRIP_MODECOLOR 221 //in message Set LED strip mode_color settings
|
||||
#define MSP_SET_MOTOR_CONFIG 222 //out message Motor configuration (min/max throttle, etc)
|
||||
#define MSP_SET_GPS_CONFIG 223 //out message GPS configuration
|
||||
#define MSP_SET_COMPASS_CONFIG 224 //out message Compass configuration
|
||||
#define MSP_SET_GPS_RESCUE 225 //in message GPS Rescues's angle, initialAltitude, descentDistance, rescueGroundSpeed, sanityChecks and minSats
|
||||
#define MSP_SET_GPS_RESCUE_PIDS 226 //in message GPS Rescues's throttleP and velocity PIDS + yaw P
|
||||
#define MSP_SET_VTXTABLE_BAND 227 //in message set vtxTable band/channel data (one band at a time)
|
||||
#define MSP_SET_VTXTABLE_POWERLEVEL 228 //in message set vtxTable powerLevel data (one powerLevel at a time)
|
||||
|
||||
// #define MSP_BIND 240 //in message no param
|
||||
// #define MSP_ALARMS 242
|
||||
|
||||
#define MSP_EEPROM_WRITE 250 //in message no param
|
||||
#define MSP_RESERVE_1 251 //reserved for system usage
|
||||
#define MSP_RESERVE_2 252 //reserved for system usage
|
||||
#define MSP_DEBUGMSG 253 //out message debug string buffer
|
||||
#define MSP_DEBUG 254 //out message debug1,debug2,debug3,debug4
|
||||
#define MSP_V2_FRAME 255 //MSPv2 payload indicator
|
||||
|
||||
// Additional commands that are not compatible with MultiWii
|
||||
#define MSP_STATUS_EX 150 //out message cycletime, errors_count, CPU load, sensor present etc
|
||||
#define MSP_UID 160 //out message Unique device ID
|
||||
#define MSP_GPSSVINFO 164 //out message get Signal Strength (only U-Blox)
|
||||
#define MSP_GPSSTATISTICS 166 //out message get GPS debugging data
|
||||
#define MSP_MULTIPLE_MSP 230 //out message request multiple MSPs in one request - limit is the TX buffer; returns each MSP in the order they were requested starting with length of MSP; MSPs with input arguments are not supported
|
||||
#define MSP_MODE_RANGES_EXTRA 238 //out message Reads the extra mode range data
|
||||
#define MSP_ACC_TRIM 240 //out message get acc angle trim values
|
||||
#define MSP_SET_ACC_TRIM 239 //in message set acc angle trim values
|
||||
#define MSP_SERVO_MIX_RULES 241 //out message Returns servo mixer configuration
|
||||
#define MSP_SET_SERVO_MIX_RULE 242 //in message Sets servo mixer configuration
|
||||
#define MSP_SET_PASSTHROUGH 245 //in message Sets up passthrough to different peripherals (4way interface, uart, etc...)
|
||||
#define MSP_SET_RTC 246 //in message Sets the RTC clock
|
||||
#define MSP_RTC 247 //out message Gets the RTC clock
|
||||
#define MSP_SET_BOARD_INFO 248 //in message Sets the board information for this board
|
||||
#define MSP_SET_SIGNATURE 249 //in message Sets the signature of the board and serial number
|
||||
|
|
@ -0,0 +1,71 @@
|
|||
#include <AP_HAL/utility/sparse-endian.h>
|
||||
#include <AP_InternalError/AP_InternalError.h>
|
||||
|
||||
#include "msp.h"
|
||||
#include "msp_sbuf.h"
|
||||
|
||||
#include <string.h>
|
||||
|
||||
#if HAL_MSP_ENABLED
|
||||
|
||||
uint8_t* MSP::sbuf_ptr(sbuf_t *buf)
|
||||
{
|
||||
return buf->ptr;
|
||||
}
|
||||
|
||||
uint16_t MSP::sbuf_bytes_remaining(const sbuf_t *buf)
|
||||
{
|
||||
return buf->end - buf->ptr;
|
||||
}
|
||||
|
||||
bool MSP::sbuf_check_bounds(const sbuf_t *buf, const uint8_t len)
|
||||
{
|
||||
if (sbuf_bytes_remaining(buf) < len) {
|
||||
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
void MSP::sbuf_switch_to_reader(sbuf_t *buf, uint8_t *base)
|
||||
{
|
||||
buf->end = buf->ptr;
|
||||
buf->ptr = base;
|
||||
}
|
||||
|
||||
void MSP::sbuf_write_u8(sbuf_t *dst, uint8_t val)
|
||||
{
|
||||
if (!sbuf_check_bounds(dst, 1)) {
|
||||
return;
|
||||
}
|
||||
*dst->ptr++ = val;
|
||||
}
|
||||
|
||||
void MSP::sbuf_write_u16(sbuf_t *dst, uint16_t val)
|
||||
{
|
||||
if (!sbuf_check_bounds(dst, 2)) {
|
||||
return;
|
||||
}
|
||||
put_le16_ptr(dst->ptr, val);
|
||||
dst->ptr += 2;
|
||||
}
|
||||
|
||||
void MSP::sbuf_write_u32(sbuf_t *dst, uint32_t val)
|
||||
{
|
||||
if (!sbuf_check_bounds(dst, 4)) {
|
||||
return;
|
||||
}
|
||||
put_le32_ptr(dst->ptr, val);
|
||||
dst->ptr += 4;
|
||||
}
|
||||
|
||||
void MSP::sbuf_write_data(sbuf_t *dst, const void *data, int len)
|
||||
{
|
||||
if (!sbuf_check_bounds(dst, len)) {
|
||||
return;
|
||||
}
|
||||
memcpy(dst->ptr, data, len);
|
||||
dst->ptr += len;
|
||||
}
|
||||
|
||||
#endif //HAL_MSP_ENABLED
|
||||
|
|
@ -0,0 +1,28 @@
|
|||
/*
|
||||
Code ported and adapted from betaflight/src/main/common/streambuf.h
|
||||
*/
|
||||
#pragma once
|
||||
#include "msp.h"
|
||||
#include <stdint.h>
|
||||
|
||||
#if HAL_MSP_ENABLED
|
||||
|
||||
namespace MSP
|
||||
{
|
||||
typedef struct sbuf_s {
|
||||
uint8_t *ptr; // data pointer must be first (sbuff_t* is equivalent to uint8_t **)
|
||||
uint8_t *end;
|
||||
} sbuf_t;
|
||||
|
||||
//helper functions
|
||||
uint8_t* sbuf_ptr(sbuf_t *buf);
|
||||
uint16_t sbuf_bytes_remaining(const sbuf_t *buf);
|
||||
bool sbuf_check_bounds(const sbuf_t *buf, const uint8_t len);
|
||||
void sbuf_switch_to_reader(sbuf_t *buf, uint8_t *base);
|
||||
void sbuf_write_u8(sbuf_t *dst, uint8_t val);
|
||||
void sbuf_write_u16(sbuf_t *dst, uint16_t val);
|
||||
void sbuf_write_u32(sbuf_t *dst, uint32_t val);
|
||||
void sbuf_write_data(sbuf_t *dst, const void *data, int len);
|
||||
}
|
||||
|
||||
#endif //HAL_MSP_ENABLED
|
||||
|
|
@ -0,0 +1,14 @@
|
|||
#pragma once
|
||||
|
||||
#if HAL_MSP_ENABLED
|
||||
|
||||
// use betaflight 4.2.0
|
||||
#define GIT_SHORT_REVISION_LENGTH 8
|
||||
#define BUILD_DATE_LENGTH 11
|
||||
#define BUILD_TIME_LENGTH 8
|
||||
|
||||
#define FC_VERSION_MAJOR 4
|
||||
#define FC_VERSION_MINOR 2
|
||||
#define FC_VERSION_PATCH_LEVEL 0
|
||||
|
||||
#endif //HAL_MSP_ENABLED
|
||||
Loading…
Reference in New Issue