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_ESC_Telem_SITL.h"
      17             : #include "AP_ESC_Telem.h"
      18             : #include <AP_HAL/AP_HAL.h>
      19             : #include <SITL/SITL.h>
      20             : 
      21             : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
      22             : 
      23             : #include <AP_Math/AP_Math.h>
      24             : 
      25             : extern const AP_HAL::HAL& hal;
      26             : 
      27        2073 : AP_ESC_Telem_SITL::AP_ESC_Telem_SITL()
      28             : {
      29        2073 : }
      30             : 
      31    41720409 : void AP_ESC_Telem_SITL::update()
      32             : {
      33    41720409 :     SITL::SIM* sitl = AP::sitl();
      34             : 
      35    41720409 :     if (!sitl) {
      36           0 :         return;
      37             :     }
      38             : 
      39             : #if HAL_WITH_ESC_TELEM
      40             : 
      41    41720409 :     if (AP_HAL::millis64() < 6000) {
      42             :         // this prevents us sending blank data at startup, which triggers
      43             :         // ESC telem messages for all channels
      44     1767462 :         return;
      45             :     }
      46    39952947 :     uint32_t mask = sitl->state.motor_mask;
      47             : 
      48             :     /*
      49             :       mask out motors we should not be providing telemetry for. On
      50             :       AP_Periph SIM_CAN_SRV_MSK are the outputs we will provide
      51             :       telemetry for, on the main firmware it is the ones we don't
      52             :       provide telemetry for
      53             :     */
      54             : #if defined(HAL_BUILD_AP_PERIPH)
      55             :     mask &= uint32_t(sitl->can_servo_mask);
      56             : #else
      57    39952947 :     mask &= ~uint32_t(sitl->can_servo_mask);
      58             : #endif
      59             :     uint8_t bit;
      60             : 
      61   180625345 :     while ((bit = __builtin_ffs(mask)) != 0) {
      62   140672398 :         uint8_t motor = bit-1;
      63   140672398 :         mask &= ~(1U<<motor);
      64             : 
      65   140672398 :         const float min_rpm = hal.util->get_soft_armed()? sitl->esc_rpm_armed : 0;
      66   140672398 :         update_rpm(motor, MAX(min_rpm, sitl->state.rpm[motor]));
      67             : 
      68             :         // some fake values so that is_telemetry_active() returns true
      69   140672398 :         TelemetryData t {
      70             :             .temperature_cdeg = 3200,
      71             :             .voltage = 16.8f,
      72             :             .current = 0.8f,
      73             :             .consumption_mah = 1.0f,
      74             :             .motor_temp_cdeg = 3500,
      75             : #if AP_EXTENDED_ESC_TELEM_ENABLED
      76             :             .input_duty = 1,
      77             :             .output_duty = 2,
      78             :             .flags = 3,
      79             :             .power_percentage = 4,
      80             : #endif
      81             :         };
      82             : 
      83   140672398 :         update_telem_data(motor, t,
      84             :             AP_ESC_Telem_Backend::TelemetryType::CURRENT
      85             :                 | AP_ESC_Telem_Backend::TelemetryType::VOLTAGE
      86             :                 | AP_ESC_Telem_Backend::TelemetryType::CONSUMPTION
      87             :                 | AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE
      88             :                 | AP_ESC_Telem_Backend::TelemetryType::MOTOR_TEMPERATURE
      89             : #if AP_EXTENDED_ESC_TELEM_ENABLED
      90             :                 | AP_ESC_Telem_Backend::TelemetryType::POWER_PERCENTAGE
      91             :                 | AP_ESC_Telem_Backend::TelemetryType::INPUT_DUTY
      92             :                 | AP_ESC_Telem_Backend::TelemetryType::OUTPUT_DUTY
      93             :                 | AP_ESC_Telem_Backend::TelemetryType::FLAGS
      94             : #endif
      95             :                 );
      96             :     }
      97             : #endif
      98             : }
      99             : 
     100             : #endif