diff --git a/.vscode/settings.json b/.vscode/settings.json index 2aac76e..1ab400e 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -5,4 +5,12 @@ }, "git.detectSubmodules": false, "cmake.sourceDirectory": "${workspaceFolder}/src/seahawk", + "files.associations": { + "stdio.h": "c", + "rmw_microros.h": "c", + "int32.h": "c", + "int16_multi_array.h": "c", + "pico_uart_transports.h": "c", + "stdint.h": "c" + }, } diff --git a/src/seahawk/CMakeLists.txt b/src/seahawk/CMakeLists.txt index 75c7bed..4d8d63b 100644 --- a/src/seahawk/CMakeLists.txt +++ b/src/seahawk/CMakeLists.txt @@ -11,12 +11,16 @@ pico_sdk_init() link_directories($ENV{MICRO_ROS_RPI_PICO_PATH}/libmicroros) add_executable(seahawk + crc.c + uart_proto.c seahawk.c pico_uart_transport.c ) target_link_libraries(seahawk pico_stdlib microros + hardware_pwm + hardware_pio ) target_include_directories(seahawk PUBLIC diff --git a/src/seahawk/crc.c b/src/seahawk/crc.c new file mode 100644 index 0000000..a0ca232 --- /dev/null +++ b/src/seahawk/crc.c @@ -0,0 +1,44 @@ +#include +#include +#include + +#include "crc.h" + +uint8_t get_crc_byte(const uint8_t *buffer, uint16_t length) { + uint8_t retVal = 0u; + uint16_t byteIndex = 0u; + + if (buffer != NULL) { + uint8_t bitIndex = 0u; + retVal = CRC_8_INIT_VALUE; + + // Do calculation procedure for each byte + for (byteIndex = 0u; byteIndex < length; byteIndex++) { + // XOR new byte with temp result + retVal ^= (buffer[byteIndex] << (CRC_8_RESULT_WIDTH - 8u)); + // Do calculation for current data + for (bitIndex = 0u; bitIndex < 8u; bitIndex++) { + if (retVal & (1u << (CRC_8_RESULT_WIDTH - 1u))) { + retVal = (retVal << 1u) ^ CRC_8_POLYNOMIAL; + } + else { + retVal = (retVal << 1u); + } + } + } + // XOR result with specified value + retVal ^= CRC_8_XOR_VALUE; + } + return retVal; +} + +void add_crc_byte(uint8_t *buffer, uint16_t length) { + uint8_t crc = get_crc_byte(buffer, length); + // Increase buffer size by 1 + buffer = (uint8_t *) realloc(buffer, length + 1); + buffer[length] = crc; +} + +bool check_crc(const uint8_t *buffer, uint16_t length_no_crc) { + return get_crc_byte(buffer, length_no_crc) == buffer[length_no_crc]; +} diff --git a/src/seahawk/crc.h b/src/seahawk/crc.h new file mode 100644 index 0000000..e323cd1 --- /dev/null +++ b/src/seahawk/crc.h @@ -0,0 +1,19 @@ +#pragma once + +#include + +// Constants used in CRC generation + +#define CRC_8_RESULT_WIDTH 8u +#define CRC_8_POLYNOMIAL 0x1Du +#define CRC_8_INIT_VALUE 0xFFu +#define CRC_8_XOR_VALUE 0xFFu + +// Given a buffer, generate a CRC byte. +uint8_t get_crc_byte(const uint8_t *buffer, uint16_t length); + +// Given a buffer, append a CRC byte. +void add_crc_byte(uint8_t *buffer, uint16_t length); + +// Length should be of buffer without the crc +bool check_crc(const uint8_t *buffer, uint16_t length_no_crc); diff --git a/src/seahawk/pico_uart_transport.c b/src/seahawk/pico_uart_transport.c index 84f784a..9eb0b38 100755 --- a/src/seahawk/pico_uart_transport.c +++ b/src/seahawk/pico_uart_transport.c @@ -2,6 +2,7 @@ #include "pico/stdlib.h" #include +#include void usleep(uint64_t us) { @@ -19,6 +20,11 @@ int clock_gettime(clockid_t unused, struct timespec *tp) bool pico_serial_transport_open(struct uxrCustomTransport * transport) { stdio_init_all(); + uart_init(uart0, 115200); + gpio_set_function(0, GPIO_FUNC_UART); + gpio_set_function(1, GPIO_FUNC_UART); + uart_set_hw_flow(uart0, false, false); + uart_set_format(uart0, 8, 1, UART_PARITY_NONE); return true; } diff --git a/src/seahawk/seahawk.c b/src/seahawk/seahawk.c index 4eb0e42..4c45d0d 100755 --- a/src/seahawk/seahawk.c +++ b/src/seahawk/seahawk.c @@ -5,24 +5,99 @@ #include #include #include +#include +#include #include #include "pico/stdlib.h" #include "pico_uart_transports.h" +#include "hardware/pio.h" +#include "hardware/pwm.h" + +#define BMS 0 +#include "uart_proto.h" +#include "crc.h" + +const uart_inst inst = {uart0, 0, 1, 115200}; const uint LED_PIN = 25; rcl_publisher_t publisher; -std_msgs__msg__Int32 msg; +rcl_subscription_t subscriber; +const uint GPIO[8] = {20, 13, 19, 11, 21, 12, 18, 10}; +const int NUM_MOTORS = 8; +const uint8_t CLOCK_DIV = 125; + +// configures the pwm generators +void config_pwm(uint16_t wrap) { + for (int i = 0; i < NUM_MOTORS; i++) { + // Sets all GPIO pins to use PWM + gpio_set_function(GPIO[i], GPIO_FUNC_PWM); + // Sets the wrap for each slice + pwm_set_wrap(pwm_gpio_to_slice_num(GPIO[i]), wrap); + /* + * If phase correct is set to false the counter + * will reset to 0 after reaching the level, + * otherwise it will decrease down to 0 + */ + pwm_set_phase_correct(pwm_gpio_to_slice_num(GPIO[i]), false); + // Set clock division + pwm_set_clkdiv_int_frac(pwm_gpio_to_slice_num(GPIO[i]), CLOCK_DIV, 0); + // Set pins to 1500microseconds for neutral + pwm_set_gpio_level(GPIO[i], 1500); + // Enable PWM + pwm_set_enabled(pwm_gpio_to_slice_num(GPIO[i]), true); + } +} +// Sets all of the motors to the given levels +void set_duty_cycle(uint16_t levels[]) { + for (int i = 0; i < NUM_MOTORS; i++) { + pwm_set_gpio_level(GPIO[i], levels[i]); + } +} + +int mothertrucker = 0; + +void timer_callback(rcl_timer_t * timer, int64_t last_call_time) { + sensor_data retval; + send_request(inst); + get_response(inst, &retval); + std_msgs__msg__Float64MultiArray msg; + double float_data[9] = { + (double) retval.kill, + (double) retval.ina780_current, + (double) retval.ina780_voltage, + (double) retval.ina780_temperature, + (double) retval.ina780_power, + (double) retval.ina780_energy, + (double) retval.bme280_temperature, + (double) retval.bme280_hum, + (double) retval.bme280_press, + }; + msg.data.data = float_data; + msg.data.size = 9; + msg.data.capacity = 9; + msg.layout.dim.size = 0; + msg.layout.dim.capacity = 0; + msg.layout.dim.data = NULL; + msg.layout.data_offset = 0; + mothertrucker = (mothertrucker + 1) % 2; + gpio_put(LED_PIN, mothertrucker); + rcl_ret_t ret = rcl_publish(&publisher, &msg, NULL); // ignore +} -void timer_callback(rcl_timer_t *timer, int64_t last_call_time) +void subscription_callback(const void * msgin) { - rcl_ret_t ret = rcl_publish(&publisher, &msg, NULL); - msg.data++; + // Set the msgin to a Int16MultiArray + std_msgs__msg__Int16MultiArray *msg = (std_msgs__msg__Int16MultiArray *) msgin; + set_duty_cycle((uint16_t *) msg->data.data); } int main() { + // const rosidl_message_type_support_t * type_support = + // ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int16MultiArray); + rmw_uros_set_custom_transport( true, NULL, @@ -35,7 +110,6 @@ int main() gpio_init(LED_PIN); gpio_set_dir(LED_PIN, GPIO_OUT); - rcl_timer_t timer; rcl_node_t node; rcl_allocator_t allocator; rclc_support_t support; @@ -55,27 +129,96 @@ int main() return ret; } - rclc_support_init(&support, 0, NULL, &allocator); + config_pwm(3000); + rclc_support_init(&support, 0, NULL, &allocator); + rclc_node_init_default(&node, "pico_node", "", &support); - rclc_publisher_init_default( - &publisher, + + // Define pwm_msg + std_msgs__msg__Int16MultiArray pwm_msg; + + // Define msg data Int16 Sequence + rosidl_runtime_c__int16__Sequence pwm_msg_data; + pwm_msg_data.size = 0; + pwm_msg_data.capacity = 8; + int16_t pwm_msg_data_data[8] = {0, 0, 0, 0, 0, 0, 0, 0}; + pwm_msg_data.data = pwm_msg_data_data; + + pwm_msg.data = pwm_msg_data; + + // Define msg layout MultiArray Layout + + std_msgs__msg__MultiArrayLayout pwm_msg_layout; + + std_msgs__msg__MultiArrayDimension__Sequence pwm_msg_layout_dim; + pwm_msg_layout_dim.size = 0; + pwm_msg_layout_dim.capacity = 0; + pwm_msg_layout_dim.data = NULL; + + pwm_msg_layout.dim = pwm_msg_layout_dim; + pwm_msg_layout.data_offset = 0; + + pwm_msg.layout = pwm_msg_layout; + + // Define bms_msg + std_msgs__msg__Float64MultiArray bms_msg; + + // Define bms_msg data Float64 Sequence + rosidl_runtime_c__double__Sequence bms_msg_data; + bms_msg_data.size = 0; + bms_msg_data.capacity = 9; + double bms_msg_data_data[9] = {0, 0, 0, 0, 0, 0, 0, 0, 0}; + bms_msg_data.data = bms_msg_data_data; + + bms_msg.data = bms_msg_data; + + // Define bms_msg_layout + + std_msgs__msg__MultiArrayLayout bms_msg_layout; + + std_msgs__msg__MultiArrayDimension__Sequence bms_msg_layout_dim; + bms_msg_layout_dim.size = 0; + bms_msg_layout_dim.capacity = 0; + bms_msg_layout_dim.data = NULL; + + bms_msg_layout.dim = bms_msg_layout_dim; + bms_msg_layout.data_offset = 0; + + bms_msg.layout = bms_msg_layout; + + uart_initialization(inst); + + const unsigned int timer_period = RCL_MS_TO_NS(1000); + rcl_timer_t timer; + rcl_ret_t rc = rclc_timer_init_default(&timer, &support, timer_period, timer_callback); + + + ret = rclc_subscription_init_default( + &subscriber, &node, - ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32), - "pico_publisher"); + ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int16MultiArray), + "pwm_values"); - rclc_timer_init_default( - &timer, - &support, - RCL_MS_TO_NS(1000), - timer_callback); + ret = rclc_publisher_init_default( + &publisher, + &node, + ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Float64MultiArray), + "bms_data"); rclc_executor_init(&executor, &support.context, 1, &allocator); - rclc_executor_add_timer(&executor, &timer); + rclc_executor_add_subscription( + &executor, + &subscriber, + &pwm_msg, + &subscription_callback, + ON_NEW_DATA); + + rc = rclc_executor_add_timer(&executor, &timer); gpio_put(LED_PIN, 1); - msg.data = 0; + while (true) { rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100)); diff --git a/src/seahawk/uart_proto.c b/src/seahawk/uart_proto.c new file mode 100644 index 0000000..a633459 --- /dev/null +++ b/src/seahawk/uart_proto.c @@ -0,0 +1,130 @@ +#include +#include +#include +#include +#include + +#include "uart_proto.h" +#include "crc.h" + +bool uart_initialization(uart_inst inst) { + gpio_set_function(inst.tx, GPIO_FUNC_UART); + gpio_set_function(inst.rx, GPIO_FUNC_UART); + uart_init(inst.uart, inst.baudrate); + return 1; +} + +bool get_response(uart_inst inst, sensor_data* retval) { + uint8_t* resp = (uint8_t *) malloc(HEADER_SIZE); + + if (resp == NULL) return false; + + uart_read_blocking(inst.uart, resp, HEADER_SIZE); + + msg_type type = resp[0]; + uint8_t byte_count = resp[1]; + + uint8_t* new_resp = (uint8_t *) realloc(resp, HEADER_CRC_SIZE + byte_count); + if (new_resp == NULL) { + free(resp); // avoid leak if realloc fails + return false; + } + resp = new_resp; + + uart_read_blocking(inst.uart, resp + PAYLOAD_OFFSET, CRC_SIZE + byte_count); + + bool success = true; + + switch(type) { + case data: + if (byte_count != DATA_PAYLOAD_SIZE || !check_crc(resp, DATA_SIZE_NO_CRC)) { + success = false; + } else { + *retval = parse_data(resp); + break; + } + case request: +#if BMS == 1 + if (byte_count != REQUEST_PAYLOAD_SIZE || !check_crc(resp, REQUEST_SIZE_NO_CRC)) { + success = false; + } else { + send_data(inst); + } + break; +#else + success = false; + break; +#endif + default: + success = false; + break; + } + free(resp); + return success; + +} + +bool send_request(uart_inst inst) { + uint8_t* msg = (uint8_t *) malloc(REQUEST_SIZE_NO_CRC); + + if (msg == NULL) return false; + + msg[0] = request; + msg[1] = REQUEST_PAYLOAD_SIZE; + + add_crc_byte(msg, REQUEST_SIZE_NO_CRC); + uart_write_blocking(inst.uart, msg, REQUEST_SIZE); + + free(msg); + + return true; +} + +#if BMS == 1 +bool send_data(uart_inst inst) { + // Make readings + float bme280_temperature; + float bme280_hum; + float bme280_press; + + bme280_read_all(&bme280_temperature, &bme280_hum, &bme280_press); + + sensor_data sen_data = { + gpio_kill_switch_triggered(), + ina780_read_current(), + ina780_read_bus_voltage(), + ina780_read_temperature(), + ina780_read_power(), + ina780_read_energy(), + bme280_temperature, + bme280_hum, + bme280_press, + }; + + // Initialize msg + uint8_t bytes = DATA_PAYLOAD_SIZE; + uint8_t* msg = (uint8_t *) malloc(DATA_SIZE_NO_CRC); // Header + byte count + payload + + if (msg == NULL) return 0; + + // Fill msg with data + msg[HEADER_OFFSET] = data; + msg[BYTE_COUNT_OFFSET] = bytes; + + memcpy(msg + PAYLOAD_OFFSET, &sen_data, bytes); + + // Add crc and transmit + add_crc_byte(msg, DATA_SIZE_NO_CRC); + uart_write_blocking(inst.uart, msg, DATA_SIZE); // Header + byte count + payload + crc + + free(msg); + + return true; +} +#endif + +sensor_data parse_data(uint8_t* resp) { + sensor_data retval; + memcpy(&retval, resp + PAYLOAD_OFFSET, DATA_PAYLOAD_SIZE); + return retval; +} diff --git a/src/seahawk/uart_proto.h b/src/seahawk/uart_proto.h new file mode 100644 index 0000000..188e853 --- /dev/null +++ b/src/seahawk/uart_proto.h @@ -0,0 +1,64 @@ +#pragma once + +#include +#include +#include "pico/stdlib.h" +#include "hardware/uart.h" + +// Data message size constants +#define DATA_SIZE 40 +#define DATA_PAYLOAD_SIZE 37 +#define DATA_SIZE_NO_CRC 39 + +// Request message size constants +#define REQUEST_SIZE 3 +#define REQUEST_PAYLOAD_SIZE 0 +#define REQUEST_SIZE_NO_CRC 2 + +#define HEADER_OFFSET 0 +#define BYTE_COUNT_OFFSET 1 +#define PAYLOAD_OFFSET 2 + +#define HEADER_SIZE 2 +#define CRC_SIZE 1 +#define HEADER_CRC_SIZE 3 + +typedef struct uart_inst{ + uart_inst_t* uart; + uint tx; + uint rx; + uint baudrate; + +} uart_inst; + +typedef enum msg_type{ + request, + data +} msg_type; + +// Could separate ina780 and bme280 readings +typedef struct sensor_data { + uint8_t kill; + float ina780_current; + float ina780_voltage; + float ina780_temperature; + float ina780_power; + double ina780_energy; + float bme280_temperature; + float bme280_hum; + float bme280_press; + +} sensor_data; + +bool uart_initialization(uart_inst inst); + +bool get_response(uart_inst inst, sensor_data* retval); + +// Request all data from BMS +bool send_request(uart_inst inst); + +// Return all data to Pi +bool send_data(uart_inst inst); + +// Read in data from BMS +sensor_data parse_data(uint8_t* data);