diff --git a/libraries/AP_MSP/AP_MSP.cpp b/libraries/AP_MSP/AP_MSP.cpp new file mode 100644 index 0000000000..c85ae080ad --- /dev/null +++ b/libraries/AP_MSP/AP_MSP.cpp @@ -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 . +*/ + +#include +#include + +#include "AP_MSP.h" +#include "AP_MSP_Telem_Generic.h" +#include "AP_MSP_Telem_DJI.h" + +#include +#include + +#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 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 \ No newline at end of file diff --git a/libraries/AP_MSP/AP_MSP.h b/libraries/AP_MSP/AP_MSP.h new file mode 100644 index 0000000000..e405001f2f --- /dev/null +++ b/libraries/AP_MSP/AP_MSP.h @@ -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 . +*/ +/* + MSP version 1 and 2 protocol library, based on betaflight/iNav implementations + code by Alex Apostoli + */ + +#pragma once + +#include + +#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 \ No newline at end of file diff --git a/libraries/AP_MSP/AP_MSP_Telem_Backend.cpp b/libraries/AP_MSP/AP_MSP_Telem_Backend.cpp new file mode 100644 index 0000000000..1515aecf79 --- /dev/null +++ b/libraries/AP_MSP/AP_MSP_Telem_Backend.cpp @@ -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 . +*/ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "AP_MSP.h" +#include "AP_MSP_Telem_Backend.h" + +#include +#include + +#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 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 \ No newline at end of file diff --git a/libraries/AP_MSP/AP_MSP_Telem_Backend.h b/libraries/AP_MSP/AP_MSP_Telem_Backend.h new file mode 100644 index 0000000000..10b2953c90 --- /dev/null +++ b/libraries/AP_MSP/AP_MSP_Telem_Backend.h @@ -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 . + + MSP telemetry library backend base class +*/ +#pragma once + +#include + +#include "msp.h" + +#include + +#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 \ No newline at end of file diff --git a/libraries/AP_MSP/AP_MSP_Telem_DJI.cpp b/libraries/AP_MSP/AP_MSP_Telem_DJI.cpp new file mode 100644 index 0000000000..686569212c --- /dev/null +++ b/libraries/AP_MSP/AP_MSP_Telem_DJI.cpp @@ -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 . +*/ + +#include +#include + +#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 \ No newline at end of file diff --git a/libraries/AP_MSP/AP_MSP_Telem_DJI.h b/libraries/AP_MSP/AP_MSP_Telem_DJI.h new file mode 100644 index 0000000000..78362d4107 --- /dev/null +++ b/libraries/AP_MSP/AP_MSP_Telem_DJI.h @@ -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 . + + 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 \ No newline at end of file diff --git a/libraries/AP_MSP/AP_MSP_Telem_Generic.cpp b/libraries/AP_MSP/AP_MSP_Telem_Generic.cpp new file mode 100644 index 0000000000..5b11ad291a --- /dev/null +++ b/libraries/AP_MSP/AP_MSP_Telem_Generic.cpp @@ -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 . +*/ + +#include +#include + +#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 \ No newline at end of file diff --git a/libraries/AP_MSP/AP_MSP_Telem_Generic.h b/libraries/AP_MSP/AP_MSP_Telem_Generic.h new file mode 100644 index 0000000000..44696ff663 --- /dev/null +++ b/libraries/AP_MSP/AP_MSP_Telem_Generic.h @@ -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 . + + 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 \ No newline at end of file diff --git a/libraries/AP_MSP/msp.cpp b/libraries/AP_MSP/msp.cpp new file mode 100644 index 0000000000..800fda9838 --- /dev/null +++ b/libraries/AP_MSP/msp.cpp @@ -0,0 +1,313 @@ +#include +#include + +#include "msp.h" + +#include + +#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(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 \ No newline at end of file diff --git a/libraries/AP_MSP/msp.h b/libraries/AP_MSP/msp.h new file mode 100644 index 0000000000..a082f9b13a --- /dev/null +++ b/libraries/AP_MSP/msp.h @@ -0,0 +1,137 @@ +#pragma once + +#ifndef HAL_MSP_ENABLED +#define HAL_MSP_ENABLED !HAL_MINIMIZE_FEATURES +#endif + +#include +#include + +#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 \ No newline at end of file diff --git a/libraries/AP_MSP/msp_osd.h b/libraries/AP_MSP/msp_osd.h new file mode 100644 index 0000000000..3c5d14d53c --- /dev/null +++ b/libraries/AP_MSP/msp_osd.h @@ -0,0 +1,145 @@ +#pragma once + +#include + +#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 \ No newline at end of file diff --git a/libraries/AP_MSP/msp_protocol.h b/libraries/AP_MSP/msp_protocol.h new file mode 100644 index 0000000000..a0380ac2a5 --- /dev/null +++ b/libraries/AP_MSP/msp_protocol.h @@ -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 . + */ + +/** + * 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 diff --git a/libraries/AP_MSP/msp_sbuf.cpp b/libraries/AP_MSP/msp_sbuf.cpp new file mode 100644 index 0000000000..3eea5e31ca --- /dev/null +++ b/libraries/AP_MSP/msp_sbuf.cpp @@ -0,0 +1,71 @@ +#include +#include + +#include "msp.h" +#include "msp_sbuf.h" + +#include + +#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 \ No newline at end of file diff --git a/libraries/AP_MSP/msp_sbuf.h b/libraries/AP_MSP/msp_sbuf.h new file mode 100644 index 0000000000..6903d5f920 --- /dev/null +++ b/libraries/AP_MSP/msp_sbuf.h @@ -0,0 +1,28 @@ +/* + Code ported and adapted from betaflight/src/main/common/streambuf.h + */ +#pragma once +#include "msp.h" +#include + +#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 \ No newline at end of file diff --git a/libraries/AP_MSP/msp_version.h b/libraries/AP_MSP/msp_version.h new file mode 100644 index 0000000000..bdb7332a77 --- /dev/null +++ b/libraries/AP_MSP/msp_version.h @@ -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 \ No newline at end of file