diff --git a/libraries/AP_PiccoloCAN/AP_PiccoloCAN.cpp b/libraries/AP_PiccoloCAN/AP_PiccoloCAN.cpp index 710c47e703..0838b0a95b 100644 --- a/libraries/AP_PiccoloCAN/AP_PiccoloCAN.cpp +++ b/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++) { diff --git a/libraries/AP_PiccoloCAN/AP_PiccoloCAN.h b/libraries/AP_PiccoloCAN/AP_PiccoloCAN.h index 80b5af3330..fadcdcb645 100644 --- a/libraries/AP_PiccoloCAN/AP_PiccoloCAN.h +++ b/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 diff --git a/libraries/AP_PiccoloCAN/piccolo_protocol/ServoPackets.c b/libraries/AP_PiccoloCAN/piccolo_protocol/ServoPackets.c index c310e6c29c..ed32559e7b 100644 --- a/libraries/AP_PiccoloCAN/piccolo_protocol/ServoPackets.c +++ b/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 * diff --git a/libraries/AP_PiccoloCAN/piccolo_protocol/ServoPackets.h b/libraries/AP_PiccoloCAN/piccolo_protocol/ServoPackets.h index ef88bb0492..5570bd2482 100644 --- a/libraries/AP_PiccoloCAN/piccolo_protocol/ServoPackets.h +++ b/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);