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_Servo_Telem.h"
      17             : 
      18             : #if AP_SERVO_TELEM_ENABLED
      19             : 
      20             : #include <AP_HAL/AP_HAL.h>
      21             : #include <AP_Logger/AP_Logger.h>
      22             : #include <AP_BoardConfig/AP_BoardConfig.h>
      23             : 
      24             : AP_Servo_Telem *AP_Servo_Telem::_singleton;
      25             : 
      26        2098 : AP_Servo_Telem::AP_Servo_Telem()
      27             : {
      28        2098 :     if (_singleton) {
      29           0 :         AP_HAL::panic("Too many AP_Servo_Telem instances");
      30             :     }
      31        2098 :     _singleton = this;
      32        2098 : }
      33             : 
      34             : // return true if the requested types of data are available
      35      966992 : bool AP_Servo_Telem::TelemetryData::present(const uint16_t type_mask) const volatile
      36             : {
      37      966992 :     return (present_types & type_mask) != 0;
      38             : }
      39             : 
      40             : // record an update to the telemetry data together with timestamp
      41             : // callback to update the data in the frontend, should be called by the driver when new data is available
      42       54320 : void AP_Servo_Telem::update_telem_data(const uint8_t servo_index, const TelemetryData& new_data)
      43             : {
      44             :     // telemetry data are not protected by a semaphore even though updated from different threads
      45             :     // each element is a primitive type and the timestamp is only updated at the end, thus a caller
      46             :     // can only get slightly more up-to-date information that perhaps they were expecting or might
      47             :     // read data that has just gone stale - both of these are safe and avoid the overhead of locking
      48             : 
      49       54320 :     if (servo_index >= ARRAY_SIZE(_telem_data) || (new_data.present_types == 0)) {
      50           0 :         return;
      51             :     }
      52       54320 :     active_mask |= 1U << servo_index;
      53             : 
      54       54320 :     volatile TelemetryData &telemdata = _telem_data[servo_index];
      55             : 
      56       54320 :     if (new_data.present(TelemetryData::Types::COMMANDED_POSITION)) {
      57       54320 :         telemdata.command_position = new_data.command_position;
      58             :     }
      59       54320 :     if (new_data.present(TelemetryData::Types::MEASURED_POSITION)) {
      60       54320 :         telemdata.measured_position = new_data.measured_position;
      61             :     }
      62       54320 :     if (new_data.present(TelemetryData::Types::FORCE)) {
      63           0 :         telemdata.force = new_data.force;
      64             :     }
      65       54320 :     if (new_data.present(TelemetryData::Types::SPEED)) {
      66           0 :         telemdata.speed = new_data.speed;
      67             :     }
      68       54320 :     if (new_data.present(TelemetryData::Types::VOLTAGE)) {
      69       54320 :         telemdata.voltage = new_data.voltage;
      70             :     }
      71       54320 :     if (new_data.present(TelemetryData::Types::CURRENT)) {
      72       54320 :         telemdata.current = new_data.current;
      73             :     }
      74       54320 :     if (new_data.present(TelemetryData::Types::DUTY_CYCLE)) {
      75           0 :         telemdata.duty_cycle = new_data.duty_cycle;
      76             :     }
      77       54320 :     if (new_data.present(TelemetryData::Types::MOTOR_TEMP)) {
      78       54320 :         telemdata.motor_temperature_cdeg = new_data.motor_temperature_cdeg;
      79             :     }
      80       54320 :     if (new_data.present(TelemetryData::Types::PCB_TEMP)) {
      81       54320 :         telemdata.pcb_temperature_cdeg = new_data.pcb_temperature_cdeg;
      82             :     }
      83       54320 :     if (new_data.present(TelemetryData::Types::STATUS)) {
      84           0 :         telemdata.status_flags = new_data.status_flags;
      85             :     }
      86             : 
      87       54320 :     telemdata.present_types |= new_data.present_types;
      88       54320 :     telemdata.last_update_ms = AP_HAL::millis();
      89             : }
      90             : 
      91     7525129 : void AP_Servo_Telem::update()
      92             : {
      93             : #if HAL_LOGGING_ENABLED
      94     7525129 :     write_log();
      95             : #endif
      96     7525129 : }
      97             : 
      98             : #if HAL_LOGGING_ENABLED
      99     7525129 : void AP_Servo_Telem::write_log()
     100             : {
     101     7525129 :     AP_Logger *logger = AP_Logger::get_singleton();
     102             : 
     103             :     // Check logging is available and enabled
     104     7525129 :     if ((logger == nullptr) || !logger->logging_enabled() || active_mask == 0) {
     105     7516300 :         return;
     106             :     }
     107             : 
     108        8829 :     const uint64_t now_us = AP_HAL::micros64();
     109             : 
     110      291357 :     for (uint8_t i = 0; i < ARRAY_SIZE(_telem_data); i++) {
     111      282528 :         const volatile TelemetryData &telemdata = _telem_data[i];
     112             : 
     113      282528 :         if (telemdata.last_update_ms == _last_telem_log_ms[i]) {
     114             :             // No new data since last log call, skip
     115      229554 :             continue;
     116             :         }
     117             : 
     118             :         // Update last log timestamp
     119       52974 :         _last_telem_log_ms[i] = telemdata.last_update_ms;
     120             : 
     121             :         // Log, use nan for float values which are not available
     122       52974 :         const struct log_CSRV pkt {
     123             :             LOG_PACKET_HEADER_INIT(LOG_CSRV_MSG),
     124             :             time_us     : now_us,
     125             :             id          : i,
     126       52974 :             position    : telemdata.present(TelemetryData::Types::MEASURED_POSITION)  ? telemdata.measured_position             : AP_Logger::quiet_nanf(),
     127       52974 :             force       : telemdata.present(TelemetryData::Types::FORCE)              ? telemdata.force                         : AP_Logger::quiet_nanf(),
     128       52974 :             speed       : telemdata.present(TelemetryData::Types::SPEED)              ? telemdata.speed                         : AP_Logger::quiet_nanf(),
     129       52974 :             power_pct   : telemdata.duty_cycle,
     130       52974 :             pos_cmd     : telemdata.present(TelemetryData::Types::COMMANDED_POSITION) ? telemdata.command_position              : AP_Logger::quiet_nanf(),
     131       52974 :             voltage     : telemdata.present(TelemetryData::Types::VOLTAGE)            ? telemdata.voltage                       : AP_Logger::quiet_nanf(),
     132       52974 :             current     : telemdata.present(TelemetryData::Types::CURRENT)            ? telemdata.current                       : AP_Logger::quiet_nanf(),
     133       52974 :             mot_temp    : telemdata.present(TelemetryData::Types::MOTOR_TEMP)         ? telemdata.motor_temperature_cdeg * 0.01 : AP_Logger::quiet_nanf(),
     134       52974 :             pcb_temp    : telemdata.present(TelemetryData::Types::PCB_TEMP)           ? telemdata.pcb_temperature_cdeg * 0.01   : AP_Logger::quiet_nanf(),
     135       52974 :             error       : telemdata.status_flags,
     136      423792 :         };
     137       52974 :         AP::logger().WriteBlock(&pkt, sizeof(pkt));
     138             :     }
     139             : 
     140             : }
     141             : #endif  // HAL_LOGGING_ENABLED
     142             : 
     143             : // Fill in telem structure if telem is available, return false if not
     144           0 : bool AP_Servo_Telem::get_telem(const uint8_t servo_index, TelemetryData& telem) const volatile
     145             : {
     146             :     // Check for valid index
     147           0 :     if (servo_index >= ARRAY_SIZE(_telem_data)) {
     148           0 :         return false;
     149             :     }
     150             : 
     151             :     // Check if data has ever been received for the servo index provided
     152           0 :     if (!is_active(servo_index)) {
     153           0 :         return false;
     154             :     }
     155             : 
     156           0 :     telem = *const_cast<TelemetryData*>(&_telem_data[servo_index]);
     157           0 :     return true;
     158             : }
     159             : 
     160           0 : bool AP_Servo_Telem::is_active(const uint8_t servo_index) const volatile
     161             : {
     162           0 :     return (active_mask & (1U << servo_index)) != 0;
     163             : }
     164             : 
     165             : // Get the AP_Servo_Telem singleton
     166        9057 : AP_Servo_Telem *AP_Servo_Telem::get_singleton()
     167             : {
     168        9057 :     return AP_Servo_Telem::_singleton;
     169             : }
     170             : 
     171             : #endif // AP_SERVO_TELEM_ENABLED