Line data    Source code

       1             : /*
       2             :  * This file is free software: you can redistribute it and/or modify it
       3             :  * under the terms of the GNU General Public License as published by the
       4             :  * Free Software Foundation, either version 3 of the License, or
       5             :  * (at your option) any later version.
       6             :  *
       7             :  * This file is distributed in the hope that it will be useful, but
       8             :  * WITHOUT ANY WARRANTY; without even the implied warranty of
       9             :  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
      10             :  * See the GNU General Public License for more details.
      11             :  *
      12             :  * You should have received a copy of the GNU General Public License along
      13             :  * with this program.  If not, see <http://www.gnu.org/licenses/>.
      14             :  *
      15             :  * Author: Oliver Walters / Currawong Engineering Pty Ltd
      16             :  */
      17             : 
      18             : #pragma once
      19             : 
      20             : #include "AP_PiccoloCAN_config.h"
      21             : 
      22             : #if AP_PICCOLOCAN_ENABLED
      23             : 
      24             : #include <AP_CANManager/AP_CANManager.h>
      25             : 
      26             : #include "AP_PiccoloCAN_config.h"
      27             : #include "AP_PiccoloCAN_Device.h"
      28             : #include "piccolo_protocol/ServoPackets.h"
      29             : 
      30             : #define PICCOLO_CAN_MAX_NUM_SERVO 16
      31             : #define PICCOLO_CAN_MAX_GROUP_SERVO (PICCOLO_CAN_MAX_NUM_SERVO / 4)
      32             : 
      33             : 
      34             : /*
      35             :  * Class representing an individual PiccoloCAN servo
      36             :  */
      37             : class AP_PiccoloCAN_Servo : public AP_PiccoloCAN_Device
      38             : {
      39             : public:
      40             : 
      41             :     virtual bool handle_can_frame(AP_HAL::CANFrame &frame) override;
      42             : 
      43           0 :     virtual bool is_enabled(void) const override { return status.statusA.status.enabled; }
      44             : 
      45             :     // Helper functions for accessing servo status data
      46           0 :     float position() const { return (float) status.statusA.position; }
      47           0 :     float commandedPosition() const { return (float) status.statusA.command; }
      48           0 :     float current() const { return (float) status.statusB.current * 0.01f; }
      49           0 :     float voltage() const { return (float) status.statusB.voltage * 0.01f; }
      50           0 :     float speed() const { return (float) status.statusB.speed; }
      51           0 :     uint8_t dutyCycle() const { return abs(status.statusB.dutyCycle); }
      52           0 :     float temperature() const { return (float) status.statusB.temperature; }
      53             : 
      54             :     int16_t command = 0;
      55             :     bool newCommand = false;
      56             :     bool newTelemetry = false;
      57             : 
      58             :     // Status / telemetry data
      59             :     struct Status_t {
      60             :         Servo_StatusA_t statusA;
      61             :         Servo_StatusB_t statusB;
      62             :     } status;
      63             : 
      64             :     // Settings information
      65             :     struct Settings_t {
      66             :         Servo_Firmware_t firmware;
      67             :         Servo_Address_t address;
      68             :         Servo_SettingsInfo_t settings;
      69             :     } settings;
      70             : };
      71             : 
      72             : #endif // AP_PICCOLOCAN_ENABLED