Line data    Source code

       1             : /*
       2             :    This program is free software: you can redistribute it and/or modify
       3             :    it under the terms of the GNU General Public License as published by
       4             :    the Free Software Foundation, either version 3 of the License, or
       5             :    (at your option) any later version.
       6             : 
       7             :    This program is distributed in the hope that it will be useful,
       8             :    but WITHOUT ANY WARRANTY; without even the implied warranty of
       9             :    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
      10             :    GNU General Public License for more details.
      11             : 
      12             :    You should have received a copy of the GNU General Public License
      13             :    along with this program.  If not, see <http://www.gnu.org/licenses/>.
      14             : */
      15             : 
      16             : #include <AP_AHRS/AP_AHRS.h>
      17             : #include <AP_RSSI/AP_RSSI.h>
      18             : 
      19             : #include "AP_MSP.h"
      20             : #include "AP_MSP_Telem_Generic.h"
      21             : #include "AP_MSP_Telem_DJI.h"
      22             : #include "AP_MSP_Telem_DisplayPort.h"
      23             : 
      24             : #include <AP_Notify/AP_Notify.h>
      25             : 
      26             : #include <ctype.h>
      27             : #include <stdio.h>
      28             : 
      29             : #if HAL_MSP_ENABLED
      30             : 
      31             : const uint16_t  OSD_FLIGHT_MODE_FOCUS_TIME = 2000;
      32             : extern const AP_HAL::HAL& hal;
      33             : 
      34             : using namespace MSP;
      35             : 
      36             : const AP_Param::GroupInfo AP_MSP::var_info[] = {
      37             : 
      38             :     // @Param: _OSD_NCELLS
      39             :     // @DisplayName: Cell count override
      40             :     // @Description: Used for average cell voltage calculation
      41             :     // @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
      42             :     // @User: Standard
      43             :     AP_GROUPINFO("_OSD_NCELLS", 1, AP_MSP, _cellcount, 0),
      44             : 
      45             :     // @Param: _OPTIONS
      46             :     // @DisplayName: MSP OSD Options
      47             :     // @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.
      48             :     // @Bitmask: 0:EnableTelemetryMode, 1: unused, 2:EnableBTFLFonts, 3:EnableINAVFonts, 4:ForceVTXHighPower
      49             :     // @User: Standard
      50             :     AP_GROUPINFO("_OPTIONS", 2, AP_MSP, _options, 0),
      51             : 
      52             :     AP_GROUPEND
      53             : };
      54             : 
      55             : AP_MSP *AP_MSP::_singleton;
      56             : 
      57        2098 : AP_MSP::AP_MSP()
      58             : {
      59        2098 :     _singleton = this;
      60        2098 :     AP_Param::setup_object_defaults(this, var_info);
      61        2098 : }
      62             : 
      63           6 : bool AP_MSP::init_backend(uint8_t backend_idx, AP_HAL::UARTDriver *uart, AP_SerialManager::SerialProtocol protocol)
      64             : {
      65           6 :     if (protocol == AP_SerialManager::SerialProtocol_MSP) {
      66           3 :         _backends[backend_idx] = NEW_NOTHROW AP_MSP_Telem_Generic(uart);
      67           3 :     } else if (protocol == AP_SerialManager::SerialProtocol_DJI_FPV) {
      68           1 :         _backends[backend_idx] = NEW_NOTHROW AP_MSP_Telem_DJI(uart);
      69             : #if HAL_WITH_MSP_DISPLAYPORT
      70           2 :     } else if (protocol == AP_SerialManager::SerialProtocol_MSP_DisplayPort) {
      71           2 :         _backends[backend_idx] = NEW_NOTHROW AP_MSP_Telem_DisplayPort(uart);
      72             : #endif
      73             :     } else {
      74           0 :         return false;
      75             :     }
      76           6 :     if (_backends[backend_idx] != nullptr) {
      77           6 :         _backends[backend_idx]->init();
      78           6 :         return true;
      79             :     }
      80           0 :     return false;
      81             : }
      82             : 
      83             : /*
      84             :  * init - perform required initialisation
      85             :  */
      86        2094 : void AP_MSP::init()
      87             : {
      88        2094 :     const AP_SerialManager &serial_manager = AP::serialmanager();
      89        2094 :     AP_HAL::UARTDriver *uart = nullptr;
      90        2094 :     uint8_t backends_using_msp_thread = 0;
      91             :     static const AP_SerialManager::SerialProtocol msp_protocols[] {
      92             :         AP_SerialManager::SerialProtocol_DJI_FPV,
      93             :         AP_SerialManager::SerialProtocol_MSP,
      94             : #if HAL_WITH_MSP_DISPLAYPORT
      95             :         AP_SerialManager::SerialProtocol_MSP_DisplayPort,
      96             : #endif
      97             :     };
      98             : 
      99        8376 :     for (const auto msp_protocol: msp_protocols) {
     100       25117 :         for (uint8_t protocol_instance=0; protocol_instance<MSP_MAX_INSTANCES-_msp_status.backend_count; protocol_instance++) {
     101       18835 :             uart = serial_manager.find_serial(msp_protocol, protocol_instance);
     102       18835 :             if (uart != nullptr) {
     103           6 :                 if (!init_backend(_msp_status.backend_count, uart, msp_protocol)) {
     104           0 :                     break;
     105             :                 }
     106           6 :                 if (_backends[_msp_status.backend_count]->use_msp_thread()) {
     107           4 :                     backends_using_msp_thread++;
     108             :                 }
     109           6 :                 _msp_status.backend_count++;
     110             :             }
     111             :         }
     112             :     }
     113             : 
     114        2094 :     if (backends_using_msp_thread > 0) {
     115             :         // we've found at least 1 msp backend, start protocol handler
     116           4 :         if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_MSP::loop, void),
     117             :                                           "MSP",
     118             :                                           1024, AP_HAL::Scheduler::PRIORITY_IO, 1)) {
     119           0 :             return;
     120             :         }
     121             :     }
     122             : }
     123             : 
     124           4 : void AP_MSP::update_osd_item_settings()
     125             : {
     126             : #if OSD_ENABLED
     127           4 :     AP_OSD* osd = AP::osd();
     128             : 
     129           4 :     if (osd == nullptr) {
     130           0 :         return;
     131             :     }
     132           4 :     _osd_item_settings[OSD_RSSI_VALUE] = &osd->screen[_msp_status.current_screen].rssi;             // OSDn_RSSI
     133           4 :     _osd_item_settings[OSD_MAIN_BATT_VOLTAGE] = &osd->screen[_msp_status.current_screen].bat_volt;  // OSDn_BAT_VOLT
     134           4 :     _osd_item_settings[OSD_CROSSHAIRS] = &osd->screen[_msp_status.current_screen].crosshair;        // OSDn_CRSSHAIR
     135           4 :     _osd_item_settings[OSD_ARTIFICIAL_HORIZON] = &osd->screen[_msp_status.current_screen].horizon;  // OSDn_HORIZON
     136           4 :     _osd_item_settings[OSD_HORIZON_SIDEBARS] = &osd->screen[_msp_status.current_screen].sidebars;   // OSDn_SIDEBARS
     137           4 :     _osd_item_settings[OSD_CRAFT_NAME] = &osd->screen[_msp_status.current_screen].message;          // OSDn_MESSAGE
     138           4 :     _osd_item_settings[OSD_FLYMODE] = &osd->screen[_msp_status.current_screen].fltmode;             // OSDn_FLTMODE
     139           4 :     _osd_item_settings[OSD_CURRENT_DRAW] = &osd->screen[_msp_status.current_screen].current;        // OSDn_CURRENT
     140           4 :     _osd_item_settings[OSD_MAH_DRAWN] = &osd->screen[_msp_status.current_screen].batused;           // OSDn_BATUSED
     141           4 :     _osd_item_settings[OSD_GPS_SPEED] = &osd->screen[_msp_status.current_screen].gspeed;            // OSDn_GSPEED
     142           4 :     _osd_item_settings[OSD_GPS_SATS] = &osd->screen[_msp_status.current_screen].sats;               // OSDn_SATS
     143           4 :     _osd_item_settings[OSD_ALTITUDE] = &osd->screen[_msp_status.current_screen].altitude;           // OSDn_ALTITUDE
     144           4 :     _osd_item_settings[OSD_POWER] = &osd->screen[_msp_status.current_screen].power;                 // OSDn_POWER
     145           4 :     _osd_item_settings[OSD_AVG_CELL_VOLTAGE] = &osd->screen[_msp_status.current_screen].cell_volt;  // OSDn_CELLVOLT
     146           4 :     _osd_item_settings[OSD_GPS_LON] = &osd->screen[_msp_status.current_screen].gps_longitude;       // OSDn_GPSLONG
     147           4 :     _osd_item_settings[OSD_GPS_LAT] = &osd->screen[_msp_status.current_screen].gps_latitude;        // OSDn_GPSLAT
     148           4 :     _osd_item_settings[OSD_PITCH_ANGLE] = &osd->screen[_msp_status.current_screen].pitch_angle;     // OSDn_PITCH
     149           4 :     _osd_item_settings[OSD_ROLL_ANGLE] = &osd->screen[_msp_status.current_screen].roll_angle;       // OSDn_ROLL
     150           4 :     _osd_item_settings[OSD_MAIN_BATT_USAGE] = &osd->screen[_msp_status.current_screen].batt_bar;    // OSDn_BATBAR
     151           4 :     _osd_item_settings[OSD_DISARMED] = &osd->screen[_msp_status.current_screen].arming;             // OSDn_ARMING
     152           4 :     _osd_item_settings[OSD_HOME_DIR] = &osd->screen[_msp_status.current_screen].home_dir;           // OSDn_HOMEDIR
     153           4 :     _osd_item_settings[OSD_HOME_DIST] = &osd->screen[_msp_status.current_screen].home_dist;         // OSDn_HOMEDIST
     154           4 :     _osd_item_settings[OSD_NUMERICAL_HEADING] = &osd->screen[_msp_status.current_screen].heading;   // OSDn_HEADING
     155           4 :     _osd_item_settings[OSD_NUMERICAL_VARIO] = &osd->screen[_msp_status.current_screen].vspeed;      // OSDn_VSPEED
     156             : #if HAL_WITH_ESC_TELEM
     157           4 :     _osd_item_settings[OSD_ESC_TMP] = &osd->screen[_msp_status.current_screen].esc_temp;            // OSDn_ESCTEMP
     158             : #endif
     159           4 :     _osd_item_settings[OSD_RTC_DATETIME] = &osd->screen[_msp_status.current_screen].clk;            // OSDn_CLK
     160             : #endif  // OSD_ENABLED
     161           4 :     _msp_status.osd_initialized = true;
     162             : }
     163             : 
     164           4 : void AP_MSP::loop(void)
     165             : {
     166           8 :     for (uint8_t i=0; i<_msp_status.backend_count; i++) {
     167             :         // one time uart init
     168             :         // note: we do not access a uart for a backend handled by another thread
     169           4 :         if (_backends[i] != nullptr && _backends[i]->use_msp_thread())  {
     170           4 :             _backends[i]->init_uart();
     171             :         }
     172             :     }
     173             : 
     174             :     while (true) {
     175        8614 :         hal.scheduler->delay(10); // 115200 baud, 18 MSP packets @4Hz, 100Hz should be OK
     176             : 
     177        8610 :         const uint32_t now = AP_HAL::millis();
     178             :         // toggle flashing every 0.7 seconds and every 2 seconds
     179        8610 :         if ((uint32_t(now * 0.00143) & 0x01) != _msp_status.flashing_on) {
     180         134 :             _msp_status.flashing_on = !_msp_status.flashing_on;
     181             :         }
     182        8610 :         if ((uint32_t(now * 0.0005) & 0x01) != _msp_status.slow_flashing_on) {
     183          46 :             _msp_status.slow_flashing_on = !_msp_status.slow_flashing_on;
     184             :         }
     185             : 
     186             :         // detect flight mode changes and steal focus from text messages
     187        8610 :         if (AP::notify().flags.flight_mode != _msp_status.last_flight_mode) {
     188           5 :             _msp_status.flight_mode_focus = true;
     189           5 :             _msp_status.last_flight_mode = AP::notify().flags.flight_mode;
     190           5 :             _msp_status.last_flight_mode_change_ms = AP_HAL::millis();
     191        8605 :         } else if (now - _msp_status.last_flight_mode_change_ms > OSD_FLIGHT_MODE_FOCUS_TIME) {
     192        7871 :             _msp_status.flight_mode_focus = false;
     193             :         }
     194             : 
     195             : #if OSD_ENABLED
     196             :         // check if we had a screen change
     197        8610 :         AP_OSD* osd = AP::osd();
     198             : 
     199        8610 :         if (osd != nullptr) {
     200        8610 :             const uint8_t screen = osd->is_readonly_screen() ? osd->get_current_screen() : _msp_status.current_screen;
     201        8610 :             if (_msp_status.current_screen != screen || !_msp_status.osd_initialized) {
     202           4 :                 _msp_status.current_screen = screen;
     203           4 :                 update_osd_item_settings();
     204             :             }
     205             :         }
     206             : #endif  // OSD_ENABLED
     207             : 
     208       17220 :         for (uint8_t i=0; i< _msp_status.backend_count; i++) {
     209             :             // note: we do not access a uart for a backend handled by another thread
     210        8610 :             if (_backends[i] != nullptr && _backends[i]->use_msp_thread()) {
     211             :                 // dynamically hide/unhide
     212        8610 :                 _backends[i]->hide_osd_items();
     213             :                 // process incoming MSP frames (and reply if needed)
     214        8610 :                 _backends[i]->process_incoming_data();
     215             :                 // push outgoing telemetry frames
     216        8610 :                 _backends[i]->process_outgoing_data();
     217             :             }
     218             :         }
     219        8610 :     }
     220             : }
     221             : 
     222           2 : AP_MSP_Telem_Backend* AP_MSP::find_protocol(const AP_SerialManager::SerialProtocol protocol) const {
     223           2 :     for (uint8_t i=0; i< _msp_status.backend_count; i++) {
     224           2 :         if (_backends[i] != nullptr && _backends[i]->get_serial_protocol() == protocol) {
     225           2 :             return _backends[i];
     226             :         }
     227             :     }
     228           0 :     return nullptr;
     229             : }
     230             : 
     231       39209 : bool AP_MSP::is_option_enabled(Option option) const
     232             : {
     233       39209 :     return (_options & (uint8_t)option) != 0;
     234             : }
     235             : 
     236             : namespace AP
     237             : {
     238       60983 : AP_MSP *msp()
     239             : {
     240       60983 :     return AP_MSP::get_singleton();
     241             : }
     242             : };
     243             : 
     244             : #endif //HAL_MSP_ENABLED