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             : #include "AP_PiccoloCAN_config.h"
      19             : 
      20             : #if AP_PICCOLOCAN_ENABLED
      21             : 
      22             : #include "AP_PiccoloCAN_ESC.h"
      23             : 
      24             : /*
      25             :  * Decode a received CAN frame.
      26             :  * It is assumed at this point that the received frame is intended for *this* ESC
      27             :  */
      28           0 : bool AP_PiccoloCAN_ESC::handle_can_frame(AP_HAL::CANFrame &frame)
      29             : {
      30           0 :     bool result = true;
      31             : 
      32             :     // The ESC address is the lower byte of the frame ID
      33           0 :     uint8_t addr = frame.id & 0xFF;
      34             : 
      35             :     // Ignore any ESC with node ID of zero
      36           0 :     if (addr == 0x00) {
      37           0 :         return false;
      38             :     }
      39             : 
      40           0 :     addr -= 1;
      41             : 
      42             :     uint8_t extended;
      43             : 
      44           0 :     if (decodeESC_StatusAPacketStructure(&frame, &status.statusA)) {
      45           0 :         newTelemetry = true;
      46           0 :         update_rpm(addr, rpm());
      47           0 :     } else if (decodeESC_StatusBPacketStructure(&frame, &status.statusB)) {
      48           0 :         AP_ESC_Telem_Backend::TelemetryData telem {};
      49             : 
      50           0 :         telem.voltage = voltage() * 10;
      51           0 :         telem.current = current() * 10;
      52           0 :         telem.motor_temp_cdeg = int16_t(motorTemperature() * 100);
      53           0 :         telem.temperature_cdeg = int16_t(temperature() * 100);
      54             : 
      55           0 :         update_telem_data(addr, telem,
      56             :             AP_ESC_Telem_Backend::TelemetryType::CURRENT |
      57             :             AP_ESC_Telem_Backend::TelemetryType::VOLTAGE |
      58             :             AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE |
      59             :             AP_ESC_Telem_Backend::TelemetryType::MOTOR_TEMPERATURE);
      60             : 
      61           0 :         newTelemetry = true;
      62           0 :     } else if (decodeESC_StatusCPacketStructure(&frame, &status.statusC)) {
      63           0 :         AP_ESC_Telem_Backend::TelemetryData telem {};
      64             : 
      65           0 :         telem.temperature_cdeg = temperature() * 100;
      66           0 :         update_telem_data(addr, telem, AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE);
      67           0 :         newTelemetry = true;
      68           0 :     } else if (decodeESC_WarningErrorStatusPacket(&frame, &status.warnings, &status.errors, &extended, &status.warnings, &status.errors)) {
      69           0 :         newTelemetry = true;
      70           0 :     } else if (decodeESC_FirmwarePacketStructure(&frame, &settings.firmware)) {
      71           0 :     } else if (decodeESC_AddressPacketStructure(&frame, &settings.address)) {
      72           0 :     } else if (decodeESC_EEPROMSettingsPacketStructure(&frame, &settings.eeprom)) {
      73             :     } else {
      74           0 :         result = false;
      75             :     }
      76             : 
      77           0 :     if (result) {
      78           0 :         reset_rx_timestamp();
      79             :     }
      80             : 
      81           0 :     return result;
      82             : }
      83             : 
      84             : 
      85             : /* Piccolo Glue Logic
      86             :  * The following functions are required by the auto-generated protogen code.
      87             :  */
      88             : 
      89             : //! \return the packet data pointer from the packet
      90           0 : uint8_t* getESCVelocityPacketData(void* pkt)
      91             : {
      92           0 :     AP_HAL::CANFrame* frame = (AP_HAL::CANFrame*) pkt;
      93             : 
      94           0 :     return (uint8_t*) frame->data;
      95             : }
      96             : 
      97             : //! \return the packet data pointer from the packet, const
      98           0 : const uint8_t* getESCVelocityPacketDataConst(const void* pkt)
      99             : {
     100           0 :     AP_HAL::CANFrame* frame = (AP_HAL::CANFrame*) pkt;
     101             : 
     102           0 :     return (const uint8_t*) frame->data;
     103             : }
     104             : 
     105             : //! Complete a packet after the data have been encoded
     106           0 : void finishESCVelocityPacket(void* pkt, int size, uint32_t packetID)
     107             : {
     108           0 :     AP_HAL::CANFrame* frame = (AP_HAL::CANFrame*) pkt;
     109             : 
     110           0 :     if (size > AP_HAL::CANFrame::MaxDataLen) {
     111           0 :         size = AP_HAL::CANFrame::MaxDataLen;
     112             :     }
     113             : 
     114           0 :     frame->dlc = size;
     115             : 
     116             :     /* Encode the CAN ID
     117             :      * 0x07mm20dd
     118             :      * - 07 = ACTUATOR group ID
     119             :      * - mm = Message ID
     120             :      * - 20 = ESC actuator type
     121             :      * - dd = Device ID
     122             :      *
     123             :      * Note: The Device ID (lower 8 bits of the frame ID) will have to be inserted later
     124             :      */
     125             : 
     126           0 :     uint32_t id = (((uint8_t) PiccoloCAN_MessageGroup::ACTUATOR) << 24) |   // CAN Group ID
     127           0 :                   ((packetID & 0xFF) << 16) |                               // Message ID
     128             :                   (((uint8_t) PiccoloCAN_DeviceType::ESC) << 8);            // Actuator type
     129             : 
     130             :     // Extended frame format
     131           0 :     id |= AP_HAL::CANFrame::FlagEFF;
     132             : 
     133           0 :     frame->id = id;
     134           0 : }
     135             : 
     136             : //! \return the size of a packet from the packet header
     137           0 : int getESCVelocityPacketSize(const void* pkt)
     138             : {
     139           0 :     AP_HAL::CANFrame* frame = (AP_HAL::CANFrame*) pkt;
     140             : 
     141           0 :     return (int) frame->dlc;
     142             : }
     143             : 
     144             : //! \return the ID of a packet from the packet header
     145           0 : uint32_t getESCVelocityPacketID(const void* pkt)
     146             : {
     147           0 :     AP_HAL::CANFrame* frame = (AP_HAL::CANFrame*) pkt;
     148             : 
     149             :     // Extract the message ID field from the 29-bit ID
     150           0 :     return (uint32_t) ((frame->id >> 16) & 0xFF);
     151             : }
     152             : 
     153             : #endif // AP_PICCOLOCAN_ENABLED