mirror of
https://github.com/ArduPilot/ardupilot.git
synced 2026-10-06 19:00:27 +08:00
Andy Piper
165bbf9ca2
Implement betaflight-style VTX control over MSP so an MSP VTX (or an OSD acting as one) can be configured from ArduPilot: - reply to MSP_VTX_CONFIG and push config as MSP v2 native - apply frequency changes from MSP_SET_VTX_CONFIG - learn the power table from MSP_SET_VTXTABLE_POWERLEVEL and decode it as dBm - report the configured pitmode in MSP_VTX_CONFIG - report the FC variant matching the active OSD symbol set - option to hold the VTX at high power while disarmed Process outgoing packets so config can be pushed unsolicited, and guard the DisplayPort scheduler when VideoTX is disabled. The whole path is gated on AP_MSP_VIDEOTX_ENABLED.
245 lines
10 KiB
C++
245 lines
10 KiB
C++
/*
|
|
This program is free software: you can redistribute it and/or modify
|
|
it under the terms of the GNU General Public License as published by
|
|
the Free Software Foundation, either version 3 of the License, or
|
|
(at your option) any later version.
|
|
|
|
This program is distributed in the hope that it will be useful,
|
|
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
GNU General Public License for more details.
|
|
|
|
You should have received a copy of the GNU General Public License
|
|
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
|
*/
|
|
|
|
#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 "AP_MSP_Telem_DisplayPort.h"
|
|
|
|
#include <AP_Notify/AP_Notify.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;
|
|
|
|
using namespace MSP;
|
|
|
|
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:1,2:2,3:3,4:4,5:5,6:6,7:7,8:8,9:9,10:10,11:11,12:12,13:13,14:14
|
|
// @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: EnableTelemetryMode-allows "push" mode telemetry when only rx line of OSD ic connected to autopilot, EnableBTFLFonts-uses indexes corresponding to Betaflight fonts if OSD uses those instead of ArduPilot fonts. EnableINAVFonts uses INAV fonts and overrides EnableBTFLFonts if that option is enabled. ForceVTXHighPower always reports the vehicle as armed so a VTX that drops to low power when disarmed stays at full power.
|
|
// @Bitmask: 0:EnableTelemetryMode, 1: unused, 2:EnableBTFLFonts, 3:EnableINAVFonts, 4:ForceVTXHighPower
|
|
// @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_NOTHROW AP_MSP_Telem_Generic(uart);
|
|
} else if (protocol == AP_SerialManager::SerialProtocol_DJI_FPV) {
|
|
_backends[backend_idx] = NEW_NOTHROW AP_MSP_Telem_DJI(uart);
|
|
#if HAL_WITH_MSP_DISPLAYPORT
|
|
} else if (protocol == AP_SerialManager::SerialProtocol_MSP_DisplayPort) {
|
|
_backends[backend_idx] = NEW_NOTHROW AP_MSP_Telem_DisplayPort(uart);
|
|
#endif
|
|
} 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();
|
|
AP_HAL::UARTDriver *uart = nullptr;
|
|
uint8_t backends_using_msp_thread = 0;
|
|
static const AP_SerialManager::SerialProtocol msp_protocols[] {
|
|
AP_SerialManager::SerialProtocol_DJI_FPV,
|
|
AP_SerialManager::SerialProtocol_MSP,
|
|
#if HAL_WITH_MSP_DISPLAYPORT
|
|
AP_SerialManager::SerialProtocol_MSP_DisplayPort,
|
|
#endif
|
|
};
|
|
|
|
for (const auto msp_protocol: msp_protocols) {
|
|
for (uint8_t protocol_instance=0; protocol_instance<MSP_MAX_INSTANCES-_msp_status.backend_count; protocol_instance++) {
|
|
uart = serial_manager.find_serial(msp_protocol, protocol_instance);
|
|
if (uart != nullptr) {
|
|
if (!init_backend(_msp_status.backend_count, uart, msp_protocol)) {
|
|
break;
|
|
}
|
|
if (_backends[_msp_status.backend_count]->use_msp_thread()) {
|
|
backends_using_msp_thread++;
|
|
}
|
|
_msp_status.backend_count++;
|
|
}
|
|
}
|
|
}
|
|
|
|
if (backends_using_msp_thread > 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::update_osd_item_settings()
|
|
{
|
|
#if OSD_ENABLED
|
|
AP_OSD* osd = AP::osd();
|
|
|
|
if (osd == nullptr) {
|
|
return;
|
|
}
|
|
_osd_item_settings[OSD_RSSI_VALUE] = &osd->screen[_msp_status.current_screen].rssi; // OSDn_RSSI
|
|
_osd_item_settings[OSD_MAIN_BATT_VOLTAGE] = &osd->screen[_msp_status.current_screen].bat_volt; // OSDn_BAT_VOLT
|
|
_osd_item_settings[OSD_CROSSHAIRS] = &osd->screen[_msp_status.current_screen].crosshair; // OSDn_CRSSHAIR
|
|
_osd_item_settings[OSD_ARTIFICIAL_HORIZON] = &osd->screen[_msp_status.current_screen].horizon; // OSDn_HORIZON
|
|
_osd_item_settings[OSD_HORIZON_SIDEBARS] = &osd->screen[_msp_status.current_screen].sidebars; // OSDn_SIDEBARS
|
|
_osd_item_settings[OSD_CRAFT_NAME] = &osd->screen[_msp_status.current_screen].message; // OSDn_MESSAGE
|
|
_osd_item_settings[OSD_FLYMODE] = &osd->screen[_msp_status.current_screen].fltmode; // OSDn_FLTMODE
|
|
_osd_item_settings[OSD_CURRENT_DRAW] = &osd->screen[_msp_status.current_screen].current; // OSDn_CURRENT
|
|
_osd_item_settings[OSD_MAH_DRAWN] = &osd->screen[_msp_status.current_screen].batused; // OSDn_BATUSED
|
|
_osd_item_settings[OSD_GPS_SPEED] = &osd->screen[_msp_status.current_screen].gspeed; // OSDn_GSPEED
|
|
_osd_item_settings[OSD_GPS_SATS] = &osd->screen[_msp_status.current_screen].sats; // OSDn_SATS
|
|
_osd_item_settings[OSD_ALTITUDE] = &osd->screen[_msp_status.current_screen].altitude; // OSDn_ALTITUDE
|
|
_osd_item_settings[OSD_POWER] = &osd->screen[_msp_status.current_screen].power; // OSDn_POWER
|
|
_osd_item_settings[OSD_AVG_CELL_VOLTAGE] = &osd->screen[_msp_status.current_screen].cell_volt; // OSDn_CELLVOLT
|
|
_osd_item_settings[OSD_GPS_LON] = &osd->screen[_msp_status.current_screen].gps_longitude; // OSDn_GPSLONG
|
|
_osd_item_settings[OSD_GPS_LAT] = &osd->screen[_msp_status.current_screen].gps_latitude; // OSDn_GPSLAT
|
|
_osd_item_settings[OSD_PITCH_ANGLE] = &osd->screen[_msp_status.current_screen].pitch_angle; // OSDn_PITCH
|
|
_osd_item_settings[OSD_ROLL_ANGLE] = &osd->screen[_msp_status.current_screen].roll_angle; // OSDn_ROLL
|
|
_osd_item_settings[OSD_MAIN_BATT_USAGE] = &osd->screen[_msp_status.current_screen].batt_bar; // OSDn_BATBAR
|
|
_osd_item_settings[OSD_DISARMED] = &osd->screen[_msp_status.current_screen].arming; // OSDn_ARMING
|
|
_osd_item_settings[OSD_HOME_DIR] = &osd->screen[_msp_status.current_screen].home_dir; // OSDn_HOMEDIR
|
|
_osd_item_settings[OSD_HOME_DIST] = &osd->screen[_msp_status.current_screen].home_dist; // OSDn_HOMEDIST
|
|
_osd_item_settings[OSD_NUMERICAL_HEADING] = &osd->screen[_msp_status.current_screen].heading; // OSDn_HEADING
|
|
_osd_item_settings[OSD_NUMERICAL_VARIO] = &osd->screen[_msp_status.current_screen].vspeed; // OSDn_VSPEED
|
|
#if HAL_WITH_ESC_TELEM
|
|
_osd_item_settings[OSD_ESC_TMP] = &osd->screen[_msp_status.current_screen].esc_temp; // OSDn_ESCTEMP
|
|
#endif
|
|
_osd_item_settings[OSD_RTC_DATETIME] = &osd->screen[_msp_status.current_screen].clk; // OSDn_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
|
|
// note: we do not access a uart for a backend handled by another thread
|
|
if (_backends[i] != nullptr && _backends[i]->use_msp_thread()) {
|
|
_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 and every 2 seconds
|
|
if ((uint32_t(now * 0.00143) & 0x01) != _msp_status.flashing_on) {
|
|
_msp_status.flashing_on = !_msp_status.flashing_on;
|
|
}
|
|
if ((uint32_t(now * 0.0005) & 0x01) != _msp_status.slow_flashing_on) {
|
|
_msp_status.slow_flashing_on = !_msp_status.slow_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;
|
|
}
|
|
|
|
#if OSD_ENABLED
|
|
// check if we had a screen change
|
|
AP_OSD* osd = AP::osd();
|
|
|
|
if (osd != nullptr) {
|
|
const uint8_t screen = osd->is_readonly_screen() ? osd->get_current_screen() : _msp_status.current_screen;
|
|
if (_msp_status.current_screen != screen || !_msp_status.osd_initialized) {
|
|
_msp_status.current_screen = screen;
|
|
update_osd_item_settings();
|
|
}
|
|
}
|
|
#endif // OSD_ENABLED
|
|
|
|
for (uint8_t i=0; i< _msp_status.backend_count; i++) {
|
|
// note: we do not access a uart for a backend handled by another thread
|
|
if (_backends[i] != nullptr && _backends[i]->use_msp_thread()) {
|
|
// 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();
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
AP_MSP_Telem_Backend* AP_MSP::find_protocol(const AP_SerialManager::SerialProtocol protocol) const {
|
|
for (uint8_t i=0; i< _msp_status.backend_count; i++) {
|
|
if (_backends[i] != nullptr && _backends[i]->get_serial_protocol() == protocol) {
|
|
return _backends[i];
|
|
}
|
|
}
|
|
return nullptr;
|
|
}
|
|
|
|
bool AP_MSP::is_option_enabled(Option option) const
|
|
{
|
|
return (_options & (uint8_t)option) != 0;
|
|
}
|
|
|
|
namespace AP
|
|
{
|
|
AP_MSP *msp()
|
|
{
|
|
return AP_MSP::get_singleton();
|
|
}
|
|
};
|
|
|
|
#endif //HAL_MSP_ENABLED
|