diff --git a/libraries/AP_PiccoloCAN/AP_PiccoloCAN.cpp b/libraries/AP_PiccoloCAN/AP_PiccoloCAN.cpp index d6b1d70b81..260b162dfb 100644 --- a/libraries/AP_PiccoloCAN/AP_PiccoloCAN.cpp +++ b/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++) { diff --git a/libraries/AP_PiccoloCAN/piccolo_protocol/ServoPackets.c b/libraries/AP_PiccoloCAN/piccolo_protocol/ServoPackets.c index ed32559e7b..440559a171 100644 --- a/libraries/AP_PiccoloCAN/piccolo_protocol/ServoPackets.c +++ b/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 diff --git a/libraries/AP_PiccoloCAN/piccolo_protocol/ServoPackets.h b/libraries/AP_PiccoloCAN/piccolo_protocol/ServoPackets.h index 5570bd2482..aceaee7cf7 100644 --- a/libraries/AP_PiccoloCAN/piccolo_protocol/ServoPackets.h +++ b/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 diff --git a/libraries/AP_PiccoloCAN/piccolo_protocol/ServoProtocol.h b/libraries/AP_PiccoloCAN/piccolo_protocol/ServoProtocol.h index fc6718cb2d..8fbd20ecba 100644 --- a/libraries/AP_PiccoloCAN/piccolo_protocol/ServoProtocol.h +++ b/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 @@ -45,10 +45,10 @@ extern "C" { #include // 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 diff --git a/libraries/AP_PiccoloCAN/piccolo_protocol/Servo_source.xml b/libraries/AP_PiccoloCAN/piccolo_protocol/Servo_source.xml index e5b2754655..df368b864b 100644 --- a/libraries/AP_PiccoloCAN/piccolo_protocol/Servo_source.xml +++ b/libraries/AP_PiccoloCAN/piccolo_protocol/Servo_source.xml @@ -1,4 +1,4 @@ - + @@ -124,7 +124,7 @@ - +