diff --git a/CMakeLists.txt b/CMakeLists.txt index 09139f0..d2893c2 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -64,8 +64,8 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE src/drivers/bq_2576/bq_2576.cpp # VESC driver src/drivers/vesc/buffer.cpp - src/drivers/vesc/VescUart.cpp src/drivers/vesc/crc.cpp + src/drivers/vesc/VescDriver.cpp # GPS driver src/drivers/gps/gps_interface.cpp src/drivers/gps/ublox_gps_interface.cpp diff --git a/boards/XCORE/mcuconf.h b/boards/XCORE/mcuconf.h index 6c40ccf..84f3fae 100644 --- a/boards/XCORE/mcuconf.h +++ b/boards/XCORE/mcuconf.h @@ -359,10 +359,10 @@ /* * SERIAL driver system settings. */ -#define STM32_SERIAL_USE_USART1 TRUE -#define STM32_SERIAL_USE_USART2 TRUE +#define STM32_SERIAL_USE_USART1 FALSE +#define STM32_SERIAL_USE_USART2 FALSE #define STM32_SERIAL_USE_USART3 FALSE -#define STM32_SERIAL_USE_UART4 TRUE +#define STM32_SERIAL_USE_UART4 FALSE #define STM32_SERIAL_USE_UART5 FALSE #define STM32_SERIAL_USE_USART6 FALSE #define STM32_SERIAL_USE_UART7 FALSE @@ -435,10 +435,10 @@ /* * UART driver system settings. */ -#define STM32_UART_USE_USART1 FALSE -#define STM32_UART_USE_USART2 FALSE +#define STM32_UART_USE_USART1 TRUE +#define STM32_UART_USE_USART2 TRUE #define STM32_UART_USE_USART3 FALSE -#define STM32_UART_USE_UART4 FALSE +#define STM32_UART_USE_UART4 TRUE #define STM32_UART_USE_UART5 FALSE #define STM32_UART_USE_USART6 TRUE #define STM32_UART_USE_UART7 TRUE diff --git a/cfg/halconf.h b/cfg/halconf.h index d761398..c7c42aa 100644 --- a/cfg/halconf.h +++ b/cfg/halconf.h @@ -142,7 +142,7 @@ * @brief Enables the SERIAL subsystem. */ #if !defined(HAL_USE_SERIAL) || defined(__DOXYGEN__) -#define HAL_USE_SERIAL TRUE +#define HAL_USE_SERIAL FALSE #endif /** diff --git a/src/drivers/gps/gps_interface.cpp b/src/drivers/gps/gps_interface.cpp index b3fab0c..1998eed 100644 --- a/src/drivers/gps/gps_interface.cpp +++ b/src/drivers/gps/gps_interface.cpp @@ -29,9 +29,6 @@ bool GpsDriver::StartDriver(UARTDriver *uart, uint32_t baudrate) { } uart_config_.rxend_cb = [](UARTDriver *uartp) { - (void)uartp; - static int i = 0; - i++; GpsDriver *instance = reinterpret_cast(uartp->config)->context; if (!instance->processing_done_) { // This is bad, processing is too slow to keep up with updates! diff --git a/src/drivers/vesc/README.md b/src/drivers/vesc/README.md index 3473a19..035026f 100644 --- a/src/drivers/vesc/README.md +++ b/src/drivers/vesc/README.md @@ -1,4 +1,4 @@ # VescUart -This library is adapted from the VescUart project which can be found here: +This library is very adapted from the VescUart project which can be found here: https://github.com/SolidGeek/VescUart diff --git a/src/drivers/vesc/VescDriver.cpp b/src/drivers/vesc/VescDriver.cpp new file mode 100644 index 0000000..6050910 --- /dev/null +++ b/src/drivers/vesc/VescDriver.cpp @@ -0,0 +1,312 @@ +// +// Created by clemens on 08.11.24. +// + +#include "VescDriver.h" + +#include "buffer.h" +#include "crc.h" +#include "datatypes.h" + +static constexpr uint32_t EVT_ID_RECEIVED = 1; +static constexpr uint32_t EVT_ID_EXPECT_PACKET = 2; + +namespace xbot::driver::esc { +VescDriver::VescDriver() { + latest_state.status = ESCState::ESCStatus::ESC_STATUS_DISCONNECTED; +}; +bool VescDriver::StartDriver(UARTDriver* uart, uint32_t baudrate) { + chDbgAssert(stopped_, "don't start the driver twice"); + chDbgAssert(uart != nullptr, "need to provide a driver"); + if (!stopped_) { + return false; + } + this->uart_ = uart; + uart_config_.speed = baudrate; + uart_config_.context = this; + bool uartStarted = uartStart(uart, &uart_config_) == MSG_OK; + if (!uartStarted) { + return false; + } + + uart_config_.rxend_cb = [](UARTDriver* uartp) { + VescDriver* instance = reinterpret_cast(uartp->config)->context; + if (!instance->processing_done_) { + // This is bad, processing is too slow to keep up with updates! + // We just read into the same buffer again + uint8_t* next_recv_buffer = + (instance->processing_buffer_ == instance->recv_buffer1_) ? instance->recv_buffer2_ : instance->recv_buffer1_; + uartStartReceiveI(uartp, RECV_BUFFER_SIZE, next_recv_buffer); + } else { + // Swap buffers and read into the next one + // Get the pointer to the receiving buffer (it's not the processing buffer) + uint8_t* next_recv_buffer = instance->processing_buffer_; + uartStartReceiveI(uartp, RECV_BUFFER_SIZE, next_recv_buffer); + instance->processing_buffer_ = + (instance->processing_buffer_ == instance->recv_buffer1_) ? instance->recv_buffer2_ : instance->recv_buffer1_; + instance->processing_buffer_len_ = RECV_BUFFER_SIZE; + instance->processing_done_ = false; + // Signal the processing thread + if (instance->processing_thread_) { + chEvtSignalI(instance->processing_thread_, 1); + } + } + }; + + stopped_ = false; + processing_thread_ = chThdCreateStatic(&thd_wa_, sizeof(thd_wa_), NORMALPRIO, threadHelper, this); +#ifdef USE_SEGGER_SYSTEMVIEW + processing_thread_->name = "VESCDriver"; +#endif + + uartStartReceive(uart, RECV_BUFFER_SIZE, recv_buffer1_); + return true; +} +void VescDriver::SetStatusUpdateInterval(uint32_t interval_millis) { + (void)interval_millis; +} +void VescDriver::SetStateCallback(const StateCallback& function) { + state_callback_ = function; +} + +void VescDriver::ProcessPayload() { + // check size, reported payload should be the same as received bytes + 1 byte header + byte length + 2 byte CRC + 1 + // trailer + uint8_t payload_length = working_buffer_[1]; + if (payload_length + 5 != working_buffer_fill_) { + // reset + working_buffer_fill_ = 0; + return; + } + // first byte needs to be 0x02, last byte needs to be 0x03 + if (working_buffer_[0] != 0x02 || working_buffer_[working_buffer_fill_ - 1] != 0x03) { + working_buffer_fill_ = 0; + return; + } + // Check the CRC + uint16_t expectedCRC = crc16(working_buffer_ + 2, payload_length); + uint16_t actualCRC = + static_cast(working_buffer_[working_buffer_fill_ - 3]) << 8 | working_buffer_[working_buffer_fill_ - 2]; + + if (expectedCRC != actualCRC) { + working_buffer_fill_ = 0; + return; + } + + uint8_t packet_id = working_buffer_[2]; + int32_t index = 0; + // 3 = start of actual payload + uint8_t *message = working_buffer_+3; + switch (packet_id) { + case COMM_FW_VERSION: // Structure defined here: + // https://github.com/vedderb/bldc/blob/43c3bbaf91f5052a35b75c2ff17b5fe99fad94d1/commands.c#L164 + + latest_state.fw_major = message[index++]; + latest_state.fw_minor = message[index++]; + break; + case COMM_GET_VALUES: // Structure defined here: + // https://github.com/vedderb/bldc/blob/43c3bbaf91f5052a35b75c2ff17b5fe99fad94d1/commands.c#L164 + + latest_state.temperature_pcb = buffer_get_float16( + message, 10.0, &index); // 2 bytes - mc_interface_temp_fet_filtered() + latest_state.temperature_motor = buffer_get_float16( + message, 10.0, + &index); // 2 bytes - mc_interface_temp_motor_filtered() + index += 4;// 4 bytes - mc_interface_read_reset_avg_motor_current() + latest_state.current_input = buffer_get_float32( + message, 100.0, + &index); // 4 bytes - mc_interface_read_reset_avg_input_current() + index += 4; // Skip 4 bytes - mc_interface_read_reset_avg_id() + index += 4; // Skip 4 bytes - mc_interface_read_reset_avg_iq() + latest_state.duty_cycle = buffer_get_float16( + message, 1000.0, + &index); // 2 bytes - mc_interface_get_duty_cycle_now() + latest_state.rpm = buffer_get_float32( + message, 1.0, &index); // 4 bytes - mc_interface_get_rpm() + latest_state.voltage_input = buffer_get_float16( + message, 10.0, &index); // 2 bytes - GET_INPUT_VOLTAGE() + index += 4; // 4 bytes - mc_interface_get_amp_hours(false) + index += 4; // 4 bytes - mc_interface_get_amp_hours_charged(false) + index += 4; // 4 bytes - mc_interface_get_watt_hours(false) + index += 4; // 4 bytes - mc_interface_get_watt_hours_charged(false) + latest_state.tacho = buffer_get_int32( + message, + &index); // 4 bytes - mc_interface_get_tachometer_value(false) + latest_state.tacho_absolute = buffer_get_int32( + message, + &index); // 4 bytes - mc_interface_get_tachometer_abs_value(false) + latest_state.status = static_cast(message[index++]) != FAULT_CODE_NONE ? ESCState::ESCStatus::ESC_STATUS_ERROR : ESCState::ESCStatus::ESC_STATUS_OK; + index += 4; // 4 bytes - mc_interface_get_pid_pos_now() + latest_state.direction = latest_state.rpm < 0; + break; + default: + // ignore + break; + } + + if (state_callback_) state_callback_(latest_state); + + working_buffer_fill_ = 0; +} +bool VescDriver::ProcessBytes(uint8_t* buffer, size_t len) { + if (len == 0) { + // expect more data + return true; + } + + while (len > 0) { + switch (working_buffer_fill_) { + case 0: { + if (len == 0) { + continue; + } + // buffer empty, looking for 0x02 + const auto header_start = static_cast(memchr(buffer, 0x02, len)); + if (header_start == nullptr) { + // reject the whole input, we don't have 0x02 + len = 0; + continue; + } + // Throw away all bytes before the header start + len -= (header_start - buffer); + buffer = header_start; + working_buffer_[working_buffer_fill_++] = *buffer; + buffer++; + len--; + } break; + case 1: { + // we have started the packet, read the length (it's a '2' packet, so length is one byte and cannot exceed our + // buffer size) + working_buffer_[working_buffer_fill_++] = *buffer; + buffer++; + len--; + } break; + default: { + // take until the payload is complete + size_t payload_length = working_buffer_[1]; + // remove the two header bytes + size_t payload_in_buffer = (working_buffer_fill_ - 2); + + // Take the remaining payload + CRC + trailer + size_t bytes_to_take = etl::min(len, payload_length - payload_in_buffer + 3); + memcpy(&working_buffer_[working_buffer_fill_], buffer, bytes_to_take); + working_buffer_fill_ += bytes_to_take; + buffer += bytes_to_take; + len -= bytes_to_take; + + if (payload_length + 5 == working_buffer_fill_) { + // finished reading payload, process it + ProcessPayload(); + // done with this packet, reset parser + working_buffer_fill_ = 0; + } + } + } + } + + // we expect more data if the working buffer is not clean + return working_buffer_fill_ != 0; +} +void VescDriver::SendPacket() { + // header starts with a 2 + payload_buffer_.prepend[3] = 2; + chDbgAssert(payload_buffer_.payload_length < 256, "Max payload size of 255"); + payload_buffer_.prepend[4] = payload_buffer_.payload_length; + uint16_t crcPayload = crc16(payload_buffer_.payload, payload_buffer_.payload_length); + payload_buffer_.payload[payload_buffer_.payload_length] = static_cast(crcPayload >> 8); + payload_buffer_.payload[payload_buffer_.payload_length + 1] = static_cast(crcPayload & 0xFF); + payload_buffer_.payload[payload_buffer_.payload_length + 2] = 3; + size_t total_size = payload_buffer_.payload_length + 5; + uartSendFullTimeout(uart_, &total_size, &payload_buffer_.prepend[3], TIME_INFINITE); +} + +void VescDriver::RequestStatus() { + chMtxLock(&mutex_); + payload_buffer_.payload_length = 1; + payload_buffer_.payload[0] = COMM_GET_VALUES; + SendPacket(); + // Signal that we are waiting for a response, so the receiving thread becomes active + chEvtSignal(processing_thread_, EVT_ID_EXPECT_PACKET); + chMtxUnlock(&mutex_); +} +void VescDriver::SetDuty(float duty) { + chMtxLock(&mutex_); + payload_buffer_.payload_length = 5; + payload_buffer_.payload[0] = COMM_SET_DUTY; + int32_t index = 1; + buffer_append_int32(payload_buffer_.payload, static_cast(duty * 100000), &index); + SendPacket(); + chMtxUnlock(&mutex_); +} + +void VescDriver::threadFunc() { + bool expects_packet = false; + while (!stopped_) { + uint32_t events; + if (expects_packet) { + // read with timeout, so that if the packet is too short to fill the whole buffer (and thereby interrupting this), + // we will still process it + events = chEvtWaitOneTimeout(ALL_EVENTS, TIME_MS2I(10)); + } else { + // we don't expect a packet, so in order to not loop all the time for empty buffers, + // wait an infinite time (or status_request_millis_ if we need to request status periodically) + if(status_request_millis_ > 0) { + events = chEvtWaitOneTimeout(ALL_EVENTS, TIME_MS2I(status_request_millis_)); + } else { + events = chEvtWaitOneTimeout(ALL_EVENTS, TIME_INFINITE); + } + } + + bool expects_packet_received = (events & EVT_ID_EXPECT_PACKET) != 0; + // handle the "expect a packet" event by setting the flag and continuing + // this will wait for the reception with a timeout + if (expects_packet_received) { + expects_packet = true; + continue; + } + + bool packet_received = (events & EVT_ID_RECEIVED) != 0; + if (!packet_received) { + // If timeout, we take the buffer and restart the reception + // Else, the ISR has already prepared the buffer for us + // Lock the core to prevent the RX interrupt from firing + chSysLock(); + // Check, that processing_done_ is still true + // (in case ISR happened between the timeout and now, this will be set to false by ISR) + if (processing_done_) { + // Stop reception and get the partial received length + size_t not_received_len = uartStopReceiveI(uart_); + if (not_received_len != UART_ERR_NOT_ACTIVE) { + // Uart was still receiving, so the buffer length is not complete + processing_buffer_len_ = RECV_BUFFER_SIZE - not_received_len; + } else { + // Uart was not active. + // This should not happen, but could during debug when pausing the chip + // we ignore this buffer, but carry on as usual + processing_buffer_len_ = 0; + } + uint8_t* next_recv_buffer = processing_buffer_; + uartStartReceiveI(uart_, RECV_BUFFER_SIZE, next_recv_buffer); + processing_buffer_ = (processing_buffer_ == recv_buffer1_) ? recv_buffer2_ : recv_buffer1_; + processing_done_ = false; + } + // allow ISR again + chSysUnlock(); + } + if (processing_buffer_len_ > 0) { + if (!ProcessBytes(processing_buffer_, processing_buffer_len_)) { + // we don't expect more data, so we can wait for the next request + expects_packet = false; + } + // In any case (partial payload received), the processing buffer needs to be reset + processing_buffer_len_ = 0; + } + processing_done_ = true; + } +} + +void VescDriver::threadHelper(void* instance) { + auto* i = static_cast(instance); + i->threadFunc(); +} +} // namespace xbot::driver::esc diff --git a/src/drivers/vesc/VescDriver.h b/src/drivers/vesc/VescDriver.h new file mode 100644 index 0000000..a80a8bd --- /dev/null +++ b/src/drivers/vesc/VescDriver.h @@ -0,0 +1,117 @@ +// +// Created by clemens on 08.11.24. +// + +#ifndef VESCDRIVER_H +#define VESCDRIVER_H + +#include "ch.h" +#include "hal.h" +#include +#include + +namespace xbot::driver::esc { +class VescDriver { +public: + + + struct ESCState { + enum class ESCStatus : uint8_t { + ESC_STATUS_DISCONNECTED = 99u, + ESC_STATUS_ERROR = 100u, + ESC_STATUS_STALLED = 150u, + ESC_STATUS_OK = 200u, + ESC_STATUS_RUNNING = 201u, + }; + + uint8_t fw_major; + uint8_t fw_minor; + + float voltage_input; + float temperature_pcb; + float temperature_motor; + float current_input; + float duty_cycle; + uint32_t tacho; + uint32_t tacho_absolute; + float direction; + float rpm; + + ESCStatus status; + }; + typedef etl::delegate StateCallback; + + + VescDriver(); + + ~VescDriver() = default; + + bool StartDriver(UARTDriver *uart, uint32_t baudrate); + void SetStatusUpdateInterval(uint32_t interval_millis); + void SetStateCallback(const StateCallback &function); + void RequestStatus(); + void SetDuty(float duty); + +private: + StateCallback state_callback_{}; + + ESCState latest_state{}; +#pragma pack(push, 1) + struct VescPayload { + // prepend space will be used for packet size / CAN ID + // its structure id dependend on packet length and if CAN or not + uint8_t prepend[5]{}; + // the actual payload + // 255 bytes for max payload + 3 bytes for CRC and a framing byte + uint8_t payload[258]{}; + // Length of valid data in payload array + uint8_t payload_length{}; + } __attribute__((packed)); +#pragma pack(pop) + + MUTEX_DECL(mutex_); + // A buffer to prepare the next payload, so that we don't need to allocate on the stack + // make sure to lock mutex_ before using. + VescPayload payload_buffer_{}; + THD_WORKING_AREA(thd_wa_, 1024){}; + // Milliseconds for automatic status requests. 0 to disable + uint32_t status_request_millis_ = 0; + + // Extend the config struct by a pointer to this instance, so that we can access it in callbacks. + struct UARTConfigEx : UARTConfig { + VescDriver *context; + }; + + static constexpr size_t RECV_BUFFER_SIZE = 260; + uint32_t last_status_request_millis_ticks_ = 0; + + // Keep two buffers for streaming data while doing processing + uint8_t recv_buffer1_[RECV_BUFFER_SIZE]{}; + uint8_t recv_buffer2_[RECV_BUFFER_SIZE]{}; + // We start by receiving into recv_buffer1, so processing_buffer is the 2 (but empty) + uint8_t *volatile processing_buffer_ = recv_buffer2_; + volatile size_t processing_buffer_len_ = 0; + + // working buffer for the receiving thread + uint8_t working_buffer_fill_ = 0; + // length of 260 = 1 header byte, 1 length byte, 255 max payload, 2 CRC, 1 trailer + uint8_t working_buffer_[260]; + bool found_header_ = false; + + UARTDriver *uart_{}; + UARTConfigEx uart_config_{}; + thread_t *processing_thread_ = nullptr; + // This is reset by the receiving ISR and set by the thread to signal if it's safe to process more data. + volatile bool processing_done_ = true; + bool stopped_ = true; + + void ProcessPayload(); + bool ProcessBytes(uint8_t * buffer, size_t len); + void SendPacket(); + void threadFunc(); + static void threadHelper(void *instance); + +}; +} + +#endif //VESCDRIVER_H diff --git a/src/drivers/vesc/VescUart.cpp b/src/drivers/vesc/VescUart.cpp deleted file mode 100644 index b2bb8c3..0000000 --- a/src/drivers/vesc/VescUart.cpp +++ /dev/null @@ -1,416 +0,0 @@ -#include "VescUart.h" - -#include -#include - -#include - -#include "crc.h" - -int VescUart::receiveUartMessage(uint8_t* payloadReceived) { - // Messages <= 255 starts with "2", 2nd byte is length - // Messages > 255 starts with "3" 2nd and 3rd byte is length combined with 1st - // >>8 and then &0xFF - - uint16_t counter = 0; - uint16_t endMessage = 256; - bool messageRead = false; - uint8_t messageReceived[256]; - uint16_t lenPayload = 0; - size_t bytes_read = 0; - size_t bytes_to_read = 1; - while (bytes_to_read > 0 && - (bytes_read = sdReadTimeout(uart_, messageReceived + counter, - bytes_to_read, TIME_MS2I(_TIMEOUT))) > 0) { - counter += bytes_read; - // keep on reading, if a larger chunk was requested - bytes_to_read -= bytes_read; - // if only one byte was requested, read another one - if (bytes_to_read == 0) { - bytes_to_read = 1; - } - if (counter == 2) { - switch (messageReceived[0]) { - case 2: - endMessage = messageReceived[1] + - 5; // Payload size + 2 for sice + 3 for SRC and End. - lenPayload = messageReceived[1]; - - if (endMessage >= sizeof(messageReceived)) { - // invalid message length, try again - counter = 0; - bytes_to_read = 1; - } else { - // we know exactly how many more bytes to read - bytes_to_read = endMessage - counter; - } - break; - - case 3: - // ToDo: Add Message Handling > 255 (starting with 3) - - break; - - default: - - break; - } - } - - if (counter >= sizeof(messageReceived)) { - counter = 0; - break; - } - - if (counter == endMessage && messageReceived[endMessage - 1] == 3) { - messageReceived[endMessage] = 0; - - messageRead = true; - break; // Exit if end of message is reached, even if there is still - // more data in the buffer. - } - } - - // if(messageRead == false && debugPort != nullptr ) { - // debugPort->println("Timeout"); - // } - - bool unpacked = false; - - if (messageRead) { - unpacked = unpackPayload(messageReceived, endMessage, payloadReceived); - } - - if (unpacked) { - // Message was read - return lenPayload; - } else { - // No Message Read - return 0; - } -} - -bool VescUart::unpackPayload(uint8_t* message, int lenMes, uint8_t* payload) { - uint16_t crcMessage = 0; - uint16_t crcPayload = 0; - - // Rebuild crc: - crcMessage = message[lenMes - 3] << 8; - crcMessage &= 0xFF00; - crcMessage += message[lenMes - 2]; - - // Extract payload: - memcpy(payload, &message[2], message[1]); - - crcPayload = crc16(payload, message[1]); - - if (crcPayload == crcMessage) { - return true; - } else { - return false; - } -} - -int VescUart::packSendPayload(uint8_t* payload, int lenPay) { - uint16_t crcPayload = crc16(payload, lenPay); - int count = 0; - uint8_t messageSend[256]; - - if (lenPay <= 256) { - messageSend[count++] = 2; - messageSend[count++] = lenPay; - } else { - messageSend[count++] = 3; - messageSend[count++] = (uint8_t)(lenPay >> 8); - messageSend[count++] = (uint8_t)(lenPay & 0xFF); - } - - memcpy(messageSend + count, payload, lenPay); - count += lenPay; - - messageSend[count++] = (uint8_t)(crcPayload >> 8); - messageSend[count++] = (uint8_t)(crcPayload & 0xFF); - messageSend[count++] = 3; - // messageSend[count] = nullptr; - - // Sending package - sendRaw(messageSend, count); - - // Returns number of send bytes - return count; -} - -bool VescUart::processReadPacket(uint8_t* message) { - COMM_PACKET_ID packetId; - int32_t index = 0; - - packetId = (COMM_PACKET_ID)message[0]; - message++; // Removes the packetId from the actual message (payload) - - switch (packetId) { - case COMM_FW_VERSION: // Structure defined here: - // https://github.com/vedderb/bldc/blob/43c3bbaf91f5052a35b75c2ff17b5fe99fad94d1/commands.c#L164 - - fw_version.major = message[index++]; - fw_version.minor = message[index++]; - return true; - case COMM_GET_VALUES: // Structure defined here: - // https://github.com/vedderb/bldc/blob/43c3bbaf91f5052a35b75c2ff17b5fe99fad94d1/commands.c#L164 - - data.tempMosfet = buffer_get_float16( - message, 10.0, &index); // 2 bytes - mc_interface_temp_fet_filtered() - data.tempMotor = buffer_get_float16( - message, 10.0, - &index); // 2 bytes - mc_interface_temp_motor_filtered() - data.avgMotorCurrent = buffer_get_float32( - message, 100.0, - &index); // 4 bytes - mc_interface_read_reset_avg_motor_current() - data.avgInputCurrent = buffer_get_float32( - message, 100.0, - &index); // 4 bytes - mc_interface_read_reset_avg_input_current() - index += 4; // Skip 4 bytes - mc_interface_read_reset_avg_id() - index += 4; // Skip 4 bytes - mc_interface_read_reset_avg_iq() - data.dutyCycleNow = buffer_get_float16( - message, 1000.0, - &index); // 2 bytes - mc_interface_get_duty_cycle_now() - data.rpm = buffer_get_float32( - message, 1.0, &index); // 4 bytes - mc_interface_get_rpm() - data.inpVoltage = buffer_get_float16( - message, 10.0, &index); // 2 bytes - GET_INPUT_VOLTAGE() - data.ampHours = buffer_get_float32( - message, 10000.0, - &index); // 4 bytes - mc_interface_get_amp_hours(false) - data.ampHoursCharged = buffer_get_float32( - message, 10000.0, - &index); // 4 bytes - mc_interface_get_amp_hours_charged(false) - data.wattHours = buffer_get_float32( - message, 10000.0, - &index); // 4 bytes - mc_interface_get_watt_hours(false) - data.wattHoursCharged = buffer_get_float32( - message, 10000.0, - &index); // 4 bytes - mc_interface_get_watt_hours_charged(false) - data.tachometer = buffer_get_int32( - message, - &index); // 4 bytes - mc_interface_get_tachometer_value(false) - data.tachometerAbs = buffer_get_int32( - message, - &index); // 4 bytes - mc_interface_get_tachometer_abs_value(false) - data.error = (mc_fault_code) - message[index++]; // 1 byte - mc_interface_get_fault() - data.pidPos = buffer_get_float32( - message, 1000000.0, - &index); // 4 bytes - mc_interface_get_pid_pos_now() - data.id = - message[index++]; // 1 byte - app_get_configuration()->controller_id - - return true; - - break; - - /* case COMM_GET_VALUES_SELECTIVE: - - uint32_t mask = 0xFFFFFFFF; */ - - default: - return false; - break; - } -} - -VescUart::VescUart(SerialDriver* uart_handle, uint32_t timeout_ms) - : _TIMEOUT(timeout_ms), uart_(uart_handle) { - nunchuck.valueX = 127; - nunchuck.valueY = 127; - nunchuck.lowerButton = false; - nunchuck.upperButton = false; -} -bool VescUart::startDriver() { - serial_config_.speed = 115200; - return sdStart(uart_, &serial_config_) == MSG_OK; -} -bool VescUart::getFWversion(void) { return getFWversion(0); } - -bool VescUart::getFWversion(uint8_t canId) { - int32_t index = 0; - int payloadSize = (canId == 0 ? 1 : 3); - // 3 = max payload size - uint8_t payload[3]; - - if (canId != 0) { - payload[index++] = {COMM_FORWARD_CAN}; - payload[index++] = canId; - } - payload[index++] = {COMM_FW_VERSION}; - - packSendPayload(payload, payloadSize); - - uint8_t message[256]; - int messageLength = receiveUartMessage(message); - if (messageLength > 0) { - return processReadPacket(message); - } - return false; -} - -bool VescUart::getVescValues(void) { return getVescValues(0); } - -bool VescUart::getVescValues(uint8_t canId) { - int32_t index = 0; - int payloadSize = (canId == 0 ? 1 : 3); - // 3 = max payload size - uint8_t payload[3]; - if (canId != 0) { - payload[index++] = {COMM_FORWARD_CAN}; - payload[index++] = canId; - } - payload[index++] = {COMM_GET_VALUES}; - - packSendPayload(payload, payloadSize); - - uint8_t message[256]; - int messageLength = receiveUartMessage(message); - - if (messageLength > 55) { - return processReadPacket(message); - } - return false; -} -void VescUart::requestVescValues(void) { requestVescValues(0); } - -void VescUart::requestVescValues(uint8_t canId) { - int32_t index = 0; - int payloadSize = (canId == 0 ? 1 : 3); - // 3 = max payload size - uint8_t payload[3]; - if (canId != 0) { - payload[index++] = {COMM_FORWARD_CAN}; - payload[index++] = canId; - } - payload[index++] = {COMM_GET_VALUES}; - - packSendPayload(payload, payloadSize); -} -bool VescUart::parseVescValues() { - uint8_t message[256]; - int messageLength = receiveUartMessage(message); - - if (messageLength > 55) { - return processReadPacket(message); - } - return false; -} -void VescUart::setNunchuckValues() { return setNunchuckValues(0); } - -void VescUart::setNunchuckValues(uint8_t canId) { - int32_t index = 0; - int payloadSize = (canId == 0 ? 11 : 13); - // 13 = max payload size - uint8_t payload[13]; - - if (canId != 0) { - payload[index++] = {COMM_FORWARD_CAN}; - payload[index++] = canId; - } - payload[index++] = {COMM_SET_CHUCK_DATA}; - payload[index++] = nunchuck.valueX; - payload[index++] = nunchuck.valueY; - buffer_append_bool(payload, nunchuck.lowerButton, &index); - buffer_append_bool(payload, nunchuck.upperButton, &index); - - // Acceleration Data. Not used, Int16 (2 byte) - payload[index++] = 0; - payload[index++] = 0; - payload[index++] = 0; - payload[index++] = 0; - payload[index++] = 0; - payload[index++] = 0; - - packSendPayload(payload, payloadSize); -} - -void VescUart::setCurrent(float current) { return setCurrent(current, 0); } - -void VescUart::setCurrent(float current, uint8_t canId) { - int32_t index = 0; - int payloadSize = (canId == 0 ? 5 : 7); - // 7 = max payload size - uint8_t payload[7]; - if (canId != 0) { - payload[index++] = {COMM_FORWARD_CAN}; - payload[index++] = canId; - } - payload[index++] = {COMM_SET_CURRENT}; - buffer_append_int32(payload, (int32_t)(current * 1000), &index); - packSendPayload(payload, payloadSize); -} - -void VescUart::setBrakeCurrent(float brakeCurrent) { - return setBrakeCurrent(brakeCurrent, 0); -} - -void VescUart::setBrakeCurrent(float brakeCurrent, uint8_t canId) { - int32_t index = 0; - int payloadSize = (canId == 0 ? 5 : 7); - // 7 = max payload size - uint8_t payload[7]; - if (canId != 0) { - payload[index++] = {COMM_FORWARD_CAN}; - payload[index++] = canId; - } - - payload[index++] = {COMM_SET_CURRENT_BRAKE}; - buffer_append_int32(payload, (int32_t)(brakeCurrent * 1000), &index); - - packSendPayload(payload, payloadSize); -} - -void VescUart::setRPM(float rpm) { return setRPM(rpm, 0); } - -void VescUart::setRPM(float rpm, uint8_t canId) { - int32_t index = 0; - int payloadSize = (canId == 0 ? 5 : 7); - // 7 = max payload size - uint8_t payload[7]; - if (canId != 0) { - payload[index++] = {COMM_FORWARD_CAN}; - payload[index++] = canId; - } - payload[index++] = {COMM_SET_RPM}; - buffer_append_int32(payload, (int32_t)(rpm), &index); - packSendPayload(payload, payloadSize); -} - -void VescUart::setDuty(float duty) { return setDuty(duty, 0); } - -void VescUart::setDuty(float duty, uint8_t canId) { - int32_t index = 0; - int payloadSize = (canId == 0 ? 5 : 7); - // 7 = max payload size - uint8_t payload[7]; - if (canId != 0) { - payload[index++] = {COMM_FORWARD_CAN}; - payload[index++] = canId; - } - payload[index++] = {COMM_SET_DUTY}; - buffer_append_int32(payload, (int32_t)(duty * 100000), &index); - - packSendPayload(payload, payloadSize); -} - -void VescUart::sendKeepalive(void) { return sendKeepalive(0); } - -void VescUart::sendKeepalive(uint8_t canId) { - int32_t index = 0; - int payloadSize = (canId == 0 ? 1 : 3); - // 3 = max payload size - uint8_t payload[3]; - if (canId != 0) { - payload[index++] = {COMM_FORWARD_CAN}; - payload[index++] = canId; - } - payload[index++] = {COMM_ALIVE}; - packSendPayload(payload, payloadSize); -} -void VescUart::sendRaw(uint8_t* data, size_t size) { - sdWrite(uart_, data, size); -} diff --git a/src/drivers/vesc/VescUart.h b/src/drivers/vesc/VescUart.h deleted file mode 100644 index a19aa80..0000000 --- a/src/drivers/vesc/VescUart.h +++ /dev/null @@ -1,231 +0,0 @@ -#ifndef _VESCUART_h -#define _VESCUART_h - -#include "buffer.h" -#include "ch.h" -#include "datatypes.h" -#include "hal.h" - -class VescUart { - /** Struct to store the telemetry data returned by the VESC */ - struct dataPackage { - float avgMotorCurrent; - float avgInputCurrent; - float dutyCycleNow; - float rpm; - float inpVoltage; - float ampHours; - float ampHoursCharged; - float wattHours; - float wattHoursCharged; - long tachometer; - long tachometerAbs; - float tempMosfet; - float tempMotor; - float pidPos; - uint8_t id; - mc_fault_code error; - }; - - /** Struct to hold the nunchuck values to send over UART */ - struct nunchuckPackage { - int valueX; - int valueY; - bool upperButton; // valUpperButton - bool lowerButton; // valLowerButton - }; - - struct FWversionPackage { - uint8_t major; - uint8_t minor; - }; - - // Timeout - specifies how long the function will wait for the vesc to respond - const uint32_t _TIMEOUT; - - public: - /** - * @brief Class constructor - */ - explicit VescUart(SerialDriver* uart, uint32_t timeout_ms = 100); - - bool startDriver(); - - /** Variabel to hold measurements returned from VESC */ - dataPackage data; - - /** Variabel to hold nunchuck values */ - nunchuckPackage nunchuck; - - /** Variable to hold firmware version */ - FWversionPackage fw_version; - - /** - * @brief Populate the firmware version variables - * - * @return True if successfull otherwise false - */ - bool getFWversion(void); - - /** - * @brief Populate the firmware version variables - * - * @param canId - The CAN ID of the VESC - * @return True if successfull otherwise false - */ - bool getFWversion(uint8_t canId); - - /** - * @brief Sends a command to VESC and stores the returned data - * - * @return True if successfull otherwise false - */ - bool getVescValues(void); - - /** - * @brief Sends a command to VESC and stores the returned data - * @param canId - The CAN ID of the VESC - * - * @return True if successfull otherwise false - */ - bool getVescValues(uint8_t canId); - /** - * @brief Sends a command to VESC to send the data. Read with - * parseVescValues() - * - * @return True if successfull otherwise false - */ - void requestVescValues(void); - - /** - * @brief Sends a command to VESC to send the data. Read with - * parseVescValues() - * @param canId - The CAN ID of the VESC - * - * @return True if successfull otherwise false - */ - void requestVescValues(uint8_t canId); - - bool parseVescValues(); - - /** - * @brief Sends values for joystick and buttons to the nunchuck app - */ - void setNunchuckValues(void); - /** - * @brief Sends values for joystick and buttons to the nunchuck app - * @param canId - The CAN ID of the VESC - */ - void setNunchuckValues(uint8_t canId); - - /** - * @brief Set the current to drive the motor - * @param current - The current to apply - */ - void setCurrent(float current); - - /** - * @brief Set the current to drive the motor - * @param current - The current to apply - * @param canId - The CAN ID of the VESC - */ - void setCurrent(float current, uint8_t canId); - - /** - * @brief Set the current to brake the motor - * @param brakeCurrent - The current to apply - */ - void setBrakeCurrent(float brakeCurrent); - - /** - * @brief Set the current to brake the motor - * @param brakeCurrent - The current to apply - * @param canId - The CAN ID of the VESC - */ - void setBrakeCurrent(float brakeCurrent, uint8_t canId); - - /** - * @brief Set the rpm of the motor - * @param rpm - The desired RPM (actually eRPM = RPM * poles) - */ - void setRPM(float rpm); - - /** - * @brief Set the rpm of the motor - * @param rpm - The desired RPM (actually eRPM = RPM * poles) - * @param canId - The CAN ID of the VESC - */ - void setRPM(float rpm, uint8_t canId); - - /** - * @brief Set the duty of the motor - * @param duty - The desired duty (0.0-1.0) - */ - void setDuty(float duty); - - /** - * @brief Set the duty of the motor - * @param duty - The desired duty (0.0-1.0) - * @param canId - The CAN ID of the VESC - */ - void setDuty(float duty, uint8_t canId); - - /** - * @brief Send a keepalive message - */ - void sendKeepalive(void); - - /** - * @brief Send a keepalive message - * @param canId - The CAN ID of the VESC - */ - void sendKeepalive(uint8_t canId); - - /** - * @brief Help Function to print struct dataPackage over Serial for Debug - */ - void printVescValues(void); - - private: - SerialDriver* uart_; - SerialConfig serial_config_{}; - void sendRaw(uint8_t* data, size_t size); - - /** - * @brief Packs the payload and sends it over Serial - * - * @param payload - The payload as a unit8_t Array with length of int - * lenPayload - * @param lenPay - Length of payload - * @return The number of bytes send - */ - int packSendPayload(uint8_t* payload, int lenPay); - - /** - * @brief Receives the message over Serial - * - * @param payloadReceived - The received payload as a unit8_t Array - * @return The number of bytes receeived within the payload - */ - int receiveUartMessage(uint8_t* payloadReceived); - - /** - * @brief Verifies the message (CRC-16) and extracts the payload - * - * @param message - The received UART message - * @param lenMes - The lenght of the message - * @param payload - The final payload ready to extract data from - * @return True if the process was a success - */ - bool unpackPayload(uint8_t* message, int lenMes, uint8_t* payload); - - /** - * @brief Extracts the data from the received payload - * - * @param message - The payload to extract data from - * @return True if the process was a success - */ - bool processReadPacket(uint8_t* message); -}; - -#endif diff --git a/src/drivers/vesc/esc_status.h b/src/drivers/vesc/esc_status.h deleted file mode 100644 index f34b955..0000000 --- a/src/drivers/vesc/esc_status.h +++ /dev/null @@ -1,14 +0,0 @@ -// -// Created by clemens on 05.11.24. -// - -#ifndef ESC_STATUS_H -#define ESC_STATUS_H -enum { - ESC_STATUS_DISCONNECTED = 99u, - ESC_STATUS_ERROR = 100u, - ESC_STATUS_STALLED = 150u, - ESC_STATUS_OK = 200u, - ESC_STATUS_RUNNING = 201u, -}; -#endif // ESC_STATUS_H diff --git a/src/main.cpp b/src/main.cpp index f57456b..ee9c6b4 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -9,6 +9,8 @@ #endif #include #include +#include +#include #include #include #include @@ -17,12 +19,12 @@ #include #include #include -#include -#include "services/diff_drive_service/diff_drive_service.hpp" + #include "services/emergency_service/emergency_service.hpp" #include "services/imu_service/imu_service.hpp" -#include "services/mower_service/mower_service.hpp" #include "services/power_service/power_service.hpp" +#include "services/diff_drive_service/diff_drive_service.hpp" +#include "services/mower_service/mower_service.hpp" EmergencyService emergency_service{1}; DiffDriveService diff_drive{2}; MowerService mower_service{3}; @@ -32,7 +34,7 @@ PowerService power_service{5}; /* * Application entry point. */ -int main(void) { +int main() { #ifdef RELEASE_BUILD // Reset the boot register for a release build, so that the chip // resets to bootloader @@ -95,6 +97,8 @@ int main(void) { diff_drive.start(); mower_service.start(); + + // Subscribe to global events and dispatch to our services event_listener_t event_listener; chEvtRegister(&mower_events, &event_listener, 1); diff --git a/src/services/diff_drive_service/diff_drive_service.cpp b/src/services/diff_drive_service/diff_drive_service.cpp index c6c470e..486299e 100644 --- a/src/services/diff_drive_service/diff_drive_service.cpp +++ b/src/services/diff_drive_service/diff_drive_service.cpp @@ -4,15 +4,12 @@ #include "diff_drive_service.hpp" -#include - #include #include "../../globals.hpp" void DiffDriveService::OnMowerStatusChanged(uint32_t new_status) { - if ((new_status & - (MOWER_FLAG_EMERGENCY_LATCH | MOWER_FLAG_EMERGENCY_ACTIVE)) == 0) { + if ((new_status & (MOWER_FLAG_EMERGENCY_LATCH | MOWER_FLAG_EMERGENCY_ACTIVE)) == 0) { // only set speed to 0 if the emergency happens, not if it's cleared return; } @@ -25,8 +22,8 @@ void DiffDriveService::OnMowerStatusChanged(uint32_t new_status) { } bool DiffDriveService::Configure() { // Check, if configuration is valid, if not retry - if (!WheelDistance.valid || !WheelTicksPerMeter.valid || - WheelDistance.value == 0 || WheelTicksPerMeter.value == 0.0) { + if (!WheelDistance.valid || !WheelTicksPerMeter.valid || WheelDistance.value == 0 || + WheelTicksPerMeter.value == 0.0) { return false; } // It's fine, we don't actually need to configure anything @@ -37,8 +34,16 @@ void DiffDriveService::OnStart() { last_ticks_valid = false; } void DiffDriveService::OnCreate() { - left_uart_.startDriver(); - right_uart_.startDriver(); + using namespace xbot::driver::esc; + // Register callbacks + left_esc_driver_.SetStateCallback( + etl::delegate::create( + *this)); + right_esc_driver_.SetStateCallback( + etl::delegate::create( + *this)); + left_esc_driver_.StartDriver(&UARTD1, 115200); + right_esc_driver_.StartDriver(&UARTD4, 115200); } void DiffDriveService::OnStop() { speed_l_ = speed_r_ = 0; @@ -52,61 +57,21 @@ void DiffDriveService::tick() { SetDuty(); } - uint32_t micros = xbot::service::system::getTimeMicros(); - left_uart_.requestVescValues(); - right_uart_.requestVescValues(); - - bool parse_left_success = left_uart_.parseVescValues(); - bool parse_right_success = right_uart_.parseVescValues(); - - StartTransaction(); - - if (parse_left_success) { - SendLeftESCTemperature(left_uart_.data.tempMosfet); - SendLeftESCCurrent(left_uart_.data.avgMotorCurrent); - SendLeftESCStatus(left_uart_.data.error == 0 ? ESC_STATUS_OK - : ESC_STATUS_ERROR); - } else { - SendLeftESCStatus(ESC_STATUS_DISCONNECTED); - } + left_esc_driver_.RequestStatus(); + right_esc_driver_.RequestStatus(); - if (parse_right_success) { - SendRightESCTemperature(right_uart_.data.tempMosfet); - SendRightESCCurrent(right_uart_.data.avgMotorCurrent); - SendRightESCStatus(left_uart_.data.error == 0 ? ESC_STATUS_OK - : ESC_STATUS_ERROR); - } else { - SendRightESCStatus(ESC_STATUS_DISCONNECTED); - } - - if (parse_left_success && parse_right_success) { - // Calculate the twist according to wheel ticks - if (last_ticks_valid) { - float dt = static_cast(micros - last_ticks_micros_) / 1000000.0f; - auto d_left = - static_cast(left_uart_.data.tachometer - last_ticks_left); - auto d_right = - static_cast(right_uart_.data.tachometer - last_ticks_right); - float vx = (d_right - d_left) / - (2.0f * dt * static_cast(WheelTicksPerMeter.value)); - float vr = (d_left + d_right) / - (2.0f * dt * static_cast(WheelTicksPerMeter.value)); - double data[6]{}; - data[0] = vx; - data[5] = vr; - SendActualTwist(data, 6); - uint32_t ticks[2]; - ticks[0] = left_uart_.data.tachometer; - ticks[1] = right_uart_.data.tachometer; - SendWheelTicks(ticks, 2); + // Check, if we have received ESC status updates recently. If not, send a disconnected message + if (xbot::service::system::getTimeMicros() - last_valid_esc_state_micros_ > 1000000) { + StartTransaction(); + if (!left_esc_state_valid_) { + SendLeftESCStatus(static_cast(VescDriver::ESCState::ESCStatus::ESC_STATUS_DISCONNECTED)); + } + if (!right_esc_state_valid_) { + SendRightESCStatus(static_cast(VescDriver::ESCState::ESCStatus::ESC_STATUS_DISCONNECTED)); } - last_ticks_valid = true; - last_ticks_left = left_uart_.data.tachometer; - last_ticks_right = right_uart_.data.tachometer; - last_ticks_micros_ = micros; + CommitTransaction(); } - CommitTransaction(); duty_sent_ = false; chMtxUnlock(&mtx); } @@ -116,17 +81,72 @@ void DiffDriveService::SetDuty() { uint32_t status_copy = mower_status; chMtxUnlock(&mower_status_mutex); if (status_copy & MOWER_FLAG_EMERGENCY_LATCH) { - left_uart_.setDuty(0); - right_uart_.setDuty(0); + left_esc_driver_.SetDuty(0); + right_esc_driver_.SetDuty(0); } else { - left_uart_.setDuty(speed_l_); - right_uart_.setDuty(speed_r_); + left_esc_driver_.SetDuty(speed_l_); + right_esc_driver_.SetDuty(speed_r_); } duty_sent_ = true; } +void DiffDriveService::LeftESCCallback(const VescDriver::ESCState& state) { + chMtxLock(&mtx); + left_esc_state_ = state; + left_esc_state_valid_ = true; + if (right_esc_state_valid_) { + ProcessStatusUpdate(); + } + chMtxUnlock(&mtx); +} +void DiffDriveService::RightESCCallback(const VescDriver::ESCState& state) { + chMtxLock(&mtx); + right_esc_state_ = state; + right_esc_state_valid_ = true; + if (left_esc_state_valid_) { + ProcessStatusUpdate(); + } + chMtxUnlock(&mtx); +} + +void DiffDriveService::ProcessStatusUpdate() { + uint32_t micros = xbot::service::system::getTimeMicros(); + last_valid_esc_state_micros_ = micros; + StartTransaction(); + SendLeftESCTemperature(left_esc_state_.temperature_pcb); + SendLeftESCCurrent(left_esc_state_.current_input); + SendLeftESCStatus(static_cast(left_esc_state_.status)); + + SendRightESCTemperature(right_esc_state_.temperature_pcb); + SendRightESCCurrent(right_esc_state_.current_input); + SendRightESCStatus(static_cast(right_esc_state_.status)); + + // Calculate the twist according to wheel ticks + if (last_ticks_valid) { + float dt = static_cast(micros - last_ticks_micros_) / 1000000.0f; + auto d_left = static_cast(left_esc_state_.tacho - last_ticks_left); + auto d_right = static_cast(right_esc_state_.tacho - last_ticks_right); + float vx = (d_right - d_left) / (2.0f * dt * static_cast(WheelTicksPerMeter.value)); + float vr = (d_left + d_right) / (2.0f * dt * static_cast(WheelTicksPerMeter.value)); + double data[6]{}; + data[0] = vx; + data[5] = vr; + SendActualTwist(data, 6); + uint32_t ticks[2]; + ticks[0] = left_esc_state_.tacho; + ticks[1] = right_esc_state_.tacho; + SendWheelTicks(ticks, 2); + } + last_ticks_valid = true; + last_ticks_left = left_esc_state_.tacho; + last_ticks_right = right_esc_state_.tacho; + last_ticks_micros_ = micros; + + right_esc_state_valid_ = left_esc_state_valid_ = false; + + CommitTransaction(); +} -bool DiffDriveService::OnControlTwistChanged(const double* new_value, - uint32_t length) { +bool DiffDriveService::OnControlTwistChanged(const double* new_value, uint32_t length) { if (length != 6) return false; chMtxLock(&mtx); // we can only do forward and rotation around one axis @@ -135,8 +155,7 @@ bool DiffDriveService::OnControlTwistChanged(const double* new_value, // TODO: update this to rad/s values and implement xESC speed control speed_r_ = linear + 0.5f * static_cast(WheelDistance.value) * angular; - speed_l_ = - -(linear - 0.5f * static_cast(WheelDistance.value) * angular); + speed_l_ = -(linear - 0.5f * static_cast(WheelDistance.value) * angular); if (speed_l_ >= 1.0) { speed_l_ = 1.0; diff --git a/src/services/diff_drive_service/diff_drive_service.hpp b/src/services/diff_drive_service/diff_drive_service.hpp index de2aefc..c06d7da 100644 --- a/src/services/diff_drive_service/diff_drive_service.hpp +++ b/src/services/diff_drive_service/diff_drive_service.hpp @@ -5,32 +5,39 @@ #ifndef DIFF_DRIVE_SERVICE_HPP #define DIFF_DRIVE_SERVICE_HPP -#include #include #include +#include +using namespace xbot::driver::esc; class DiffDriveService : public DiffDriveServiceBase { private: THD_WORKING_AREA(wa, 1024); - VescUart left_uart_{&SD1}; - VescUart right_uart_{&SD4}; + VescDriver left_esc_driver_{}; + VescDriver right_esc_driver_{}; - long last_ticks_left = 0; - long last_ticks_right = 0; + VescDriver::ESCState left_esc_state_{}; + VescDriver::ESCState right_esc_state_{}; + bool left_esc_state_valid_ = false; + bool right_esc_state_valid_ = false; + uint32_t last_valid_esc_state_micros_ = 0; + + uint32_t last_ticks_left = 0; + uint32_t last_ticks_right = 0; bool last_ticks_valid = false; uint32_t last_ticks_micros_ = 0; - mutex_t mtx{}; + MUTEX_DECL(mtx); float speed_l_ = 0; float speed_r_ = 0; - bool duty_sent_ = false; + + public: explicit DiffDriveService(uint16_t service_id) : DiffDriveServiceBase(service_id, 20000, wa, sizeof(wa)) { - chMtxObjectInit(&mtx); } void OnMowerStatusChanged(uint32_t new_status); @@ -46,6 +53,10 @@ class DiffDriveService : public DiffDriveServiceBase { void SetDuty(); + void LeftESCCallback(const VescDriver::ESCState &state); + void RightESCCallback(const VescDriver::ESCState &state); + void ProcessStatusUpdate(); + protected: bool OnControlTwistChanged(const double* new_value, uint32_t length) override; }; diff --git a/src/services/mower_service/mower_service.cpp b/src/services/mower_service/mower_service.cpp index 9e1c8bf..31cceb4 100644 --- a/src/services/mower_service/mower_service.cpp +++ b/src/services/mower_service/mower_service.cpp @@ -4,7 +4,6 @@ #include "mower_service.hpp" -#include #include @@ -12,7 +11,7 @@ bool MowerService::Configure() { // No configuration needed return true; } -void MowerService::OnCreate() { mower_uart_.startDriver(); } +void MowerService::OnCreate() { mower_driver_.StartDriver(&UARTD2, 115200); } void MowerService::OnStart() { mower_duty_ = 0; } void MowerService::OnStop() { mower_duty_ = 0; } @@ -24,20 +23,6 @@ void MowerService::tick() { SetDuty(); } - mower_uart_.requestVescValues(); - - bool parse_success = mower_uart_.parseVescValues(); - - StartTransaction(); - if (parse_success) { - SendMowerESCTemperature(mower_uart_.data.tempMosfet); - SendMowerMotorCurrent(mower_uart_.data.avgMotorCurrent); - SendMowerStatus(mower_uart_.data.error == 0 ? ESC_STATUS_OK - : ESC_STATUS_ERROR); - } else { - SendMowerStatus(ESC_STATUS_DISCONNECTED); - } - CommitTransaction(); duty_sent_ = false; chMtxUnlock(&mtx); @@ -49,9 +34,9 @@ void MowerService::SetDuty() { uint32_t status_copy = mower_status; chMtxUnlock(&mower_status_mutex); if (status_copy & MOWER_FLAG_EMERGENCY_LATCH) { - mower_uart_.setDuty(0); + mower_driver_.SetDuty(0); } else { - mower_uart_.setDuty(mower_duty_); + mower_driver_.SetDuty(mower_duty_); } duty_sent_ = true; } @@ -83,6 +68,5 @@ void MowerService::OnMowerStatusChanged(uint32_t new_status) { } MowerService::MowerService(const uint16_t service_id) - : MowerServiceBase(service_id, 1000000, wa, sizeof(wa)), mower_uart_(&SD2) { - chMtxObjectInit(&mtx); + : MowerServiceBase(service_id, 1000000, wa, sizeof(wa)) { } diff --git a/src/services/mower_service/mower_service.hpp b/src/services/mower_service/mower_service.hpp index 2628634..73fab2c 100644 --- a/src/services/mower_service/mower_service.hpp +++ b/src/services/mower_service/mower_service.hpp @@ -6,7 +6,7 @@ #define MOWER_SERVICE_HPP #include -#include +#include #include @@ -25,7 +25,7 @@ class MowerService : public MowerServiceBase { private: void tick() override; void SetDuty(); - mutex_t mtx{}; + MUTEX_DECL(mtx); protected: bool OnMowerEnabledChanged(const uint8_t& new_value) override; @@ -34,7 +34,7 @@ class MowerService : public MowerServiceBase { THD_WORKING_AREA(wa, 1024){}; float mower_duty_ = 0; bool duty_sent_ = false; - VescUart mower_uart_; + xbot::driver::esc::VescDriver mower_driver_{}; }; #endif // MOWER_SERVICE_HPP