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_Common/AP_FWVersion.h>
      17             : #include <AP_Vehicle/AP_Vehicle_Type.h>
      18             : #include <AP_Stats/AP_Stats.h>
      19             : #include <AP_RSSI/AP_RSSI.h>
      20             : #include <AP_Notify/AP_Notify.h>
      21             : 
      22             : #include "AP_MSP.h"
      23             : #include "AP_MSP_Telem_DJI.h"
      24             : 
      25             : #include <stdio.h>
      26             : 
      27             : #if HAL_MSP_ENABLED
      28             : extern const AP_HAL::HAL& hal;
      29             : 
      30             : using namespace MSP;
      31             : 
      32           1 : bool AP_MSP_Telem_DJI::init_uart()
      33             : {
      34           1 :     if (_msp_port.uart != nullptr)  {
      35           1 :         _msp_port.uart->set_flow_control(AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE);
      36           1 :         _msp_port.uart->begin(AP_SERIALMANAGER_MSP_BAUD, AP_SERIALMANAGER_MSP_BUFSIZE_RX, AP_SERIALMANAGER_MSP_BUFSIZE_TX);
      37           1 :         return true;
      38             :     }
      39           0 :     return false;
      40             : }
      41             : 
      42        1913 : bool AP_MSP_Telem_DJI::is_scheduler_enabled() const
      43             : {
      44        1913 :     const AP_MSP *msp = AP::msp();
      45        1913 :     return msp && msp->is_option_enabled(AP_MSP::Option::TELEMETRY_MODE);
      46             : }
      47             : 
      48        1913 : void AP_MSP_Telem_DJI::hide_osd_items(void)
      49             : {
      50        1913 :     const AP_MSP *msp = AP::msp();
      51        1913 :     if (msp == nullptr) {
      52           0 :         return;
      53             :     }
      54             :     // apply base class defaults
      55        1913 :     AP_MSP_Telem_Backend::hide_osd_items();
      56             : 
      57             :     // apply DJI OSD specific rules
      58        1913 :     const AP_Notify& notify = AP::notify();
      59             :     // default is hide the DJI flightmode widget
      60        1913 :     BIT_SET(osd_hidden_items_bitmask, OSD_FLYMODE);
      61             : 
      62        1913 :     if (msp->_msp_status.flashing_on) {
      63             :         // flash flightmode on failsafe
      64         931 :         if (notify.flags.failsafe_battery || notify.flags.failsafe_gcs || notify.flags.failsafe_radio || notify.flags.ekf_bad) {
      65           0 :             BIT_CLEAR(osd_hidden_items_bitmask, OSD_FLYMODE);
      66             :         }
      67             :     }
      68             : }
      69             : 
      70          42 : uint32_t AP_MSP_Telem_DJI::get_osd_flight_mode_bitmask(void)
      71             : {
      72          42 :     uint32_t mode_mask = AP_MSP_Telem_Backend::get_osd_flight_mode_bitmask();
      73          42 :     const AP_Notify& notify = AP::notify();
      74             : 
      75             :     // check failsafe
      76          42 :     if (notify.flags.failsafe_battery || notify.flags.failsafe_gcs || notify.flags.failsafe_radio || notify.flags.ekf_bad ) {
      77           0 :         BIT_SET(mode_mask, DJI_FLAG_FS);
      78             :     }
      79          42 :     return mode_mask;
      80             : }
      81             : 
      82           0 : MSPCommandResult AP_MSP_Telem_DJI::msp_process_out_fc_variant(sbuf_t *dst)
      83             : {
      84           0 :     sbuf_write_data(dst, "BTFL", FLIGHT_CONTROLLER_IDENTIFIER_LENGTH);
      85           0 :     return MSP_RESULT_ACK;
      86             : }
      87             : 
      88          42 : MSPCommandResult AP_MSP_Telem_DJI::msp_process_out_esc_sensor_data(sbuf_t *dst)
      89             : {
      90             : #if HAL_WITH_ESC_TELEM
      91          42 :     int16_t highest_temperature = 0;
      92          42 :     AP_ESC_Telem& telem = AP::esc_telem();
      93          42 :     if (!displaying_stats_screen()) {
      94          42 :         telem.get_highest_temperature(highest_temperature);
      95             :     } else {
      96             : #if OSD_ENABLED
      97           0 :         AP_OSD *osd = AP::osd();
      98           0 :         if (osd == nullptr) {
      99           0 :             return MSP_RESULT_ERROR;
     100             :         }
     101           0 :         WITH_SEMAPHORE(osd->get_semaphore());
     102           0 :         highest_temperature = osd->get_stats_info().max_esc_temp;
     103             : #endif
     104           0 :     }
     105             : 
     106             :     const struct PACKED {
     107             :         uint8_t temp;
     108             :         uint16_t rpm;
     109             :     } esc_sensor_data {
     110          42 :         temp : uint8_t(highest_temperature * 0.01f),            // deg, report max temperature
     111          84 :         rpm : uint16_t(telem.get_average_motor_rpm() * 0.1f)    // rpm, report average RPM across all motors
     112          42 :     };
     113             : 
     114          42 :     sbuf_write_data(dst, &esc_sensor_data, sizeof(esc_sensor_data));
     115             : #endif
     116          42 :     return MSP_RESULT_ACK;
     117             : }
     118             : 
     119        1099 : void AP_MSP_Telem_DJI::update_home_pos(home_state_t &home_state)
     120             : {
     121        1099 :     AP_MSP_Telem_Backend::update_home_pos(home_state);
     122             : #if OSD_ENABLED
     123        1099 :     if (!displaying_stats_screen()) {
     124        1099 :         return;
     125             :     }
     126           0 :     AP_MSP *msp = AP::msp();
     127           0 :     if (msp == nullptr) {
     128           0 :         return;
     129             :     }
     130           0 :     AP_OSD *osd = AP::osd();
     131           0 :     if (osd == nullptr) {
     132           0 :         return;
     133             :     }
     134           0 :     WITH_SEMAPHORE(osd->get_semaphore());
     135             :     // override telemetry with max distance and altitude info
     136             :     // alternate max distance with traveled distance every 2 seconds
     137           0 :     if (msp->_msp_status.slow_flashing_on) {
     138           0 :         home_state.home_distance_m = osd->get_stats_info().max_dist_m;
     139             :     } else {
     140           0 :         home_state.home_distance_m = osd->get_stats_info().last_distance_m;
     141             :     }
     142           0 :     home_state.rel_altitude_cm = osd->get_stats_info().max_alt_m * 100;
     143             : #endif
     144           0 : }
     145             : 
     146         126 : void AP_MSP_Telem_DJI::update_battery_state(battery_state_t &_battery_state)
     147             : {
     148         126 :     AP_MSP_Telem_Backend::update_battery_state(_battery_state);
     149             : #if OSD_ENABLED
     150         126 :     if (!displaying_stats_screen()) {
     151         126 :         return;
     152             :     }
     153           0 :     AP_OSD *osd = AP::osd();
     154           0 :     if (osd == nullptr) {
     155           0 :         return;
     156             :     }
     157           0 :     WITH_SEMAPHORE(osd->get_semaphore());
     158             :     // override telemetry with max current and voltage info
     159           0 :     _battery_state.batt_current_a = osd->get_stats_info().max_current_a;
     160           0 :     _battery_state.batt_voltage_v = osd->get_stats_info().min_voltage_v;
     161             : #endif
     162           0 : }
     163             : 
     164        1015 : void AP_MSP_Telem_DJI::update_gps_state(gps_state_t &gps_state)
     165             : {
     166        1015 :     AP_MSP_Telem_Backend::update_gps_state(gps_state);
     167             : #if OSD_ENABLED
     168        1015 :     if (!displaying_stats_screen()) {
     169        1015 :         return;
     170             :     }
     171           0 :     AP_OSD *osd = AP::osd();
     172           0 :     if (osd == nullptr) {
     173           0 :         return;
     174             :     }
     175           0 :     WITH_SEMAPHORE(osd->get_semaphore());
     176             :     // override telemetry with max speed info
     177           0 :     gps_state.speed_cms = osd->get_stats_info().max_speed_mps * 100;
     178             : #endif
     179           0 : }
     180             : 
     181           0 : void AP_MSP_Telem_DJI::update_airspeed(airspeed_state_t &airspeed_state)
     182             : {
     183           0 :     AP_MSP_Telem_Backend::update_airspeed(airspeed_state);
     184             : #if OSD_ENABLED
     185           0 :     if (!displaying_stats_screen()) {
     186           0 :         return;
     187             :     }
     188           0 :     AP_OSD *osd = AP::osd();
     189           0 :     if (osd == nullptr) {
     190           0 :         return;
     191             :     }
     192           0 :     WITH_SEMAPHORE(osd->get_semaphore());
     193             :     // override telemetry with max speed info
     194           0 :     airspeed_state.airspeed_estimate_ms = osd->get_stats_info().max_airspeed_mps;
     195             : #endif
     196           0 : }
     197             : 
     198          17 : void AP_MSP_Telem_DJI::update_flight_mode_str(char *flight_mode_str, uint8_t size, bool wind_enabled)
     199             : {
     200          17 :     AP_MSP_Telem_Backend::update_flight_mode_str(flight_mode_str, size, wind_enabled);
     201             : #if OSD_ENABLED
     202          17 :     if (!displaying_stats_screen()) {
     203          17 :         return;
     204             :     }
     205           0 :     AP_Stats *stats = AP::stats();
     206           0 :     if (stats != nullptr) {
     207           0 :         uint32_t t = stats->get_flight_time_s();
     208             :         // need to check snprintf return value to prevent format-truncation warning in GCC because of -Werror=format-truncation
     209           0 :         if (snprintf(flight_mode_str, size, "%s %3u:%02u", "STATS", unsigned(t/60), unsigned(t%60)) < 0) {
     210           0 :             snprintf(flight_mode_str, size, "%s", "STATS --:--");
     211             :         }
     212             :     } else {
     213           0 :         snprintf(flight_mode_str, size, "%s", "STATS");
     214             :     }
     215             : #endif
     216             : }
     217             : 
     218        1015 : bool AP_MSP_Telem_DJI::get_rssi(float &rssi) const
     219             : {
     220        1015 :     if (!AP_MSP_Telem_Backend::get_rssi(rssi)) {
     221        1015 :         return false;
     222             :     }
     223             : #if OSD_ENABLED
     224           0 :     if (!displaying_stats_screen()) {
     225           0 :         return true;
     226             :     }
     227             : #if AP_RSSI_ENABLED
     228           0 :     AP_RSSI* ap_rssi = AP::rssi();
     229           0 :     if (ap_rssi == nullptr) {
     230           0 :         return false;
     231             :     }
     232           0 :     if (!ap_rssi->enabled()) {
     233           0 :         return false;
     234             :     }
     235             : #else
     236             :     return false;
     237             : #endif
     238           0 :     AP_OSD *osd = AP::osd();
     239           0 :     if (osd == nullptr) {
     240           0 :         return false;
     241             :     }
     242           0 :     WITH_SEMAPHORE(osd->get_semaphore());
     243             :     // override telemetry with min rssi info
     244             :     // Note: return false when min_rssi has not been updated yet by AP_OSD::update_stats()
     245           0 :     if (is_equal(osd->get_stats_info().min_rssi, FLT_MAX)) {
     246           0 :         return false;
     247             :     }
     248           0 :     rssi = osd->get_stats_info().min_rssi; // range is [0-1]
     249             : #endif
     250           0 :     return true;
     251           0 : }
     252             : #endif //HAL_MSP_ENABLED