AP_PiccoloCAN: Record servo telemetry information to log

- Servo position
- Servo speed
- Servo force / torque
- Servo duty cycle

Updated servo protocol file to accommodate extra variable data

libraries/AP_PiccoloCAN/AP_PiccoloCAN.cpp

@@ -328,7 +328,23 @@ void AP_PiccoloCAN::update()
 
         WITH_SEMAPHORE(_telem_sem);
 
-        // TODO - Add servo log data here
+        for (uint8_t ii = 0; ii < PICCOLO_CAN_MAX_NUM_SERVO; ii++) {
+            CBSServo_Info_t &servo = _servo_info[ii];
+
+            if (servo.newTelemetry) {
+
+                logger->Write_ServoStatus(
+                    timestamp,
+                    ii,
+                    (float) servo.statusA.position,         // Servo position (represented in microsecond units)
+                    (float) servo.statusB.current / 100.0f, // Servo force (actually servo current, 0.01A per bit)
+                    (float) servo.statusB.speed,            // Servo speed (degrees per second)
+                    (uint8_t) abs(servo.statusB.dutyCycle)  // Servo duty cycle (absolute value as it can be +/- 100%)
+                );
+
+                servo.newTelemetry = false;
+            }
+        }
 
         for (uint8_t i = 0; i < PICCOLO_CAN_MAX_NUM_ESC; i++) {
 

libraries/AP_PiccoloCAN/piccolo_protocol/ServoPackets.c

@@ -769,6 +769,14 @@ void encodeServo_StatusBPacketStructure(void* _pg_pkt, const Servo_StatusB_t* _p
     // Range of temperature is -128 to 127.
     int8ToBytes(_pg_user->temperature, _pg_data, &_pg_byteindex);
 
+    // Motor duty cycle
+    // Range of dutyCycle is -128 to 127.
+    int8ToBytes(_pg_user->dutyCycle, _pg_data, &_pg_byteindex);
+
+    // Servo output shaft speed
+    // Range of speed is -32768 to 32767.
+    int16ToBeBytes(_pg_user->speed, _pg_data, &_pg_byteindex);
+
     // complete the process of creating the packet
     finishServoPacket(_pg_pkt, _pg_byteindex, getServo_StatusBPacketID());
 
@@ -800,6 +808,10 @@ int decodeServo_StatusBPacketStructure(const void* _pg_pkt, Servo_StatusB_t* _pg
     // The raw data from the packet
     _pg_data = getServoPacketDataConst(_pg_pkt);
 
+    // this packet has default fields, make sure they are set
+    _pg_user->dutyCycle = 0;
+    _pg_user->speed = 0;
+
     // Servo current
     // Range of current is 0 to 65535.
     _pg_user->current = uint16FromBeBytes(_pg_data, &_pg_byteindex);
@@ -812,6 +824,20 @@ int decodeServo_StatusBPacketStructure(const void* _pg_pkt, Servo_StatusB_t* _pg
     // Range of temperature is -128 to 127.
     _pg_user->temperature = int8FromBytes(_pg_data, &_pg_byteindex);
 
+    if(_pg_byteindex + 1 > _pg_numbytes)
+        return 1;
+
+    // Motor duty cycle
+    // Range of dutyCycle is -128 to 127.
+    _pg_user->dutyCycle = int8FromBytes(_pg_data, &_pg_byteindex);
+
+    if(_pg_byteindex + 2 > _pg_numbytes)
+        return 1;
+
+    // Servo output shaft speed
+    // Range of speed is -32768 to 32767.
+    _pg_user->speed = int16FromBeBytes(_pg_data, &_pg_byteindex);
+
     return 1;
 
 }// decodeServo_StatusBPacketStructure
@@ -824,8 +850,10 @@ int decodeServo_StatusBPacketStructure(const void* _pg_pkt, Servo_StatusB_t* _pg
  * \param current is Servo current
  * \param voltage is Servo supply voltage
  * \param temperature is Servo temperature
+ * \param dutyCycle is Motor duty cycle
+ * \param speed is Servo output shaft speed
  */
-void encodeServo_StatusBPacket(void* _pg_pkt, uint16_t current, uint16_t voltage, int8_t temperature)
+void encodeServo_StatusBPacket(void* _pg_pkt, uint16_t current, uint16_t voltage, int8_t temperature, int8_t dutyCycle, int16_t speed)
 {
     uint8_t* _pg_data = getServoPacketData(_pg_pkt);
     int _pg_byteindex = 0;
@@ -842,6 +870,14 @@ void encodeServo_StatusBPacket(void* _pg_pkt, uint16_t current, uint16_t voltage
     // Range of temperature is -128 to 127.
     int8ToBytes(temperature, _pg_data, &_pg_byteindex);
 
+    // Motor duty cycle
+    // Range of dutyCycle is -128 to 127.
+    int8ToBytes(dutyCycle, _pg_data, &_pg_byteindex);
+
+    // Servo output shaft speed
+    // Range of speed is -32768 to 32767.
+    int16ToBeBytes(speed, _pg_data, &_pg_byteindex);
+
     // complete the process of creating the packet
     finishServoPacket(_pg_pkt, _pg_byteindex, getServo_StatusBPacketID());
 
@@ -855,9 +891,11 @@ void encodeServo_StatusBPacket(void* _pg_pkt, uint16_t current, uint16_t voltage
  * \param current receives Servo current
  * \param voltage receives Servo supply voltage
  * \param temperature receives Servo temperature
+ * \param dutyCycle receives Motor duty cycle
+ * \param speed receives Servo output shaft speed
  * \return 0 is returned if the packet ID or size is wrong, else 1
  */
-int decodeServo_StatusBPacket(const void* _pg_pkt, uint16_t* current, uint16_t* voltage, int8_t* temperature)
+int decodeServo_StatusBPacket(const void* _pg_pkt, uint16_t* current, uint16_t* voltage, int8_t* temperature, int8_t* dutyCycle, int16_t* speed)
 {
     int _pg_byteindex = 0;
     const uint8_t* _pg_data = getServoPacketDataConst(_pg_pkt);
@@ -870,6 +908,10 @@ int decodeServo_StatusBPacket(const void* _pg_pkt, uint16_t* current, uint16_t*
     if(_pg_numbytes < getServo_StatusBMinDataLength())
         return 0;
 
+    // this packet has default fields, make sure they are set
+    (*dutyCycle) = 0;
+    (*speed) = 0;
+
     // Servo current
     // Range of current is 0 to 65535.
     (*current) = uint16FromBeBytes(_pg_data, &_pg_byteindex);
@@ -882,6 +924,20 @@ int decodeServo_StatusBPacket(const void* _pg_pkt, uint16_t* current, uint16_t*
     // Range of temperature is -128 to 127.
     (*temperature) = int8FromBytes(_pg_data, &_pg_byteindex);
 
+    if(_pg_byteindex + 1 > _pg_numbytes)
+        return 1;
+
+    // Motor duty cycle
+    // Range of dutyCycle is -128 to 127.
+    (*dutyCycle) = int8FromBytes(_pg_data, &_pg_byteindex);
+
+    if(_pg_byteindex + 2 > _pg_numbytes)
+        return 1;
+
+    // Servo output shaft speed
+    // Range of speed is -32768 to 32767.
+    (*speed) = int16FromBeBytes(_pg_data, &_pg_byteindex);
+
     return 1;
 
 }// decodeServo_StatusBPacket

libraries/AP_PiccoloCAN/piccolo_protocol/ServoPackets.h

@@ -233,6 +233,8 @@ typedef struct
     uint16_t current;     //!< Servo current
     uint16_t voltage;     //!< Servo supply voltage
     int8_t   temperature; //!< Servo temperature
+    int8_t   dutyCycle;   //!< Motor duty cycle
+    int16_t  speed;       //!< Servo output shaft speed
 }Servo_StatusB_t;
 
 //! Create the Servo_StatusB packet
@@ -242,10 +244,10 @@ void encodeServo_StatusBPacketStructure(void* pkt, const Servo_StatusB_t* user);
 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);
+void encodeServo_StatusBPacket(void* pkt, uint16_t current, uint16_t voltage, int8_t temperature, int8_t dutyCycle, int16_t speed);
 
 //! Decode the Servo_StatusB packet to parameters
-int decodeServo_StatusBPacket(const void* pkt, uint16_t* current, uint16_t* voltage, int8_t* temperature);
+int decodeServo_StatusBPacket(const void* pkt, uint16_t* current, uint16_t* voltage, int8_t* temperature, int8_t* dutyCycle, int16_t* speed);
 
 //! return the packet ID for the Servo_StatusB packet
 #define getServo_StatusBPacketID() (PKT_SERVO_STATUS_B)
@@ -254,7 +256,7 @@ int decodeServo_StatusBPacket(const void* pkt, uint16_t* current, uint16_t* volt
 #define getServo_StatusBMinDataLength() (5)
 
 //! return the maximum encoded length for the Servo_StatusB packet
-#define getServo_StatusBMaxDataLength() (5)
+#define getServo_StatusBMaxDataLength() (8)
 
 /*!
  * The *SERVO_STATUS_C* packet contains servo position data. It is a cut-down

libraries/AP_PiccoloCAN/piccolo_protocol/ServoProtocol.h

@@ -35,9 +35,9 @@ extern "C" {
  * 
  * The protocol API enumeration is incremented anytime the protocol is changed
  * in a way that affects compatibility with earlier versions of the protocol.
- * The protocol enumeration for this version is: 20
+ * The protocol enumeration for this version is: 21
  * 
- * The protocol version is 2.10
+ * The protocol version is 2.11
  */
 
 #include <stdbool.h>
@@ -45,10 +45,10 @@ extern "C" {
 #include <string.h>	// C string manipulation function header
 
 //! \return the protocol API enumeration
-#define getServoApi() 20
+#define getServoApi() 21
 
 //! \return the protocol version string
-#define getServoVersion() "2.10"
+#define getServoVersion() "2.11"
 
 // Translation provided externally. The macro takes a `const char *` and returns a `const char *`
 #ifndef translateServo

libraries/AP_PiccoloCAN/piccolo_protocol/Servo_source.xml

@@ -1,4 +1,4 @@
-<Protocol name="Servo" prefix="Servo_" api="20" version="2.10" mapfile="Servo_SettingsMap" verifyfile="Servo_SettingsVerify" endian="big" supportSpecialFloat="false" supportInt64="false" supportFloat64="false" supportBool="true" comment="This is the ICD for the Currawong Engineering CAN Servo. This document details the Servo command and packet structure for communication with and configuration of the Servo"><Include name="string.h" comment="C string manipulation function header" global="true" />
+<Protocol name="Servo" prefix="Servo_" api="21" version="2.11" mapfile="Servo_SettingsMap" verifyfile="Servo_SettingsVerify" endian="big" supportSpecialFloat="false" supportInt64="false" supportFloat64="false" supportBool="true" comment="This is the ICD for the Currawong Engineering CAN Servo. This document details the Servo command and packet structure for communication with and configuration of the Servo"><Include name="string.h" comment="C string manipulation function header" global="true" />
 
     <Enum name="ServoModes" lookup="true" prefix="SERVO_MODE_">
         <Value name="NORMAL" comment="The servo is in normal operational mode" />
@@ -124,7 +124,7 @@
         <Value name="FIRMWARE" comment="Servo firmware information" />
         <Value name="SYSTEM_INFO" comment="Servo system info (uptime, etc)" />
         <Value name="TELEMETRY_CONFIG" comment="Telemetry settings" />
-        <Value name="EEPROM" comment="Non-volatile settings configuration information" />
+        <Value name="SETTINGS_INFO" comment="Non-volatile settings configuration information" />
         <Value name="TELLTALE_A" value="0x7A" comment="Servo telltale information" hidden="true" />
 
         
@@ -183,13 +183,13 @@
     
 
     
-    <Packet name="MultiPositionCommand" ID="PKT_SERVO_MULTI_COMMAND_1" map="false" file="ServoPackets" parameterInterface="true" comment="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.">
+    <Packet name="MultiPositionCommand" ID="PKT_SERVO_MULTI_COMMAND_1 PKT_SERVO_MULTI_COMMAND_2 PKT_SERVO_MULTI_COMMAND_3 PKT_SERVO_MULTI_COMMAND_4 PKT_SERVO_MULTI_COMMAND_5 PKT_SERVO_MULTI_COMMAND_6 PKT_SERVO_MULTI_COMMAND_7 PKT_SERVO_MULTI_COMMAND_8 PKT_SERVO_MULTI_COMMAND_9 PKT_SERVO_MULTI_COMMAND_10 PKT_SERVO_MULTI_COMMAND_11 PKT_SERVO_MULTI_COMMAND_12 PKT_SERVO_MULTI_COMMAND_13 PKT_SERVO_MULTI_COMMAND_14 PKT_SERVO_MULTI_COMMAND_15 PKT_SERVO_MULTI_COMMAND_16" map="false" file="ServoPackets" parameterInterface="true" comment="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.">
         <Data name="commandA" inMemoryType="signed16" comment="Servo command for servo with address offset 0" units="dimensionless" range="-20,000 to +20,000" notes="Input limits depend on servo I/O mapping" />
         <Data name="commandB" inMemoryType="signed16" comment="Servo command for servo with address offset 1" units="dimensionless" range="-20,000 to +20,000" notes="Input limits depend on servo I/O mapping" />
         <Data name="commandC" inMemoryType="signed16" comment="Servo command for servo with address offset 3" units="dimensionless" range="-20,000 to +20,000" notes="Input limits depend on servo I/O mapping" />
         <Data name="commandD" inMemoryType="signed16" comment="Servo command for servo with address offset 3" units="dimensionless" range="-20,000 to +20,000" notes="Input limits depend on servo I/O mapping" />
     </Packet>
-
+o
     <Packet name="PositionCommand" ID="PKT_SERVO_POSITION_COMMAND" comment="Send this command to move the servo(s) to the commanded position. Position command units depend on the configuration of the servo. Send with the broadcast ID (0xFF) to send the position command to *all* servos." file="ServoPackets" parameterInterface="true">
         <Data name="command" inMemoryType="signed16" comment="Servo command" units="dimensionless" range="-20,000 to +20,000" notes="Input limits depend on servo I/O mapping" />
     </Packet>
@@ -207,7 +207,7 @@
         <Data name="title" inMemoryType="unsigned8" array="8" />
     </Packet>
 
-    <Packet name="StatusA" ID="PKT_SERVO_STATUS_A" file="ServoPackets" parameterInterface="true" comment="The *SERVO_STATUS_A* packet contains status, warning and error information, in addition to the servo position">
+    <Packet name="StatusA" ID="PKT_SERVO_STATUS_A" file="ServoPackets" parameterInterface="true" structureInterface="true" comment="The *SERVO_STATUS_A* packet contains status, warning and error information, in addition to the servo position">
         <Data name="status" struct="StatusBits" comment="Status bits contain information on servo operation" />
         <Data name="warnings" struct="WarningBits" comment="Warning bits indicate servo is operation outside of desired range" />
         <Data name="errors" struct="ErrorBits" comment="These bits indicate critical system error information" />
@@ -215,13 +215,15 @@
         <Data name="command" inMemoryType="signed16" comment="Servo commanded position" />
     </Packet>
 
-    <Packet name="StatusB" ID="PKT_SERVO_STATUS_B" file="ServoPackets" parameterInterface="true" comment="The *SERVO_STATUS_B* packet contains various servo feedback data">
+    <Packet name="StatusB" ID="PKT_SERVO_STATUS_B" file="ServoPackets" parameterInterface="true" structureInterface="true" comment="The *SERVO_STATUS_B* packet contains various servo feedback data">
         <Data name="current" inMemoryType="unsigned16" units="10mA per bit" comment="Servo current" />
         <Data name="voltage" inMemoryType="unsigned16" units="10mV per bit" comment="Servo supply voltage" />
         <Data name="temperature" inMemoryType="signed8" comment="Servo temperature" units="1C per bit" />
+        <Data name="dutyCycle" inMemoryType="signed8" default="0" comment="Motor duty cycle" units="1% per bit" range="-100% to +100%" />
+        <Data name="speed" inMemoryType="signed16" default="0" comment="Servo output shaft speed" units="1 degree per second" />
     </Packet>
     
-    <Packet name="StatusC" ID="PKT_SERVO_STATUS_C" file="ServoPackets" parameterInterface="true" comment="The *SERVO_STATUS_C* packet contains servo position data. It is a cut-down packet to allow high-speed feedback on servo position">
+    <Packet name="StatusC" ID="PKT_SERVO_STATUS_C" file="ServoPackets" parameterInterface="true" structureInterface="true" comment="The *SERVO_STATUS_C* packet contains servo position data. It is a cut-down packet to allow high-speed feedback on servo position">
         <Data name="position" inMemoryType="signed16" comment="Servo position, mapped to input units" />
     </Packet>    
 
@@ -233,7 +235,7 @@
         <Data name="resolution" inMemoryType="unsigned8" comment="Accelerometer measurement resolution, in 'bits'." />
     </Packet>
 
-    <Packet name="Address" ID="PKT_SERVO_ADDRESS" file="ServoPackets" parameterInterface="true">
+    <Packet name="Address" ID="PKT_SERVO_ADDRESS" file="ServoPackets" parameterInterface="true" structureInterface="true">
         <Data name="hwRev" inMemoryType="unsigned8" comment="Hardware revision" />
         <Data name="serialNumber" inMemoryType="unsigned32" encodedType="unsigned24" comment="Servo serial number" />
         <Data name="userIDA" inMemoryType="unsigned16" comment="Programmable User ID value 1/2" />
@@ -246,7 +248,7 @@
 
     
 
-    <Packet name="Firmware" ID="PKT_SERVO_FIRMWARE" file="ServoPackets" parameterInterface="true">
+    <Packet name="Firmware" ID="PKT_SERVO_FIRMWARE" file="ServoPackets" parameterInterface="true" structureInterface="true">
         <Data name="major" inMemoryType="unsigned8" comment="Firmware version, major number" />
         <Data name="minor" inMemoryType="unsigned8" comment="Firmware version, minor number" />
         <Data name="day" inMemoryType="unsigned8" comment="Firmware release date, day-of-month" />
@@ -255,7 +257,7 @@
         <Data name="checksum" inMemoryType="unsigned16" comment="Firmware checksum, 16-bit" />
     </Packet>
 
-    <Packet name="SystemInfo" ID="PKT_SERVO_SYSTEM_INFO" file="ServoPackets" parameterInterface="true">
+    <Packet name="SystemInfo" ID="PKT_SERVO_SYSTEM_INFO" file="ServoPackets" parameterInterface="true" structureInterface="true">
         <Data name="msSinceReset" inMemoryType="unsigned32" comment="Time since last power cycle (milliseconds)" />
         <Data name="powerCycles" inMemoryType="unsigned16" comment="Number of recorded power cycles" />
         <Data name="resetCode" inMemoryType="unsigned8" comment="Processor code indicating cause of most recent reset event" />
@@ -263,7 +265,7 @@
     </Packet>
 
     
-    <Packet name="TelemetryConfig" ID="PKT_SERVO_TELEMETRY_CONFIG" file="ServoPackets" parameterInterface="true" comment="Telemetry settings configuration packet">
+    <Packet name="TelemetryConfig" ID="PKT_SERVO_TELEMETRY_CONFIG" file="ServoPackets" parameterInterface="true" structureInterface="true" comment="Telemetry settings configuration packet">
         <Data name="settings" struct="TelemetrySettings" comment="Servo telemetry settings" />
         
         <Data name="reserved" array="3" inMemoryType="null" encodedType="unsigned8" constant="0" comment="Reserved for future use" />
@@ -271,7 +273,7 @@
         <Data name="icdVersion" inMemoryType="unsigned8" constant="getServoApi()" comment="Servo ICD revision" />
     </Packet>
 
-    <Packet name="EEPROMInformation" ID="PKT_SERVO_EEPROM" file="ServoPackets" parameterInterface="true" structureInterface="true">
+    <Packet name="SettingsInfo" ID="PKT_SERVO_SETTINGS_INFO" file="ServoPackets" parameterInterface="true" structureInterface="true">
         <Data name="eeUnlocked" inMemoryType="bitfield1" comment="Set if the servo is unlocked and ready to receive settings updates" />
         <Data name="eeVersion" inMemoryType="bitfield7" comment="Version of non-volatile settings configuration" />
         <Data name="eeSize" inMemoryType="unsigned16" comment="Size of non-volatile settings data" />