Line data    Source code

       1             : #pragma once
       2             : 
       3             : #include <AP_HAL/AP_HAL.h>
       4             : #include <AP_Param/AP_Param.h>
       5             : #include <SRV_Channel/SRV_Channel_config.h>
       6             : #include "AP_ESC_Telem_Backend.h"
       7             : 
       8             : #if HAL_WITH_ESC_TELEM
       9             : 
      10             : #ifndef ESC_TELEM_MAX_ESCS
      11             :     #define ESC_TELEM_MAX_ESCS NUM_SERVO_CHANNELS
      12             : #endif
      13             : static_assert(ESC_TELEM_MAX_ESCS > 0, "Cannot have 0 ESC telemetry instances");
      14             : 
      15             : #define ESC_TELEM_DATA_TIMEOUT_MS 5000UL
      16             : #define ESC_RPM_DATA_TIMEOUT_US 1000000UL
      17             : 
      18             : class AP_ESC_Telem {
      19             : public:
      20             :     friend class AP_ESC_Telem_Backend;
      21             : 
      22             :     AP_ESC_Telem();
      23             : 
      24             :     /* Do not allow copies */
      25             :     CLASS_NO_COPY(AP_ESC_Telem);
      26             : 
      27             :     static const struct AP_Param::GroupInfo var_info[];
      28             : 
      29             :     static AP_ESC_Telem *get_singleton();
      30             : 
      31             :     // get an individual ESC's slewed rpm if available, returns true on success
      32             :     bool get_rpm(uint8_t esc_index, float& rpm) const;
      33             : 
      34             :     // get an individual ESC's raw rpm and error rate if available
      35             :     bool get_raw_rpm_and_error_rate(uint8_t esc_index, float& rpm, float& error_rate) const;
      36             : 
      37             :     // get raw telemetry data, used by IOMCU
      38           0 :     const volatile AP_ESC_Telem_Backend::TelemetryData& get_telem_data(uint8_t esc_index) const {
      39           0 :         return _telem_data[esc_index];
      40             :     }
      41             : 
      42             :     // return the average motor RPM
      43             :     float get_average_motor_rpm(uint32_t servo_channel_mask) const;
      44             : 
      45             :     // return the average motor RPM
      46          42 :     float get_average_motor_rpm() const { return get_average_motor_rpm(0xFFFFFFFF); }
      47             : 
      48             :     // determine whether all the motors in servo_channel_mask are running
      49             :     bool are_motors_running(uint32_t servo_channel_mask, float min_rpm, float max_rpm) const;
      50             : 
      51             :     // get an individual ESC's temperature in centi-degrees if available, returns true on success
      52             :     bool get_temperature(uint8_t esc_index, int16_t& temp) const;
      53             : 
      54             :     // get an individual motor's temperature in centi-degrees if available, returns true on success
      55             :     bool get_motor_temperature(uint8_t esc_index, int16_t& temp) const;
      56             : 
      57             :     // get the highest ESC temperature in centi-degrees if available, returns true if there is valid data for at least one ESC
      58             :     bool get_highest_temperature(int16_t& temp) const;
      59             : 
      60             :     // get an individual ESC's current in Ampere if available, returns true on success
      61             :     bool get_current(uint8_t esc_index, float& amps) const;
      62             : 
      63             :     // get an individual ESC's usage time in seconds if available, returns true on success
      64             :     bool get_usage_seconds(uint8_t esc_index, uint32_t& usage_sec) const;
      65             : 
      66             :     // get an individual ESC's voltage in Volt if available, returns true on success
      67             :     bool get_voltage(uint8_t esc_index, float& volts) const;
      68             : 
      69             :     // get an individual ESC's consumption in milli-Ampere.hour if available, returns true on success
      70             :     bool get_consumption_mah(uint8_t esc_index, float& consumption_mah) const;
      71             : 
      72             : #if AP_EXTENDED_ESC_TELEM_ENABLED
      73             :     // get an individual ESC's input duty cycle if available, returns true on success
      74             :     bool get_input_duty(uint8_t esc_index, uint8_t& input_duty) const;
      75             : 
      76             :     // get an individual ESC's output duty cycle if available, returns true on success
      77             :     bool get_output_duty(uint8_t esc_index, uint8_t& output_duty) const;
      78             : 
      79             :     // get an individual ESC's status flags if available, returns true on success
      80             :     bool get_flags(uint8_t esc_index, uint32_t& flags) const;
      81             : 
      82             :     // get an individual ESC's percentage of output power if available, returns true on success
      83             :     bool get_power_percentage(uint8_t esc_index, uint8_t& power_percentage) const;
      84             : #endif
      85             : 
      86             :     // return the average motor frequency in Hz for dynamic filtering
      87       77375 :     float get_average_motor_frequency_hz(uint32_t servo_channel_mask) const { return get_average_motor_rpm(servo_channel_mask) * (1.0f / 60.0f); };
      88             : 
      89             :     // return the average motor frequency in Hz for dynamic filtering
      90       77375 :     float get_average_motor_frequency_hz() const { return get_average_motor_frequency_hz(0xFFFFFFFF); }
      91             : 
      92             :     // return all of the motor frequencies in Hz for dynamic filtering
      93             :     uint8_t get_motor_frequencies_hz(uint8_t nfreqs, float* freqs) const;
      94             : 
      95             :     // get the number of ESCs that sent valid telemetry data in the last ESC_TELEM_DATA_TIMEOUT_MS
      96             :     uint8_t get_num_active_escs() const;
      97             : 
      98             :     // get mask of ESCs that sent valid telemetry data in the last
      99             :     // ESC_TELEM_DATA_TIMEOUT_MS
     100             :     uint32_t get_active_esc_mask() const;
     101             : 
     102             :     // return an active ESC with the highest RPM for the purposes of reporting (e.g. in the OSD)
     103             :     uint8_t get_max_rpm_esc() const;
     104             : 
     105             :     // return the last time telemetry data was received in ms for the given ESC or 0 if never
     106       14132 :     uint32_t get_last_telem_data_ms(uint8_t esc_index) const {
     107       14132 :         if (esc_index >= ESC_TELEM_MAX_ESCS) {return 0;}
     108       14132 :         return _telem_data[esc_index].last_update_ms;
     109             :     }
     110             : 
     111             :     // send telemetry data to mavlink
     112             :     void send_esc_telemetry_mavlink(uint8_t mav_chan);
     113             : 
     114             :     // update at 10Hz to log telemetry
     115             :     void update();
     116             : 
     117             :     // is rpm telemetry configured for the provided channel mask
     118             :     bool is_telemetry_active(uint32_t servo_channel_mask) const;
     119             : 
     120             :     // callback to update the rpm in the frontend, should be called by the driver when new data is available
     121             :     // can also be called from scripting
     122             :     void update_rpm(const uint8_t esc_index, const float new_rpm, const float error_rate);
     123             : 
     124             :     // callback to update the data in the frontend, should be called by the driver when new data is available
     125             :     void update_telem_data(const uint8_t esc_index, const AP_ESC_Telem_Backend::TelemetryData& new_data, const uint16_t data_mask);
     126             : 
     127             : #if AP_SCRIPTING_ENABLED
     128             :     /*
     129             :       set RPM scale factor from script
     130             :      */
     131             :     void set_rpm_scale(const uint8_t esc_index, const float scale_factor);
     132             : #endif
     133             : 
     134             : private:
     135             : 
     136             :     // helper that validates RPM data
     137             :     static bool rpm_data_within_timeout (const volatile AP_ESC_Telem_Backend::RpmData &instance, const uint32_t timeout_us);
     138             :     static bool was_rpm_data_ever_reported (const volatile AP_ESC_Telem_Backend::RpmData &instance);
     139             : 
     140             : #if AP_EXTENDED_DSHOT_TELEM_V2_ENABLED
     141             :     // helpers that aggregate data in EDTv2 messages
     142             :     static uint16_t merge_edt2_status(uint16_t old_status, uint16_t new_status);
     143             :     static uint16_t merge_edt2_stress(uint16_t old_stress, uint16_t new_stress);
     144             : #endif
     145             : 
     146             :     // rpm data
     147             :     volatile AP_ESC_Telem_Backend::RpmData _rpm_data[ESC_TELEM_MAX_ESCS];
     148             :     // telemetry data
     149             :     volatile AP_ESC_Telem_Backend::TelemetryData _telem_data[ESC_TELEM_MAX_ESCS];
     150             : 
     151             :     uint32_t _last_telem_log_ms[ESC_TELEM_MAX_ESCS];
     152             :     uint32_t _last_rpm_log_us[ESC_TELEM_MAX_ESCS];
     153             :     uint8_t next_idx;
     154             : 
     155             : #if AP_SCRIPTING_ENABLED
     156             :     // allow for scaling of RPMs via lua scripts
     157             :     float rpm_scale_factor[ESC_TELEM_MAX_ESCS];
     158             :     uint32_t rpm_scale_mask;
     159             : #endif
     160             :     
     161             :     bool _have_data;
     162             : 
     163             :     AP_Int8 mavlink_offset;
     164             : 
     165             :     static AP_ESC_Telem *_singleton;
     166             : };
     167             : 
     168             : namespace AP {
     169             :     AP_ESC_Telem &esc_telem();
     170             : };
     171             : 
     172             : #endif // HAL_WITH_ESC_TELEM