AP_PiccoloCAN: Framework for CAN servo outputs

libraries/AP_PiccoloCAN/AP_PiccoloCAN.cpp

@@ -69,6 +69,21 @@ const AP_Param::GroupInfo AP_PiccoloCAN::var_info[] = {
     // @Range: 1 500
     AP_GROUPINFO("ESC_RT", 2, AP_PiccoloCAN, _esc_hz, PICCOLO_MSG_RATE_HZ_DEFAULT),
 
+    // @Param: SRV_BM
+    // @DisplayName: Servo channels
+    // @Description: Bitmask defining which servo channels are to be transmitted over Piccolo CAN
+    // @Bitmask: 0: Servo 1, 1: Servo 2, 2: Servo 3, 3: Servo 4, 4: Servo 5, 5: Servo 6, 6: Servo 7, 7: Servo 8, 8: Servo 9, 9: Servo 10, 10: Servo 11, 11: Servo 12, 12: Servo 13, 13: Servo 14, 14: Servo 15, 15: Servo 16
+    // @User: Advanced
+    AP_GROUPINFO("SRV_BM", 3, AP_PiccoloCAN, _srv_bm, 0xFFFF),
+
+    // @Param: SRV_RT
+    // @DisplayName: Servo command output rate
+    // @Description: Output rate of servo command messages
+    // @Units: Hz
+    // @User: Advanced
+    // @Range: 1 500
+    AP_GROUPINFO("SRV_RT", 4, AP_PiccoloCAN, _srv_hz, PICCOLO_MSG_RATE_HZ_DEFAULT),
+
     AP_GROUPEND
 };
 
@@ -145,6 +160,7 @@ void AP_PiccoloCAN::loop()
     AP_HAL::CANFrame rxFrame;
 
     uint16_t esc_tx_counter = 0;
+    uint16_t servo_tx_counter = 0;
 
     // CAN Frame ID components
     uint8_t frame_id_group;     // Piccolo message group
@@ -152,16 +168,22 @@ void AP_PiccoloCAN::loop()
 
     while (true) {
 
-        _esc_hz = constrain_int16(_esc_hz, PICCOLO_MSG_RATE_HZ_MIN, PICCOLO_MSG_RATE_HZ_MAX);
-
-        uint16_t escCmdRateMs = (uint16_t) ((float) 1000 / _esc_hz);
-
         if (!_initialized) {
             debug_can(AP_CANManager::LOG_ERROR, "PiccoloCAN: not initialized\n\r");
             hal.scheduler->delay_microseconds(10000);
             continue;
         }
 
+        // Calculate the output rate for ESC commands
+        _esc_hz = constrain_int16(_esc_hz, PICCOLO_MSG_RATE_HZ_MIN, PICCOLO_MSG_RATE_HZ_MAX);
+
+        uint16_t escCmdRateMs = (uint16_t) ((float) 1000 / _esc_hz);
+
+        // Calculate the output rate for servo commands
+        _srv_hz = constrain_int16(_srv_hz, PICCOLO_MSG_RATE_HZ_MIN, PICCOLO_MSG_RATE_HZ_MAX);
+
+        uint16_t servoCmdRateMs = (uint16_t) ((float) 1000 / _srv_hz);
+
         uint64_t timeout = AP_HAL::micros64() + 250ULL;
 
         // 1ms loop delay
@@ -170,10 +192,15 @@ void AP_PiccoloCAN::loop()
         // Transmit ESC commands at regular intervals
         if (esc_tx_counter++ > escCmdRateMs) {
             esc_tx_counter = 0;
-
             send_esc_messages();
         }
 
+        // Transmit servo commands at regular intervals
+        if (servo_tx_counter++ > servoCmdRateMs) {
+            servo_tx_counter = 0;
+            send_servo_messages();
+        }
+
         // Look for any message responses on the CAN bus
         while (read_frame(rxFrame, timeout)) {
 
@@ -191,6 +218,11 @@ void AP_PiccoloCAN::loop()
             case MessageGroup::ACTUATOR:
 
                 switch (ActuatorType(frame_id_device)) {
+                case ActuatorType::SERVO:
+                    if (handle_servo_message(rxFrame)) {
+                        // Returns true if the message was successfully decoded
+                    }
+                    break;
                 case ActuatorType::ESC:
                     if (handle_esc_message(rxFrame)) {
                         // Returns true if the message was successfully decoded
@@ -254,6 +286,16 @@ bool AP_PiccoloCAN::read_frame(AP_HAL::CANFrame &recv_frame, uint64_t timeout)
 // called from SRV_Channels
 void AP_PiccoloCAN::update()
 {
+    uint64_t timestamp = AP_HAL::micros64();
+
+    /* Read out the servo commands from the channel mixer */
+    for (uint8_t ii = 0; ii < PICCOLO_CAN_MAX_NUM_SERVO; ii++) {
+
+        if (is_servo_channel_active(ii)) {
+            // TODO - Extract output command here
+        }
+    }
+
     /* Read out the ESC commands from the channel mixer */
     for (uint8_t i = 0; i < PICCOLO_CAN_MAX_NUM_ESC; i++) {
 
@@ -278,6 +320,8 @@ void AP_PiccoloCAN::update()
 
         WITH_SEMAPHORE(_telem_sem);
 
+        // TODO - Add servo log data here
+
         for (uint8_t i = 0; i < PICCOLO_CAN_MAX_NUM_ESC; i++) {
 
             PiccoloESC_Info_t &esc = _esc_info[i];
@@ -370,6 +414,75 @@ void AP_PiccoloCAN::send_esc_telemetry_mavlink(uint8_t mav_chan)
 }
 
 
+// send servo messages over CAN
+void AP_PiccoloCAN::send_servo_messages(void)
+{
+    AP_HAL::CANFrame txFrame;
+
+    uint64_t timeout = AP_HAL::micros64() + 1000ULL;
+
+    // No servos are selected? Don't send anything!
+    if (_srv_bm == 0x00) {
+        return;
+    }
+
+    bool send_cmd = false;
+    int16_t cmd[4] {};
+    uint8_t idx;
+
+    // Transmit bulk command packets to 4x servos simultaneously
+    for (uint8_t ii = 0; ii < PICCOLO_CAN_MAX_GROUP_SERVO; ii++) {
+
+        send_cmd = false;
+
+        for (uint8_t jj = 0; jj < 4; jj++) {
+            
+            idx = (ii * 4) + jj;
+
+            // Set default command value if an output field is unused
+            cmd[jj] = 0x7FFF;
+
+            // Skip servo if the output is not enabled
+            if (!is_servo_channel_active(idx)) {
+                continue;
+            }
+
+            /* Check if the servo is enabled.
+             * If it is not enabled, send an enable message.
+             */
+
+            if (!is_servo_present(idx) || !is_servo_enabled(idx)) {
+                // Servo is not enabled
+                encodeServo_EnablePacket(&txFrame);
+                txFrame.id |= (idx + 1);
+                write_frame(txFrame, timeout);
+            } else if (_servo_info[idx].newCommand) {
+                // A new command is provided
+                send_cmd = true;
+                cmd[jj] = _servo_info[idx].command;
+                _servo_info[idx].newCommand = false;
+            }
+        }
+
+        if (send_cmd) {
+            encodeServo_MultiPositionCommandPacket(
+                &txFrame,
+                cmd[0],
+                cmd[1],
+                cmd[2],
+                cmd[3],
+                (PKT_SERVO_MULTI_COMMAND_1 + ii)
+            );
+
+            // Broadcast the command to all servos
+            txFrame.id |= 0xFF;
+
+            write_frame(txFrame, timeout);
+        }
+    }
+}
+
+
 // send ESC messages over CAN
 void AP_PiccoloCAN::send_esc_messages(void)
 {
@@ -398,6 +511,9 @@ void AP_PiccoloCAN::send_esc_messages(void)
 
                 idx = (ii * 4) + jj;
 
+                // Set default command value if an output field is unused
+                cmd[jj] = 0x7FFF;
+
                 // Skip an ESC if the motor channel is not enabled
                 if (!is_esc_channel_active(idx)) {
                     continue;
@@ -416,8 +532,8 @@ void AP_PiccoloCAN::send_esc_messages(void)
                     cmd[jj] = _esc_info[idx].command;
                     _esc_info[idx].newCommand = false;
                 } else {
-                    // A command of 0xFFFF is 'out of range' and will be ignored by the corresponding ESC
-                    cmd[jj] = 0xFFFF;
+                    // A command of 0x7FFF is 'out of range' and will be ignored by the corresponding ESC
+                    cmd[jj] = 0x7FFF;
                 }
             }
 
@@ -452,6 +568,57 @@ void AP_PiccoloCAN::send_esc_messages(void)
 }
 
 
+// interpret a servo message received over CAN
+bool AP_PiccoloCAN::handle_servo_message(AP_HAL::CANFrame &frame)
+{
+    uint64_t timestamp = AP_HAL::micros64();
+
+    // The servo address is the lower byte of the frame ID
+    uint8_t addr = frame.id & 0xFF;
+
+    // Ignore servo with an invalid node ID
+    if (addr == 0x00) {
+        return false;
+    }
+
+    // Subtract to get the address in memory
+    addr -= 1;
+
+    // Maximum number of servos allowed
+    if (addr >= PICCOLO_CAN_MAX_NUM_SERVO) {
+        return false;
+    }
+
+    CBSServo_Info_t &servo = _servo_info[addr];
+
+    bool result = true;
+
+    // Throw the incoming packet against each decoding routine
+    if (decodeServo_StatusAPacketStructure(&frame, &servo.statusA)) {
+        servo.newTelemetry = true;
+    } else if (decodeServo_StatusBPacketStructure(&frame, &servo.statusB)) {
+        servo.newTelemetry = true;
+    } else if (decodeServo_FirmwarePacketStructure(&frame, &servo.firmware)) {
+        // TODO
+    } else if (decodeServo_AddressPacketStructure(&frame, &servo.address)) {
+        // TODO
+    } else if (decodeServo_SettingsInfoPacketStructure(&frame, &servo.settings)) {
+        // TODO
+    } else if (decodeServo_TelemetryConfigPacketStructure(&frame, &servo.telemetry)) {
+    } else {
+        // Incoming frame did not match any of the packet decoding routines
+        result = false;
+    }
+
+    if (result) {
+        // Reset the rx timestamp
+        servo.last_rx_msg_timestamp = timestamp;
+    }
+
+    return result;
+}
+
+
 // interpret an ESC message received over CAN
 bool AP_PiccoloCAN::handle_esc_message(AP_HAL::CANFrame &frame)
 {
@@ -460,7 +627,7 @@ bool AP_PiccoloCAN::handle_esc_message(AP_HAL::CANFrame &frame)
 #if HAL_WITH_ESC_TELEM
     uint64_t timestamp = AP_HAL::micros64();
 
-    // The ESC address is the lower byte of the address
+    // The ESC address is the lower byte of the frame ID
     uint8_t addr = frame.id & 0xFF;
 
     // Ignore any ESC with node ID of zero
@@ -555,7 +722,27 @@ bool AP_PiccoloCAN::handle_esc_message(AP_HAL::CANFrame &frame)
 }
 
 /**
- * Check if a given ESC channel is "active" (has been configured correctly)
+ * Check if a given servo channel is "active" (has been configured for Piccolo control output)
+ */
+bool AP_PiccoloCAN::is_servo_channel_active(uint8_t chan)
+{
+    // First check if the particular servo channel is enabled in the channel mask
+    if (((_srv_bm >> chan) & 0x01) == 0x00) {
+        return false;
+    }
+
+    // Check if a servo function is assigned for this motor channel
+    // TODO - ?
+    if (1) {
+        return true;
+    }
+
+    return false;
+
+}
+
+/**
+ * Check if a given ESC channel is "active" (has been configured for Piccolo control output)
  */
 bool AP_PiccoloCAN::is_esc_channel_active(uint8_t chan)
 {
@@ -575,6 +762,34 @@ bool AP_PiccoloCAN::is_esc_channel_active(uint8_t chan)
 }
 
 
+/**
+ * Determine if a servo is present on the CAN bus (has telemetry data been received)
+ */
+bool AP_PiccoloCAN::is_servo_present(uint8_t chan, uint64_t timeout_ms)
+{
+    if (chan >= PICCOLO_CAN_MAX_NUM_SERVO) {
+        return false;
+    }
+
+    CBSServo_Info_t &servo = _servo_info[chan];
+
+    // No messages received from this servo
+    if (servo.last_rx_msg_timestamp == 0) {
+        return false;
+    }
+
+    uint64_t now = AP_HAL::micros64();
+
+    uint64_t timeout_us = timeout_ms * 1000ULL;
+
+    if (now > (servo.last_rx_msg_timestamp + timeout_us)) {
+        return false;
+    }
+
+    return true;
+}
+
+
 /**
  * Determine if an ESC is present on the CAN bus (has telemetry data been received)
  */
@@ -603,6 +818,26 @@ bool AP_PiccoloCAN::is_esc_present(uint8_t chan, uint64_t timeout_ms) const
 }
 
 
+/**
+ * Check if a given servo is enabled
+ */
+bool AP_PiccoloCAN::is_servo_enabled(uint8_t chan)
+{
+    if (chan >= PICCOLO_CAN_MAX_NUM_SERVO) {
+        return false;
+    }
+
+    // If the servo is not present, we cannot determine if it is enabled or not
+    if (!is_servo_present(chan)) {
+        return false;
+    }
+
+    CBSServo_Info_t &servo = _servo_info[chan];
+
+    return servo.statusA.status.enabled;
+}
+
+
 /**
  * Check if a given ESC is enabled (both hardware and software enable flags)
  */
@@ -631,6 +866,18 @@ bool AP_PiccoloCAN::is_esc_enabled(uint8_t chan)
 
 bool AP_PiccoloCAN::pre_arm_check(char* reason, uint8_t reason_len)
 {
+    // Check that each required servo is present on the bus
+    for (uint8_t ii = 0; ii < PICCOLO_CAN_MAX_NUM_SERVO; ii++) {
+
+        if (is_servo_channel_active(ii)) {
+
+            if (!is_servo_present(ii)) {
+                snprintf(reason, reason_len, "Servo %u not detected", ii + 1);
+                return false;
+            }
+        }
+    }
+
     // Check that each required ESC is present on the bus
     for (uint8_t ii = 0; ii < PICCOLO_CAN_MAX_NUM_ESC; ii++) {
 

libraries/AP_PiccoloCAN/AP_PiccoloCAN.h

@@ -84,11 +84,23 @@ public:
     // called from SRV_Channels
     void update();
 
+    // send ESC telemetry messages over MAVLink
+    void send_esc_telemetry_mavlink(uint8_t mav_chan);
+
+    // return true if a particular servo is 'active' on the Piccolo interface
+    bool is_servo_channel_active(uint8_t chan);
+
     // return true if a particular ESC is 'active' on the Piccolo interface
     bool is_esc_channel_active(uint8_t chan);
 
-    // return true if a particular ESC has been detected
-    bool is_esc_present(uint8_t chan, uint64_t timeout_ms = 2000) const;
+    // return true if a particular servo has been detected on the CAN interface
+    bool is_servo_present(uint8_t chan, uint64_t timeout_ms = 2000);
+
+    // return true if a particular ESC has been detected on the CAN interface
+    bool is_esc_present(uint8_t chan, uint64_t timeout_ms = 2000);
+
+    // return true if a particular servo is enabled
+    bool is_servo_enabled(uint8_t chan);
 
     // return true if a particular ESC is enabled
     bool is_esc_enabled(uint8_t chan);
@@ -113,6 +125,12 @@ private:
     // interpret an ESC message received over CAN
     bool handle_esc_message(AP_HAL::CANFrame &frame);
 
+    // send servo commands over CAN
+    void send_servo_messages(void);
+
+    // interpret a servo message received over CAN
+    bool handle_servo_message(AP_HAL::CANFrame &frame);
+
     bool _initialized;
     char _thread_name[16];
     uint8_t _driver_index;
@@ -131,7 +149,15 @@ private:
         Servo_Address_t address;
         Servo_SettingsInfo_t settings;
         Servo_SystemInfo_t systemInfo;
-        Servo_TelemetrySettings_t telemetry;
+        Servo_TelemetryConfig_t telemetry;
+
+        /* Internal state information */
+
+        int16_t command;    //! Raw command to send to each servo
+        bool newCommand;    //! Is the command "new"?
+        bool newTelemetry;  //! Is there new telemetry data available?
+
+        uint64_t last_rx_msg_timestamp = 0; //! Time of most recently received message
 
     } _servo_info[PICCOLO_CAN_MAX_NUM_SERVO];
 
@@ -178,6 +204,8 @@ private:
     AP_Int32 _esc_bm;       //! ESC selection bitmask
     AP_Int16 _esc_hz;       //! ESC update rate (Hz)
 
+    AP_Int32 _srv_bm;       //! Servo selection bitmask
+    AP_Int16 _srv_hz;       //! Servo update rate (Hz)
 };
 
 #endif // HAL_PICCOLO_CAN_ENABLE

libraries/AP_PiccoloCAN/piccolo_protocol/ServoPackets.c

@@ -261,19 +261,19 @@ int decodeServo_Config_t(const uint8_t* _pg_data, int* _pg_bytecount, Servo_Conf
 /*!
  * \brief Create the Servo_MultiPositionCommand packet
  *
- * This command can be used to command multiple servos (1 to 4) with a single
- * CAN frame). The addresses of the targetted servos must be sequential, with
- * the initial address based on the packet ID. The example packet shown below
- * corresponds to servos with IDs {1, 2, 3, 4}. For other addresses refer to
- * the *ServoMultiCommandPackets* enumeration. Note: The CAN frame must be sent
- * to the broadcast ID (0xFF) for all servos to receive this message.
+ * This packet can be used to simultaneously command multiple servos which have
+ * sequential CAN ID values. This packet must be sent as a broadcast packet
+ * (address = 0xFF) such that all servos can receive it. These commands can be
+ * sent to groups of servos with ID values up to 64, using different
+ * PKT_SERVO_MULTI_COMMAND_x packet ID values.
  * \param _pg_pkt points to the packet which will be created by this function
  * \param commandA is Servo command for servo with address offset 0
  * \param commandB is Servo command for servo with address offset 1
  * \param commandC is Servo command for servo with address offset 3
  * \param commandD is Servo command for servo with address offset 3
+ * \param id is the packet identifier for _pg_pkt
  */
-void encodeServo_MultiPositionCommandPacket(void* _pg_pkt, int16_t commandA, int16_t commandB, int16_t commandC, int16_t commandD)
+void encodeServo_MultiPositionCommandPacket(void* _pg_pkt, int16_t commandA, int16_t commandB, int16_t commandC, int16_t commandD, uint32_t _pg_id)
 {
     uint8_t* _pg_data = getServoPacketData(_pg_pkt);
     int _pg_byteindex = 0;
@@ -295,19 +295,18 @@ void encodeServo_MultiPositionCommandPacket(void* _pg_pkt, int16_t commandA, int
     int16ToBeBytes(commandD, _pg_data, &_pg_byteindex);
 
     // complete the process of creating the packet
-    finishServoPacket(_pg_pkt, _pg_byteindex, getServo_MultiPositionCommandPacketID());
+    finishServoPacket(_pg_pkt, _pg_byteindex, _pg_id);
 
 }// encodeServo_MultiPositionCommandPacket
 
 /*!
  * \brief Decode the Servo_MultiPositionCommand packet
  *
- * This command can be used to command multiple servos (1 to 4) with a single
- * CAN frame). The addresses of the targetted servos must be sequential, with
- * the initial address based on the packet ID. The example packet shown below
- * corresponds to servos with IDs {1, 2, 3, 4}. For other addresses refer to
- * the *ServoMultiCommandPackets* enumeration. Note: The CAN frame must be sent
- * to the broadcast ID (0xFF) for all servos to receive this message.
+ * This packet can be used to simultaneously command multiple servos which have
+ * sequential CAN ID values. This packet must be sent as a broadcast packet
+ * (address = 0xFF) such that all servos can receive it. These commands can be
+ * sent to groups of servos with ID values up to 64, using different
+ * PKT_SERVO_MULTI_COMMAND_x packet ID values.
  * \param _pg_pkt points to the packet being decoded by this function
  * \param commandA receives Servo command for servo with address offset 0
  * \param commandB receives Servo command for servo with address offset 1
@@ -321,8 +320,24 @@ int decodeServo_MultiPositionCommandPacket(const void* _pg_pkt, int16_t* command
     const uint8_t* _pg_data = getServoPacketDataConst(_pg_pkt);
     int _pg_numbytes = getServoPacketSize(_pg_pkt);
 
-    // Verify the packet identifier
-    if(getServoPacketID(_pg_pkt) != getServo_MultiPositionCommandPacketID())
+    // Verify the packet identifier, multiple options exist
+    uint32_t packetid = getServoPacketID(_pg_pkt);
+    if( packetid != PKT_SERVO_MULTI_COMMAND_1 &&
+        packetid != PKT_SERVO_MULTI_COMMAND_2 &&
+        packetid != PKT_SERVO_MULTI_COMMAND_3 &&
+        packetid != PKT_SERVO_MULTI_COMMAND_4 &&
+        packetid != PKT_SERVO_MULTI_COMMAND_5 &&
+        packetid != PKT_SERVO_MULTI_COMMAND_6 &&
+        packetid != PKT_SERVO_MULTI_COMMAND_7 &&
+        packetid != PKT_SERVO_MULTI_COMMAND_8 &&
+        packetid != PKT_SERVO_MULTI_COMMAND_9 &&
+        packetid != PKT_SERVO_MULTI_COMMAND_10 &&
+        packetid != PKT_SERVO_MULTI_COMMAND_11 &&
+        packetid != PKT_SERVO_MULTI_COMMAND_12 &&
+        packetid != PKT_SERVO_MULTI_COMMAND_13 &&
+        packetid != PKT_SERVO_MULTI_COMMAND_14 &&
+        packetid != PKT_SERVO_MULTI_COMMAND_15 &&
+        packetid != PKT_SERVO_MULTI_COMMAND_16 )
         return 0;
 
     if(_pg_numbytes < getServo_MultiPositionCommandMinDataLength())
@@ -555,6 +570,92 @@ int decodeServo_SetTitlePacket(const void* _pg_pkt, uint8_t title[8])
 
 }// decodeServo_SetTitlePacket
 
+/*!
+ * \brief Create the Servo_StatusA packet
+ *
+ * The *SERVO_STATUS_A* packet contains status, warning and error information,
+ * in addition to the servo position
+ * \param _pg_pkt points to the packet which will be created by this function
+ * \param _pg_user points to the user data that will be encoded in _pg_pkt
+ */
+void encodeServo_StatusAPacketStructure(void* _pg_pkt, const Servo_StatusA_t* _pg_user)
+{
+    uint8_t* _pg_data = getServoPacketData(_pg_pkt);
+    int _pg_byteindex = 0;
+
+    // Status bits contain information on servo operation
+    encodeServo_StatusBits_t(_pg_data, &_pg_byteindex, &_pg_user->status);
+
+    // Warning bits indicate servo is operation outside of desired range
+    encodeServo_WarningBits_t(_pg_data, &_pg_byteindex, &_pg_user->warnings);
+
+    // These bits indicate critical system error information
+    encodeServo_ErrorBits_t(_pg_data, &_pg_byteindex, &_pg_user->errors);
+
+    // Servo position, mapped to input units
+    // Range of position is -32768 to 32767.
+    int16ToBeBytes(_pg_user->position, _pg_data, &_pg_byteindex);
+
+    // Servo commanded position
+    // Range of command is -32768 to 32767.
+    int16ToBeBytes(_pg_user->command, _pg_data, &_pg_byteindex);
+
+    // complete the process of creating the packet
+    finishServoPacket(_pg_pkt, _pg_byteindex, getServo_StatusAPacketID());
+
+}// encodeServo_StatusAPacketStructure
+
+/*!
+ * \brief Decode the Servo_StatusA packet
+ *
+ * The *SERVO_STATUS_A* packet contains status, warning and error information,
+ * in addition to the servo position
+ * \param _pg_pkt points to the packet being decoded by this function
+ * \param _pg_user receives the data decoded from the packet
+ * \return 0 is returned if the packet ID or size is wrong, else 1
+ */
+int decodeServo_StatusAPacketStructure(const void* _pg_pkt, Servo_StatusA_t* _pg_user)
+{
+    int _pg_numbytes;
+    int _pg_byteindex = 0;
+    const uint8_t* _pg_data;
+
+    // Verify the packet identifier
+    if(getServoPacketID(_pg_pkt) != getServo_StatusAPacketID())
+        return 0;
+
+    // Verify the packet size
+    _pg_numbytes = getServoPacketSize(_pg_pkt);
+    if(_pg_numbytes < getServo_StatusAMinDataLength())
+        return 0;
+
+    // The raw data from the packet
+    _pg_data = getServoPacketDataConst(_pg_pkt);
+
+    // Status bits contain information on servo operation
+    if(decodeServo_StatusBits_t(_pg_data, &_pg_byteindex, &_pg_user->status) == 0)
+        return 0;
+
+    // Warning bits indicate servo is operation outside of desired range
+    if(decodeServo_WarningBits_t(_pg_data, &_pg_byteindex, &_pg_user->warnings) == 0)
+        return 0;
+
+    // These bits indicate critical system error information
+    if(decodeServo_ErrorBits_t(_pg_data, &_pg_byteindex, &_pg_user->errors) == 0)
+        return 0;
+
+    // Servo position, mapped to input units
+    // Range of position is -32768 to 32767.
+    _pg_user->position = int16FromBeBytes(_pg_data, &_pg_byteindex);
+
+    // Servo commanded position
+    // Range of command is -32768 to 32767.
+    _pg_user->command = int16FromBeBytes(_pg_data, &_pg_byteindex);
+
+    return 1;
+
+}// decodeServo_StatusAPacketStructure
+
 /*!
  * \brief Create the Servo_StatusA packet
  *
@@ -644,6 +745,77 @@ int decodeServo_StatusAPacket(const void* _pg_pkt, Servo_StatusBits_t* status, S
 
 }// decodeServo_StatusAPacket
 
+/*!
+ * \brief Create the Servo_StatusB packet
+ *
+ * The *SERVO_STATUS_B* packet contains various servo feedback data
+ * \param _pg_pkt points to the packet which will be created by this function
+ * \param _pg_user points to the user data that will be encoded in _pg_pkt
+ */
+void encodeServo_StatusBPacketStructure(void* _pg_pkt, const Servo_StatusB_t* _pg_user)
+{
+    uint8_t* _pg_data = getServoPacketData(_pg_pkt);
+    int _pg_byteindex = 0;
+
+    // Servo current
+    // Range of current is 0 to 65535.
+    uint16ToBeBytes(_pg_user->current, _pg_data, &_pg_byteindex);
+
+    // Servo supply voltage
+    // Range of voltage is 0 to 65535.
+    uint16ToBeBytes(_pg_user->voltage, _pg_data, &_pg_byteindex);
+
+    // Servo temperature
+    // Range of temperature is -128 to 127.
+    int8ToBytes(_pg_user->temperature, _pg_data, &_pg_byteindex);
+
+    // complete the process of creating the packet
+    finishServoPacket(_pg_pkt, _pg_byteindex, getServo_StatusBPacketID());
+
+}// encodeServo_StatusBPacketStructure
+
+/*!
+ * \brief Decode the Servo_StatusB packet
+ *
+ * The *SERVO_STATUS_B* packet contains various servo feedback data
+ * \param _pg_pkt points to the packet being decoded by this function
+ * \param _pg_user receives the data decoded from the packet
+ * \return 0 is returned if the packet ID or size is wrong, else 1
+ */
+int decodeServo_StatusBPacketStructure(const void* _pg_pkt, Servo_StatusB_t* _pg_user)
+{
+    int _pg_numbytes;
+    int _pg_byteindex = 0;
+    const uint8_t* _pg_data;
+
+    // Verify the packet identifier
+    if(getServoPacketID(_pg_pkt) != getServo_StatusBPacketID())
+        return 0;
+
+    // Verify the packet size
+    _pg_numbytes = getServoPacketSize(_pg_pkt);
+    if(_pg_numbytes < getServo_StatusBMinDataLength())
+        return 0;
+
+    // The raw data from the packet
+    _pg_data = getServoPacketDataConst(_pg_pkt);
+
+    // Servo current
+    // Range of current is 0 to 65535.
+    _pg_user->current = uint16FromBeBytes(_pg_data, &_pg_byteindex);
+
+    // Servo supply voltage
+    // Range of voltage is 0 to 65535.
+    _pg_user->voltage = uint16FromBeBytes(_pg_data, &_pg_byteindex);
+
+    // Servo temperature
+    // Range of temperature is -128 to 127.
+    _pg_user->temperature = int8FromBytes(_pg_data, &_pg_byteindex);
+
+    return 1;
+
+}// decodeServo_StatusBPacketStructure
+
 /*!
  * \brief Create the Servo_StatusB packet
  *
@@ -714,6 +886,63 @@ int decodeServo_StatusBPacket(const void* _pg_pkt, uint16_t* current, uint16_t*
 
 }// decodeServo_StatusBPacket
 
+/*!
+ * \brief Create the Servo_StatusC packet
+ *
+ * The *SERVO_STATUS_C* packet contains servo position data. It is a cut-down
+ * packet to allow high-speed feedback on servo position
+ * \param _pg_pkt points to the packet which will be created by this function
+ * \param _pg_user points to the user data that will be encoded in _pg_pkt
+ */
+void encodeServo_StatusCPacketStructure(void* _pg_pkt, const Servo_StatusC_t* _pg_user)
+{
+    uint8_t* _pg_data = getServoPacketData(_pg_pkt);
+    int _pg_byteindex = 0;
+
+    // Servo position, mapped to input units
+    // Range of position is -32768 to 32767.
+    int16ToBeBytes(_pg_user->position, _pg_data, &_pg_byteindex);
+
+    // complete the process of creating the packet
+    finishServoPacket(_pg_pkt, _pg_byteindex, getServo_StatusCPacketID());
+
+}// encodeServo_StatusCPacketStructure
+
+/*!
+ * \brief Decode the Servo_StatusC packet
+ *
+ * The *SERVO_STATUS_C* packet contains servo position data. It is a cut-down
+ * packet to allow high-speed feedback on servo position
+ * \param _pg_pkt points to the packet being decoded by this function
+ * \param _pg_user receives the data decoded from the packet
+ * \return 0 is returned if the packet ID or size is wrong, else 1
+ */
+int decodeServo_StatusCPacketStructure(const void* _pg_pkt, Servo_StatusC_t* _pg_user)
+{
+    int _pg_numbytes;
+    int _pg_byteindex = 0;
+    const uint8_t* _pg_data;
+
+    // Verify the packet identifier
+    if(getServoPacketID(_pg_pkt) != getServo_StatusCPacketID())
+        return 0;
+
+    // Verify the packet size
+    _pg_numbytes = getServoPacketSize(_pg_pkt);
+    if(_pg_numbytes < getServo_StatusCMinDataLength())
+        return 0;
+
+    // The raw data from the packet
+    _pg_data = getServoPacketDataConst(_pg_pkt);
+
+    // Servo position, mapped to input units
+    // Range of position is -32768 to 32767.
+    _pg_user->position = int16FromBeBytes(_pg_data, &_pg_byteindex);
+
+    return 1;
+
+}// decodeServo_StatusCPacketStructure
+
 /*!
  * \brief Create the Servo_StatusC packet
  *
@@ -1421,6 +1650,75 @@ int decodeServo_SystemInfoPacket(const void* _pg_pkt, uint32_t* msSinceReset, ui
 
 }// decodeServo_SystemInfoPacket
 
+/*!
+ * \brief Create the Servo_TelemetryConfig packet
+ *
+ * Telemetry settings configuration packet
+ * \param _pg_pkt points to the packet which will be created by this function
+ * \param _pg_user points to the user data that will be encoded in _pg_pkt
+ */
+void encodeServo_TelemetryConfigPacketStructure(void* _pg_pkt, const Servo_TelemetryConfig_t* _pg_user)
+{
+    uint8_t* _pg_data = getServoPacketData(_pg_pkt);
+    int _pg_byteindex = 0;
+    unsigned _pg_i = 0;
+
+    // Servo telemetry settings
+    encodeServo_TelemetrySettings_t(_pg_data, &_pg_byteindex, &_pg_user->settings);
+
+    // Reserved for future use
+    for(_pg_i = 0; _pg_i < 3; _pg_i++)
+        uint8ToBytes((uint8_t)(0), _pg_data, &_pg_byteindex);
+
+    // Servo ICD revision
+    uint8ToBytes((uint8_t)(getServoApi()), _pg_data, &_pg_byteindex);
+
+    // complete the process of creating the packet
+    finishServoPacket(_pg_pkt, _pg_byteindex, getServo_TelemetryConfigPacketID());
+
+}// encodeServo_TelemetryConfigPacketStructure
+
+/*!
+ * \brief Decode the Servo_TelemetryConfig packet
+ *
+ * Telemetry settings configuration packet
+ * \param _pg_pkt points to the packet being decoded by this function
+ * \param _pg_user receives the data decoded from the packet
+ * \return 0 is returned if the packet ID or size is wrong, else 1
+ */
+int decodeServo_TelemetryConfigPacketStructure(const void* _pg_pkt, Servo_TelemetryConfig_t* _pg_user)
+{
+    int _pg_numbytes;
+    int _pg_byteindex = 0;
+    const uint8_t* _pg_data;
+
+    // Verify the packet identifier
+    if(getServoPacketID(_pg_pkt) != getServo_TelemetryConfigPacketID())
+        return 0;
+
+    // Verify the packet size
+    _pg_numbytes = getServoPacketSize(_pg_pkt);
+    if(_pg_numbytes < getServo_TelemetryConfigMinDataLength())
+        return 0;
+
+    // The raw data from the packet
+    _pg_data = getServoPacketDataConst(_pg_pkt);
+
+    // Servo telemetry settings
+    if(decodeServo_TelemetrySettings_t(_pg_data, &_pg_byteindex, &_pg_user->settings) == 0)
+        return 0;
+
+    // Reserved for future use
+    _pg_byteindex += 1*3;
+
+    // Servo ICD revision
+    // Range of icdVersion is 0 to 255.
+    _pg_user->icdVersion = uint8FromBytes(_pg_data, &_pg_byteindex);
+
+    return 1;
+
+}// decodeServo_TelemetryConfigPacketStructure
+
 /*!
  * \brief Create the Servo_TelemetryConfig packet
  *

libraries/AP_PiccoloCAN/piccolo_protocol/ServoPackets.h

@@ -72,12 +72,11 @@ int decodeServo_Config_t(const uint8_t* data, int* bytecount, Servo_Config_t* us
 #define getServo_ConfigMaxDataLength() (8)
 
 /*!
- * This command can be used to command multiple servos (1 to 4) with a single
- * CAN frame). The addresses of the targetted servos must be sequential, with
- * the initial address based on the packet ID. The example packet shown below
- * corresponds to servos with IDs {1, 2, 3, 4}. For other addresses refer to
- * the *ServoMultiCommandPackets* enumeration. Note: The CAN frame must be sent
- * to the broadcast ID (0xFF) for all servos to receive this message.
+ * This packet can be used to simultaneously command multiple servos which have
+ * sequential CAN ID values. This packet must be sent as a broadcast packet
+ * (address = 0xFF) such that all servos can receive it. These commands can be
+ * sent to groups of servos with ID values up to 64, using different
+ * PKT_SERVO_MULTI_COMMAND_x packet ID values.
  */
 typedef struct
 {
@@ -88,14 +87,11 @@ typedef struct
 }Servo_MultiPositionCommand_t;
 
 //! Create the Servo_MultiPositionCommand packet from parameters
-void encodeServo_MultiPositionCommandPacket(void* pkt, int16_t commandA, int16_t commandB, int16_t commandC, int16_t commandD);
+void encodeServo_MultiPositionCommandPacket(void* pkt, int16_t commandA, int16_t commandB, int16_t commandC, int16_t commandD, uint32_t id);
 
 //! Decode the Servo_MultiPositionCommand packet to parameters
 int decodeServo_MultiPositionCommandPacket(const void* pkt, int16_t* commandA, int16_t* commandB, int16_t* commandC, int16_t* commandD);
 
-//! return the packet ID for the Servo_MultiPositionCommand packet
-#define getServo_MultiPositionCommandPacketID() (PKT_SERVO_MULTI_COMMAND_1)
-
 //! return the minimum encoded length for the Servo_MultiPositionCommand packet
 #define getServo_MultiPositionCommandMinDataLength() (8)
 
@@ -208,6 +204,12 @@ typedef struct
     int16_t             command;  //!< Servo commanded position
 }Servo_StatusA_t;
 
+//! Create the Servo_StatusA packet
+void encodeServo_StatusAPacketStructure(void* pkt, const Servo_StatusA_t* user);
+
+//! Decode the Servo_StatusA packet
+int decodeServo_StatusAPacketStructure(const void* pkt, Servo_StatusA_t* user);
+
 //! Create the Servo_StatusA packet from parameters
 void encodeServo_StatusAPacket(void* pkt, const Servo_StatusBits_t* status, const Servo_WarningBits_t* warnings, const Servo_ErrorBits_t* errors, int16_t position, int16_t command);
 
@@ -233,6 +235,12 @@ typedef struct
     int8_t   temperature; //!< Servo temperature
 }Servo_StatusB_t;
 
+//! Create the Servo_StatusB packet
+void encodeServo_StatusBPacketStructure(void* pkt, const Servo_StatusB_t* user);
+
+//! Decode the Servo_StatusB packet
+int decodeServo_StatusBPacketStructure(const void* pkt, Servo_StatusB_t* user);
+
 //! Create the Servo_StatusB packet from parameters
 void encodeServo_StatusBPacket(void* pkt, uint16_t current, uint16_t voltage, int8_t temperature);
 
@@ -257,6 +265,12 @@ typedef struct
     int16_t position; //!< Servo position, mapped to input units
 }Servo_StatusC_t;
 
+//! Create the Servo_StatusC packet
+void encodeServo_StatusCPacketStructure(void* pkt, const Servo_StatusC_t* user);
+
+//! Decode the Servo_StatusC packet
+int decodeServo_StatusCPacketStructure(const void* pkt, Servo_StatusC_t* user);
+
 //! Create the Servo_StatusC packet from parameters
 void encodeServo_StatusCPacket(void* pkt, int16_t position);
 
@@ -430,6 +444,12 @@ typedef struct
     uint8_t                   icdVersion; //!< Servo ICD revision. Field is encoded constant.
 }Servo_TelemetryConfig_t;
 
+//! Create the Servo_TelemetryConfig packet
+void encodeServo_TelemetryConfigPacketStructure(void* pkt, const Servo_TelemetryConfig_t* user);
+
+//! Decode the Servo_TelemetryConfig packet
+int decodeServo_TelemetryConfigPacketStructure(const void* pkt, Servo_TelemetryConfig_t* user);
+
 //! Create the Servo_TelemetryConfig packet from parameters
 void encodeServo_TelemetryConfigPacket(void* pkt, const Servo_TelemetrySettings_t* settings);