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_Servo.h"
      23             : 
      24             : /*
      25             :  * Decode a recevied CAN frame.
      26             :  * It is assumed at this point that the received frame is intended for *this* servo
      27             :  */
      28           0 : bool AP_PiccoloCAN_Servo::handle_can_frame(AP_HAL::CANFrame &frame)
      29             : {
      30           0 :     bool result = true;
      31             : 
      32           0 :     if (decodeServo_StatusAPacketStructure(&frame, &status.statusA)) {
      33           0 :         newTelemetry = true;
      34           0 :     } else if (decodeServo_StatusBPacketStructure(&frame, &status.statusB)) {
      35           0 :         newTelemetry = true;
      36           0 :     } else if (decodeServo_FirmwarePacketStructure(&frame, &settings.firmware)) {
      37           0 :     } else if (decodeServo_AddressPacketStructure(&frame, &settings.address)) {
      38           0 :     } else if (decodeServo_SettingsInfoPacketStructure(&frame, &settings.settings)) {
      39             :     } else {
      40             :         // Incoming frame did not match any packet decoding routine
      41           0 :         result = false;
      42             :     }
      43             : 
      44           0 :     if (result) {
      45           0 :         reset_rx_timestamp();
      46             :     }
      47             : 
      48           0 :     return result;
      49             : }
      50             : 
      51             : 
      52             : /* Piccolo Glue Logic
      53             :  * The following functions are required by the auto-generated protogen code.
      54             :  */
      55             : 
      56             : //! \return the packet data pointer from the packet
      57           0 : uint8_t* getServoPacketData(void* pkt)
      58             : {
      59           0 :     AP_HAL::CANFrame* frame = (AP_HAL::CANFrame*) pkt;
      60             : 
      61           0 :     return (uint8_t*) frame->data;
      62             : }
      63             : 
      64             : //! \return the packet data pointer from the packet, const
      65           0 : const uint8_t* getServoPacketDataConst(const void* pkt)
      66             : {
      67           0 :     AP_HAL::CANFrame* frame = (AP_HAL::CANFrame*) pkt;
      68             : 
      69           0 :     return (const uint8_t*) frame->data;
      70             : }
      71             : 
      72             : //! Complete a packet after the data have been encoded
      73           0 : void finishServoPacket(void* pkt, int size, uint32_t packetID)
      74             : {
      75           0 :     AP_HAL::CANFrame* frame = (AP_HAL::CANFrame*) pkt;
      76             : 
      77           0 :     if (size > AP_HAL::CANFrame::MaxDataLen) {
      78           0 :         size = AP_HAL::CANFrame::MaxDataLen;
      79             :     }
      80             : 
      81           0 :     frame->dlc = size;
      82             : 
      83             :     /* Encode the CAN ID
      84             :      * 0x07mm20dd
      85             :      * - 07 = ACTUATOR group ID
      86             :      * - mm = Message ID
      87             :      * - 00 = Servo actuator type
      88             :      * - dd = Device ID
      89             :      *
      90             :      * Note: The Device ID (lower 8 bits of the frame ID) will have to be inserted later
      91             :      */
      92             : 
      93           0 :     uint32_t id = (((uint8_t) PiccoloCAN_MessageGroup::ACTUATOR) << 24) |   // CAN Group ID
      94           0 :                   ((packetID & 0xFF) << 16) |                               // Message ID
      95             :                   (((uint8_t) PiccoloCAN_DeviceType::SERVO) << 8);          // Actuator type
      96             : 
      97             :     // Extended frame format
      98           0 :     id |= AP_HAL::CANFrame::FlagEFF;
      99             : 
     100           0 :     frame->id = id;
     101           0 : }
     102             : 
     103             : //! \return the size of a packet from the packet header
     104           0 : int getServoPacketSize(const void* pkt)
     105             : {
     106           0 :     AP_HAL::CANFrame* frame = (AP_HAL::CANFrame*) pkt;
     107             : 
     108           0 :     return (int) frame->dlc;
     109             : }
     110             : 
     111             : //! \return the ID of a packet from the packet header
     112           0 : uint32_t getServoPacketID(const void* pkt)
     113             : {
     114           0 :     AP_HAL::CANFrame* frame = (AP_HAL::CANFrame*) pkt;
     115             : 
     116             :     // Extract the message ID field from the 29-bit ID
     117           0 :     return (uint32_t) ((frame->id >> 16) & 0xFF);
     118             : }
     119             : 
     120             : #endif // AP_PICCOLOCAN_ENABLED