You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
244 lines
10 KiB
244 lines
10 KiB
/* |
|
This program is free software: you can redistribute it and/or modify |
|
it under the terms of the GNU General Public License as published by |
|
the Free Software Foundation, either version 3 of the License, or |
|
(at your option) any later version. |
|
|
|
This program is distributed in the hope that it will be useful, |
|
but WITHOUT ANY WARRANTY; without even the implied warranty of |
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
|
GNU General Public License for more details. |
|
|
|
You should have received a copy of the GNU General Public License |
|
along with this program. If not, see <http://www.gnu.org/licenses/>. |
|
*/ |
|
|
|
#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 |
|
// @Bitmask: 0:EnableTelemetryMode, 1: DisableDJIWorkarounds, 2:EnableBTFLFonts |
|
// @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); |
|
#if HAL_WITH_MSP_DISPLAYPORT |
|
} else if (protocol == AP_SerialManager::SerialProtocol_MSP_DisplayPort) { |
|
_backends[backend_idx] = new 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
|
|
|