diff --git a/.clang-format b/.clang-format new file mode 100644 index 0000000..925c757 --- /dev/null +++ b/.clang-format @@ -0,0 +1,3 @@ +BasedOnStyle : Google +PointerAlignment: Left +DerivePointerAlignment: false diff --git a/components/ads1015/CMakeLists.txt b/components/ads1015/CMakeLists.txt index 0d8c30a..802ad45 100644 --- a/components/ads1015/CMakeLists.txt +++ b/components/ads1015/CMakeLists.txt @@ -1,5 +1,5 @@ idf_component_register( SRCS "ads1015.c" INCLUDE_DIRS "include" - REQUIRES esp_driver_i2c i2c_bus esp_driver_gpio + REQUIRES esp_driver_i2c i2c_bus esp_driver_gpio signal_bus ) \ No newline at end of file diff --git a/components/ads1015/ads1015.c b/components/ads1015/ads1015.c index 0dad4d0..ab9955f 100644 --- a/components/ads1015/ads1015.c +++ b/components/ads1015/ads1015.c @@ -1,164 +1,152 @@ #include "ads1015.h" + #include "driver/i2c_master.h" #include "esp_err.h" #include "esp_log.h" #include "freertos/FreeRTOS.h" +#include "signal_bus.h" #define TAG "ADS1015" -static TaskHandle_t adc_task_handle = NULL; +static TaskHandle_t adc_task_handle = NULL; -static void IRAM_ATTR ads1015_isr(void *arg){ - BaseType_t higher_priority_task_woken = pdFALSE; +static void IRAM_ATTR ads1015_isr(void* arg) { + BaseType_t higher_priority_task_woken = pdFALSE; - if (adc_task_handle){ - xTaskNotifyFromISR( - adc_task_handle, - 0, - eNoAction, - &higher_priority_task_woken - ); - } + if (adc_task_handle) { + xTaskNotifyFromISR(adc_task_handle, 0, eNoAction, + &higher_priority_task_woken); + } - if (higher_priority_task_woken) { - portYIELD_FROM_ISR(); - } + if (higher_priority_task_woken) { + portYIELD_FROM_ISR(); + } } -void adc_task(void *arg){ - ads1015_handle_t *handle = (ads1015_handle_t *)arg; - uint16_t config_reg = handle->config_reg | (1 << ADS1015_OS_BIT); // Modify so that writing will start conversions - bool mux_state = true; // Differential input to read (true = A2-A3, false = A0-A1) - - ads1015_write_register(handle, ADS1015_CONFIG, config_reg); // Start conversions - while (1) { - // Wait until RDY interrupt fires - ulTaskNotifyTake(pdTRUE, portMAX_DELAY); - - // Read conversion value - uint16_t raw; - if (ads1015_read_register(handle, ADS1015_CONVERSION, &raw) == ESP_OK){ - int16_t value = ((int16_t)raw) >> 4; - ads1015_check_current(value, mux_state); - } - - // Switch MUX inputs and start next conversion - config_reg &= ~(0b111 << ADS1015_MUX_SHIFT); // clear MUX bits - config_reg |= (mux_state ? (ADS1015_MUX_AIN0_AIN1 << ADS1015_MUX_SHIFT) - : (ADS1015_MUX_AIN2_AIN3 << ADS1015_MUX_SHIFT)); - mux_state = !mux_state; - - ads1015_write_register(handle, ADS1015_CONFIG, config_reg); +void adc_task(void* arg) { + ads1015_handle_t* handle = (ads1015_handle_t*)arg; + uint16_t config_reg = + handle->config_reg | + (1 << ADS1015_OS_BIT); // Modify so that writing will start conversions + bool mux_state = + true; // Differential input to read (true = A2-A3, false = A0-A1) + + ads1015_write_register(handle, ADS1015_CONFIG, + config_reg); // Start conversions + while (1) { + // Wait until RDY interrupt fires + ulTaskNotifyTake(pdTRUE, portMAX_DELAY); + + // Read conversion value + uint16_t raw; + if (ads1015_read_register(handle, ADS1015_CONVERSION, &raw) == ESP_OK) { + int16_t value = ((int16_t)raw) >> 4; + if (value >= CONFIG_ADS1015_HIGH_THRESH || + value <= CONFIG_ADS1015_LOW_THRESH) { + ESP_LOGI(TAG, "Overcurrent detection fired"); + xEventGroupSetBits(g_motor_events, CURRENT_0); + } } -} -esp_err_t ads1015_init(ads1015_handle_t *handle, const ads1015_config_t *config) { - ESP_LOGI(TAG, "Initializing ADS1015..."); + // Switch MUX inputs and start next conversion + config_reg &= ~(0b111 << ADS1015_MUX_SHIFT); // clear MUX bits + config_reg |= (mux_state ? (ADS1015_MUX_AIN0_AIN1 << ADS1015_MUX_SHIFT) + : (ADS1015_MUX_AIN2_AIN3 << ADS1015_MUX_SHIFT)); + mux_state = !mux_state; - if (handle == NULL || config == NULL) { - return ESP_ERR_INVALID_ARG; - } + ads1015_write_register(handle, ADS1015_CONFIG, config_reg); + } +} + +esp_err_t ads1015_init(ads1015_handle_t* handle, const ads1015_config_t* config) { + if (handle == NULL || config == NULL) { + return ESP_ERR_INVALID_ARG; + } - i2c_device_config_t dev_config = { + i2c_device_config_t dev_config = { .dev_addr_length = I2C_ADDR_BIT_LEN_7, .device_address = config->i2c_addr, .scl_speed_hz = config->i2c_speed_hz, - }; + }; - esp_err_t ret = i2c_master_bus_add_device(config->bus_handle, &dev_config, &handle->dev_handle); - if (ret != ESP_OK) { - return ret; - } + esp_err_t ret = i2c_master_bus_add_device(config->bus_handle, &dev_config, + &handle->dev_handle); + if (ret != ESP_OK) { + return ret; + } - // Set comparator threshold registers for RDY mode - ret = ads1015_write_register(handle, ADS1015_HIGH_THRESH_REG, 0x8000); - if (ret != ESP_OK) { - return ret; - } - ret = ads1015_write_register(handle, ADS1015_LOW_THRESH_REG, 0x0000); - if (ret != ESP_OK) { - return ret; - } + // Set comparator threshold registers for RDY mode + ret = ads1015_write_register(handle, ADS1015_HIGH_THRESH_REG, 0x8000); + if (ret != ESP_OK) { + return ret; + } + ret = ads1015_write_register(handle, ADS1015_LOW_THRESH_REG, 0x0000); + if (ret != ESP_OK) { + return ret; + } - // Write config register - uint16_t config_reg = ads1015_build_config( - ADS1015_MUX_AIN0_AIN1, - ADS1015_PGA_4_096V, - ADS1015_MODE_SINGLESHOT, - config->adc_data_rate, - ADS1015_COMP_TRADITIONAL, - ADS1015_COMP_ACTIVE_LOW, - ADS1015_COMP_NON_LATCHING, - ADS1015_COMP_ASSERT_1, - false // don't conversions immediately - ); - - ret = ads1015_write_register(handle, ADS1015_CONFIG, config_reg); - if (ret != ESP_OK) { - return ret; - } + // Write config register + uint16_t config_reg = ads1015_build_config( + ADS1015_MUX_AIN0_AIN1, ADS1015_PGA_4_096V, ADS1015_MODE_SINGLESHOT, + config->adc_data_rate, ADS1015_COMP_TRADITIONAL, ADS1015_COMP_ACTIVE_LOW, + ADS1015_COMP_NON_LATCHING, ADS1015_COMP_ASSERT_1, + false // don't conversions immediately + ); - handle->config_reg = config_reg; - - // Configure alert GPIO and interrupt service - gpio_config_t io_conf = { - .intr_type = GPIO_INTR_NEGEDGE, - .mode = GPIO_MODE_INPUT, - .pin_bit_mask = 1ULL << config->rdy_gpio, - .pull_up_en = GPIO_PULLUP_ENABLE, - .pull_down_en = GPIO_PULLDOWN_DISABLE, - }; - - ret = gpio_config(&io_conf); - if (ret != ESP_OK) { - return ret; - } - ret = gpio_isr_handler_add(config->rdy_gpio, ads1015_isr, NULL); - if (ret != ESP_OK) { - return ret; - } - - // Start waiting task - xTaskCreate(adc_task, "adc_task", 4096, handle, 10, &adc_task_handle); + ret = ads1015_write_register(handle, ADS1015_CONFIG, config_reg); + if (ret != ESP_OK) { + return ret; + } - ESP_LOGI(TAG, "ADS1015 initialized"); + handle->config_reg = config_reg; - return ESP_OK; -} + // Configure alert GPIO and interrupt service + gpio_config_t io_conf = { + .intr_type = GPIO_INTR_NEGEDGE, + .mode = GPIO_MODE_INPUT, + .pin_bit_mask = 1ULL << config->rdy_gpio, + .pull_up_en = GPIO_PULLUP_ENABLE, + .pull_down_en = GPIO_PULLDOWN_DISABLE, + }; -esp_err_t ads1015_check_current(int16_t value, bool mux_state){ - if (value >= CONFIG_ADS1015_HIGH_THRESH || value <= CONFIG_ADS1015_LOW_THRESH){ - ESP_LOGW(TAG, "Threshhold exceeded: %i (mux: %s)", value, (mux_state ? ("A2-A3") - : ("A0-A1"))); - // Trigger E-stop - } - return ESP_OK; + ret = gpio_config(&io_conf); + if (ret != ESP_OK) { + return ret; + } + ret = gpio_isr_handler_add(config->rdy_gpio, ads1015_isr, NULL); + if (ret != ESP_OK) { + return ret; + } + + // Start waiting task + xTaskCreate(adc_task, "adc_task", 4096, handle, 10, &adc_task_handle); + + ESP_LOGI(TAG, "ADS1015 initialized"); + + return ESP_OK; } -esp_err_t ads1015_read_register(ads1015_handle_t *handle, ads1015_register_t reg, uint16_t *data) { +esp_err_t ads1015_read_register(ads1015_handle_t* handle, + ads1015_register_t reg, uint16_t* data) { if (handle == NULL || data == NULL) { return ESP_ERR_INVALID_ARG; } uint8_t rx_buf[2]; - esp_err_t ret = i2c_master_transmit_receive( - handle->dev_handle, - (uint8_t *)®, - 1, - rx_buf, - 2, - ADS1015_I2C_MASTER_TIMEOUT_MS - ); - - if (ret == ESP_OK) { - *data = (rx_buf[0] << 8) | rx_buf[1]; - } + esp_err_t ret = + i2c_master_transmit_receive(handle->dev_handle, (uint8_t*)®, 1, rx_buf, + 2, ADS1015_I2C_MASTER_TIMEOUT_MS); - return ret; + if (ret == ESP_OK) { + *data = (rx_buf[0] << 8) | rx_buf[1]; + } + + return ret; } -esp_err_t ads1015_write_register(ads1015_handle_t *handle, ads1015_register_t reg, uint16_t data) { +esp_err_t ads1015_write_register(ads1015_handle_t* handle, + ads1015_register_t reg, uint16_t data) { if (handle == NULL) { return ESP_ERR_INVALID_ARG; } @@ -167,9 +155,6 @@ esp_err_t ads1015_write_register(ads1015_handle_t *handle, ads1015_register_t re write_buf[0] = reg; write_buf[1] = (data >> 8) & 0xFF; // MSB first write_buf[2] = data & 0xFF; // LSB - return i2c_master_transmit( - handle->dev_handle, - write_buf, - sizeof(write_buf), - ADS1015_I2C_MASTER_TIMEOUT_MS); + return i2c_master_transmit(handle->dev_handle, write_buf, sizeof(write_buf), + ADS1015_I2C_MASTER_TIMEOUT_MS); } diff --git a/components/ads1015/include/ads1015.h b/components/ads1015/include/ads1015.h index 0f9d696..6ef8a97 100644 --- a/components/ads1015/include/ads1015.h +++ b/components/ads1015/include/ads1015.h @@ -1,147 +1,138 @@ #ifndef _ADS1015_H_ #define _ADS1015_H_ -#include "driver/i2c_types.h" #include "driver/gpio.h" +#include "driver/i2c_types.h" #include "esp_err.h" #include "freertos/FreeRTOS.h" #include "freertos/semphr.h" #define ADS1015_I2C_MASTER_TIMEOUT_MS 1000 -#define ADS1015_MUX_MASK (0x7 << 12) +#define ADS1015_MUX_MASK (0x7 << 12) // Config Register Bit Positions -#define ADS1015_OS_BIT 15 -#define ADS1015_MUX_SHIFT 12 -#define ADS1015_PGA_SHIFT 9 -#define ADS1015_MODE_BIT 8 -#define ADS1015_DR_SHIFT 5 -#define ADS1015_COMP_MODE_BIT 4 -#define ADS1015_COMP_POL_BIT 3 -#define ADS1015_COMP_LAT_BIT 2 -#define ADS1015_COMP_QUE_SHIFT 0 +#define ADS1015_OS_BIT 15 +#define ADS1015_MUX_SHIFT 12 +#define ADS1015_PGA_SHIFT 9 +#define ADS1015_MODE_BIT 8 +#define ADS1015_DR_SHIFT 5 +#define ADS1015_COMP_MODE_BIT 4 +#define ADS1015_COMP_POL_BIT 3 +#define ADS1015_COMP_LAT_BIT 2 +#define ADS1015_COMP_QUE_SHIFT 0 /** * @brief ADS1015 register addresses */ typedef enum { - ADS1015_CONVERSION = 0x00, - ADS1015_CONFIG = 0x01, - ADS1015_LOW_THRESH_REG = 0x02, - ADS1015_HIGH_THRESH_REG = 0x03, + ADS1015_CONVERSION = 0x00, + ADS1015_CONFIG = 0x01, + ADS1015_LOW_THRESH_REG = 0x02, + ADS1015_HIGH_THRESH_REG = 0x03, } ads1015_register_t; /** * @brief ADS1015 MUX register bit definitions */ typedef enum { - ADS1015_MUX_AIN0_AIN1 = 0b000, - ADS1015_MUX_AIN0_AIN3 = 0b001, - ADS1015_MUX_AIN1_AIN3 = 0b010, - ADS1015_MUX_AIN2_AIN3 = 0b011, - ADS1015_MUX_AIN0_GND = 0b100, - ADS1015_MUX_AIN1_GND = 0b101, - ADS1015_MUX_AIN2_GND = 0b110, - ADS1015_MUX_AIN3_GND = 0b111 + ADS1015_MUX_AIN0_AIN1 = 0b000, + ADS1015_MUX_AIN0_AIN3 = 0b001, + ADS1015_MUX_AIN1_AIN3 = 0b010, + ADS1015_MUX_AIN2_AIN3 = 0b011, + ADS1015_MUX_AIN0_GND = 0b100, + ADS1015_MUX_AIN1_GND = 0b101, + ADS1015_MUX_AIN2_GND = 0b110, + ADS1015_MUX_AIN3_GND = 0b111 } ads1015_mux_t; /** * @brief ADS1015 PGA register bit definitions */ typedef enum { - ADS1015_PGA_6_144V = 0b000, - ADS1015_PGA_4_096V = 0b001, - ADS1015_PGA_2_048V = 0b010, - ADS1015_PGA_1_024V = 0b011, - ADS1015_PGA_0_512V = 0b100, - ADS1015_PGA_0_256V = 0b101 + ADS1015_PGA_6_144V = 0b000, + ADS1015_PGA_4_096V = 0b001, + ADS1015_PGA_2_048V = 0b010, + ADS1015_PGA_1_024V = 0b011, + ADS1015_PGA_0_512V = 0b100, + ADS1015_PGA_0_256V = 0b101 } ads1015_pga_t; /** * @brief ADS1015 mode register bit definitions */ typedef enum { - ADS1015_MODE_CONTINUOUS = 0, - ADS1015_MODE_SINGLESHOT = 1 + ADS1015_MODE_CONTINUOUS = 0, + ADS1015_MODE_SINGLESHOT = 1 } ads1015_mode_t; /** * @brief ADS1015 data rate register bit definitions */ typedef enum { - ADS1015_DR_128SPS = 0b000, - ADS1015_DR_250SPS = 0b001, - ADS1015_DR_490SPS = 0b010, - ADS1015_DR_920SPS = 0b011, - ADS1015_DR_1600SPS = 0b100, - ADS1015_DR_2400SPS = 0b101, - ADS1015_DR_3300SPS = 0b110 + ADS1015_DR_128SPS = 0b000, + ADS1015_DR_250SPS = 0b001, + ADS1015_DR_490SPS = 0b010, + ADS1015_DR_920SPS = 0b011, + ADS1015_DR_1600SPS = 0b100, + ADS1015_DR_2400SPS = 0b101, + ADS1015_DR_3300SPS = 0b110 } ads1015_dr_t; /** * @brief ADS1015 comparator settings register bit definitions */ typedef enum { - ADS1015_COMP_TRADITIONAL = 0, - ADS1015_COMP_WINDOW = 1 + ADS1015_COMP_TRADITIONAL = 0, + ADS1015_COMP_WINDOW = 1 } ads1015_comp_mode_t; typedef enum { - ADS1015_COMP_ACTIVE_LOW = 0, - ADS1015_COMP_ACTIVE_HIGH = 1 + ADS1015_COMP_ACTIVE_LOW = 0, + ADS1015_COMP_ACTIVE_HIGH = 1 } ads1015_comp_pol_t; typedef enum { - ADS1015_COMP_NON_LATCHING = 0, - ADS1015_COMP_LATCHING = 1 + ADS1015_COMP_NON_LATCHING = 0, + ADS1015_COMP_LATCHING = 1 } ads1015_comp_lat_t; typedef enum { - ADS1015_COMP_ASSERT_1 = 0b00, - ADS1015_COMP_ASSERT_2 = 0b01, - ADS1015_COMP_ASSERT_4 = 0b10, - ADS1015_COMP_DISABLE = 0b11 + ADS1015_COMP_ASSERT_1 = 0b00, + ADS1015_COMP_ASSERT_2 = 0b01, + ADS1015_COMP_ASSERT_4 = 0b10, + ADS1015_COMP_DISABLE = 0b11 } ads1015_comp_que_t; static inline uint16_t ads1015_build_config( - ads1015_mux_t mux, - ads1015_pga_t pga, - ads1015_mode_t mode, - ads1015_dr_t dr, - ads1015_comp_mode_t comp_mode, - ads1015_comp_pol_t comp_pol, - ads1015_comp_lat_t comp_lat, - ads1015_comp_que_t comp_que, - bool start_conversion -) { - return - ((start_conversion ? 1 : 0) << ADS1015_OS_BIT) | - (mux << ADS1015_MUX_SHIFT) | - (pga << ADS1015_PGA_SHIFT) | - (mode << ADS1015_MODE_BIT) | - (dr << ADS1015_DR_SHIFT) | - (comp_mode << ADS1015_COMP_MODE_BIT) | - (comp_pol << ADS1015_COMP_POL_BIT) | - (comp_lat << ADS1015_COMP_LAT_BIT) | - (comp_que << ADS1015_COMP_QUE_SHIFT); + ads1015_mux_t mux, ads1015_pga_t pga, ads1015_mode_t mode, ads1015_dr_t dr, + ads1015_comp_mode_t comp_mode, ads1015_comp_pol_t comp_pol, + ads1015_comp_lat_t comp_lat, ads1015_comp_que_t comp_que, + bool start_conversion) { + return ((start_conversion ? 1 : 0) << ADS1015_OS_BIT) | + (mux << ADS1015_MUX_SHIFT) | (pga << ADS1015_PGA_SHIFT) | + (mode << ADS1015_MODE_BIT) | (dr << ADS1015_DR_SHIFT) | + (comp_mode << ADS1015_COMP_MODE_BIT) | + (comp_pol << ADS1015_COMP_POL_BIT) | + (comp_lat << ADS1015_COMP_LAT_BIT) | + (comp_que << ADS1015_COMP_QUE_SHIFT); } /** * @brief ADS1015 configuration structure */ typedef struct { - uint8_t i2c_addr; /**< I2C device address */ - uint32_t i2c_speed_hz; /**< I2C bus speed in Hz */ - i2c_master_bus_handle_t bus_handle; /**< I2C master bus handle */ - gpio_num_t rdy_gpio; /**< GPIO number for ALERT pin */ - uint8_t adc_data_rate; /**< Data rate setting for ADC conversions */ + uint8_t i2c_addr; /**< I2C device address */ + uint32_t i2c_speed_hz; /**< I2C bus speed in Hz */ + i2c_master_bus_handle_t bus_handle; /**< I2C master bus handle */ + gpio_num_t rdy_gpio; /**< GPIO number for ALERT pin */ + uint8_t adc_data_rate; /**< Data rate setting for ADC conversions */ } ads1015_config_t; /** * @brief ADS1015 device handle */ typedef struct { - i2c_master_dev_handle_t dev_handle; /**< I2C device handle */ - uint16_t config_reg; /**< ADS1015 config register values */ + i2c_master_dev_handle_t dev_handle; /**< I2C device handle */ + uint16_t config_reg; /**< ADS1015 config register values */ } ads1015_handle_t; /** @@ -159,17 +150,7 @@ typedef struct { * frequency) * - ESP_ERR_*: Other ESP-IDF error codes from I2C operations */ -esp_err_t ads1015_init(ads1015_handle_t *handle, const ads1015_config_t *config); - -/** - * @brief Check the ADC value against the configured threshholds and trigger E-stop if it exceeds them - * - * @param[in] value ADC value to check - * - * @return - * - ESP_OK: Success - */ -esp_err_t ads1015_check_current(int16_t value, bool mux_state); +esp_err_t ads1015_init(ads1015_handle_t* handle, const ads1015_config_t* config); /** * @brief Read one or more registers from the ADS1015 @@ -183,7 +164,8 @@ esp_err_t ads1015_check_current(int16_t value, bool mux_state); * - ESP_ERR_INVALID_ARG: Invalid argument (NULL pointer) * - ESP_ERR_*: Other ESP-IDF error codes from I2C operations */ -esp_err_t ads1015_read_register(ads1015_handle_t *handle, ads1015_register_t reg, uint16_t *data); +esp_err_t ads1015_read_register(ads1015_handle_t* handle, + ads1015_register_t reg, uint16_t* data); /** * @brief Write a single byte to a ADS1015 register @@ -197,6 +179,7 @@ esp_err_t ads1015_read_register(ads1015_handle_t *handle, ads1015_register_t reg * - ESP_ERR_INVALID_ARG: Invalid argument (NULL handle) * - ESP_ERR_*: Other ESP-IDF error codes from I2C operations */ -esp_err_t ads1015_write_register(ads1015_handle_t *handle, ads1015_register_t reg, uint16_t data); +esp_err_t ads1015_write_register(ads1015_handle_t* handle, + ads1015_register_t reg, uint16_t data); -#endif // _ADS1015_H_ +#endif // _ADS1015_H_ diff --git a/components/ads1015/test/test_ads1015.c b/components/ads1015/test/test_ads1015.c index 9dd1323..8517eb2 100644 --- a/components/ads1015/test/test_ads1015.c +++ b/components/ads1015/test/test_ads1015.c @@ -1,6 +1,7 @@ +#include "ads1015.h" #include "driver/i2c_master.h" +#include "freertos/idf_additions.h" #include "i2c_bus.h" -#include "ads1015.h" #include "unity_fixture.h" static i2c_bus_t bus_handle; @@ -39,9 +40,8 @@ TEST(ADS1015, ADS1015_Initialization) { } TEST(ADS1015, ADS1015_Wrong_Address) { - ads1015_config_t config = { - .i2c_addr = 0x00, // Invalid address + .i2c_addr = 0x00, // Invalid address .i2c_speed_hz = 400000, .bus_handle = bus_handle.handle, }; diff --git a/components/driver_socket/driver_socket.c b/components/driver_socket/driver_socket.c index 616738b..492e64f 100644 --- a/components/driver_socket/driver_socket.c +++ b/components/driver_socket/driver_socket.c @@ -1,10 +1,11 @@ #include "driver_socket.h" + +#include "driver_socket_api.h" +#include "endian.h" #include "esp_err.h" #include "esp_log.h" #include "netdb.h" #include "sys/socket.h" -#include "driver_socket_api.h" -#include "endian.h" #define TAG "driver_socket" #define INVALID_SOCK -1 @@ -20,12 +21,12 @@ */ #define YIELD_TO_ALL_MS 50 -void driver_socket_task(void *arg) { +void driver_socket_task(void* arg) { static uint8_t rx_buffer[128]; - task_args_t *task_args = (task_args_t *)arg; - SemaphoreHandle_t *server_ready = &task_args->server_ready; + task_args_t* task_args = (task_args_t*)arg; + SemaphoreHandle_t* server_ready = &task_args->server_ready; struct addrinfo hints = {.ai_socktype = SOCK_STREAM}; - struct addrinfo *address_info; + struct addrinfo* address_info; int listen_sock = INVALID_SOCK; const size_t max_socks = CONFIG_LWIP_MAX_SOCKETS - 1; static int sock[CONFIG_LWIP_MAX_SOCKETS - 1]; @@ -87,7 +88,7 @@ void driver_socket_task(void *arg) { // Main loop for accepting new connections and serving all connected clients while (1) { - struct sockaddr_storage source_addr; // Large enough for both IPv4 or IPv6 + struct sockaddr_storage source_addr; // Large enough for both IPv4 or IPv6 socklen_t addr_len = sizeof(source_addr); // Find a free socket @@ -102,13 +103,13 @@ void driver_socket_task(void *arg) { if (new_sock_index < max_socks) { // Try to accept a new connections sock[new_sock_index] = - accept(listen_sock, (struct sockaddr *)&source_addr, &addr_len); + accept(listen_sock, (struct sockaddr*)&source_addr, &addr_len); if (sock[new_sock_index] < 0) { if (errno == - EWOULDBLOCK) { // The listener socket did not accepts any connection - // continue to serve open connections and try to - // accept again upon the next iteration + EWOULDBLOCK) { // The listener socket did not accepts any + // connection continue to serve open connections and + // try to accept again upon the next iteration ESP_LOGV(TAG, "No pending connections..."); } else { log_socket_error(TAG, listen_sock, errno, @@ -135,7 +136,6 @@ void driver_socket_task(void *arg) { // We serve all the connected clients in this loop for (int i = 0; i < max_socks; ++i) { if (sock[i] != INVALID_SOCK) { - // This is an open socket -> try to serve it int len = try_receive(TAG, sock[i], rx_buffer, sizeof(rx_buffer)); if (len < 0) { @@ -168,8 +168,8 @@ void driver_socket_task(void *arg) { } } - } // one client's socket - } // for all sockets + } // one client's socket + } // for all sockets // Yield to other tasks vTaskDelay(pdMS_TO_TICKS(YIELD_TO_ALL_MS)); @@ -190,10 +190,19 @@ void driver_socket_task(void *arg) { vTaskDelete(NULL); } -esp_err_t driver_socket_init(driver_socket_handle_t *socket_handle, - const driver_socket_config_t *config) { +esp_err_t driver_socket_init( + driver_socket_handle_t* socket_handle, const driver_socket_config_t* config, + const driver_socket_api_motor_interface_t* motor_interface) { + if (socket_handle == NULL || config == NULL || motor_interface == NULL) { + return ESP_ERR_INVALID_ARG; + } + + esp_err_t ret = driver_socket_api_init(motor_interface); + if (ret != ESP_OK) { + return ret; + } - task_args_t *task_args = malloc(sizeof(task_args_t)); + task_args_t* task_args = malloc(sizeof(task_args_t)); if (task_args == NULL) { ESP_LOGE(TAG, "Unable to allocate memory for task arguments"); return ESP_ERR_NO_MEM; @@ -219,44 +228,44 @@ esp_err_t driver_socket_init(driver_socket_handle_t *socket_handle, return ESP_OK; } -void log_socket_error(const char *tag, const int sock, const int err, - const char *message) { +void log_socket_error(const char* tag, const int sock, const int err, + const char* message) { ESP_LOGE(tag, "[sock=%d]: %s\n" "error=%d: %s", sock, message, err, strerror(err)); } -char *get_clients_address(struct sockaddr_storage *source_addr) { +char* get_clients_address(struct sockaddr_storage* source_addr) { static char address_str[128]; - char *res = NULL; + char* res = NULL; // Convert ip address to string if (source_addr->ss_family == PF_INET) { - res = inet_ntoa_r(((struct sockaddr_in *)source_addr)->sin_addr, - address_str, sizeof(address_str) - 1); + res = inet_ntoa_r(((struct sockaddr_in*)source_addr)->sin_addr, address_str, + sizeof(address_str) - 1); } #ifdef CONFIG_LWIP_IPV6 else if (source_addr->ss_family == PF_INET6) { - res = inet6_ntoa_r(((struct sockaddr_in6 *)source_addr)->sin6_addr, + res = inet6_ntoa_r(((struct sockaddr_in6*)source_addr)->sin6_addr, address_str, sizeof(address_str) - 1); } #endif if (!res) { - address_str[0] = '\0'; // Returns empty string if conversion didn't succeed + address_str[0] = '\0'; // Returns empty string if conversion didn't succeed } return address_str; } -int try_receive(const char *tag, const int sock, uint8_t *data, - size_t max_len) { +int try_receive(const char* tag, const int sock, uint8_t* data, + size_t max_len) { int len = recv(sock, data, max_len, 0); if (len < 0) { if (errno == EINPROGRESS || errno == EAGAIN || errno == EWOULDBLOCK) { - return 0; // Not an error + return 0; // Not an error } if (errno == ENOTCONN) { ESP_LOGW(tag, "[sock=%d]: Connection closed", sock); - return -2; // Socket has been disconnected + return -2; // Socket has been disconnected } log_socket_error(tag, sock, errno, "Error occurred during receiving"); return -1; @@ -265,16 +274,17 @@ int try_receive(const char *tag, const int sock, uint8_t *data, return len; } -int socket_send(const char *tag, const int sock, const uint8_t * data, const size_t len) -{ - int to_write = len; - while (to_write > 0) { - int written = send(sock, data + (len - to_write), to_write, 0); - if (written < 0 && errno != EINPROGRESS && errno != EAGAIN && errno != EWOULDBLOCK) { - log_socket_error(tag, sock, errno, "Error occurred during sending"); - return -1; - } - to_write -= written; +int socket_send(const char* tag, const int sock, const uint8_t* data, + const size_t len) { + int to_write = len; + while (to_write > 0) { + int written = send(sock, data + (len - to_write), to_write, 0); + if (written < 0 && errno != EINPROGRESS && errno != EAGAIN && + errno != EWOULDBLOCK) { + log_socket_error(tag, sock, errno, "Error occurred during sending"); + return -1; } - return len; + to_write -= written; + } + return len; } diff --git a/components/driver_socket/driver_socket_api.c b/components/driver_socket/driver_socket_api.c index f635687..f2660a9 100644 --- a/components/driver_socket/driver_socket_api.c +++ b/components/driver_socket/driver_socket_api.c @@ -1,29 +1,34 @@ #include "driver_socket_api.h" + #include "endian.h" #include "esp_log.h" #include "freertos//FreeRTOS.h" #define TAG "driver_socket_api" -static void -driver_socket_api_header_swap_endianess(driver_socket_api_header_t *header) { +static driver_socket_api_motor_interface_t s_motor_interface; + +static void driver_socket_api_header_swap_endianess( + driver_socket_api_header_t* header) { header->msg_id = be32toh(header->msg_id); header->cmd_id = be16toh(header->cmd_id); header->len = be16toh(header->len); } -static void driver_socket_api_polar_pan_payload_swap_endianess(driver_socket_api_polar_pan_payload_t *payload) { +static void driver_socket_api_polar_pan_payload_swap_endianess( + driver_socket_api_polar_pan_payload_t* payload) { payload->delta_azimuth = be32toh(payload->delta_azimuth); payload->delta_altitude = be32toh(payload->delta_altitude); payload->delay_ms = be32toh(payload->delay_ms); payload->time_ms = be32toh(payload->time_ms); } -static void driver_socket_api_home_payload_swap_endianess(driver_socket_api_home_payload_t *payload) { +static void driver_socket_api_home_payload_swap_endianess( + driver_socket_api_home_payload_t* payload) { payload->delay_ms = be32toh(payload->delay_ms); } -static uint8_t xor_checksum(uint8_t *data, size_t len) { +static uint8_t xor_checksum(uint8_t* data, size_t len) { uint8_t result = 0; for (size_t i = 0; i < len; i++) { result ^= data[i]; @@ -31,22 +36,33 @@ static uint8_t xor_checksum(uint8_t *data, size_t len) { return result; } -static esp_err_t validate_checksum(uint8_t *data, size_t len, uint8_t checksum) { - uint8_t calculated_checksum = xor_checksum((uint8_t *)data, len); +static esp_err_t validate_checksum(uint8_t* data, size_t len, + uint8_t checksum) { + uint8_t calculated_checksum = xor_checksum((uint8_t*)data, len); return (calculated_checksum == checksum) ? ESP_OK : ESP_ERR_INVALID_CRC; } -esp_err_t driver_socket_api_process(uint8_t *buffer, size_t buffer_size) { +esp_err_t driver_socket_api_init( + const driver_socket_api_motor_interface_t* motor_interface) { + if (motor_interface == NULL) { + return ESP_ERR_INVALID_ARG; + } + s_motor_interface = *motor_interface; + return ESP_OK; +} + +esp_err_t driver_socket_api_process(uint8_t* buffer, size_t buffer_size) { ESP_LOGI(TAG, "Received data size: %d", buffer_size); ESP_LOG_BUFFER_HEX_LEVEL(TAG, buffer, buffer_size, ESP_LOG_INFO); - esp_err_t err = validate_checksum(buffer, buffer_size - 1, buffer[buffer_size - 1]); + esp_err_t err = + validate_checksum(buffer, buffer_size - 1, buffer[buffer_size - 1]); if (err != ESP_OK) { ESP_LOGE(TAG, "Checksum validation failed"); return err; } - driver_socket_api_wrapper_t *wrapper = (driver_socket_api_wrapper_t *)buffer; + driver_socket_api_wrapper_t* wrapper = (driver_socket_api_wrapper_t*)buffer; driver_socket_api_header_swap_endianess(&wrapper->header); @@ -58,24 +74,29 @@ esp_err_t driver_socket_api_process(uint8_t *buffer, size_t buffer_size) { ESP_LOGI(TAG, "Processing HANDSHAKE command"); break; case DRIVER_SOCKET_API_CMD_ID_POLAR_PAN: - driver_socket_api_polar_pan_payload_t *polar_pan_payload = (driver_socket_api_polar_pan_payload_t *)&wrapper->payload_head; + driver_socket_api_polar_pan_payload_t* polar_pan_payload = + (driver_socket_api_polar_pan_payload_t*)&wrapper->payload_head; driver_socket_api_polar_pan_payload_swap_endianess(polar_pan_payload); - ESP_LOGI(TAG, "Processing POLAR_PAN command: delta_azimuth=%d, delta_altitude=%d, delay_ms=%d, time_ms=%d", - polar_pan_payload->delta_azimuth, polar_pan_payload->delta_altitude, polar_pan_payload->delay_ms, polar_pan_payload->time_ms); + s_motor_interface.polar_pan( + polar_pan_payload->delta_azimuth, polar_pan_payload->delta_altitude, + polar_pan_payload->delay_ms, polar_pan_payload->time_ms); break; case DRIVER_SOCKET_API_CMD_ID_HOME: - driver_socket_api_home_payload_t *home_payload = (driver_socket_api_home_payload_t *)&wrapper->payload_head; + driver_socket_api_home_payload_t* home_payload = + (driver_socket_api_home_payload_t*)&wrapper->payload_head; driver_socket_api_home_payload_swap_endianess(home_payload); - ESP_LOGI(TAG, "Processing HOME command: delay_ms=%d", home_payload->delay_ms); + s_motor_interface.home(home_payload->delay_ms); break; case DRIVER_SOCKET_API_CMD_ID_POLAR_PAN_START: - driver_socket_api_polar_pan_start_payload_t *polar_pan_start_payload = (driver_socket_api_polar_pan_start_payload_t *)&wrapper->payload_head; + driver_socket_api_polar_pan_start_payload_t* polar_pan_start_payload = + (driver_socket_api_polar_pan_start_payload_t*)&wrapper->payload_head; // No endianess swap needed for int8_t fields - ESP_LOGI(TAG, "Processing POLAR_PAN_START command: delta_azimuth=%d, delta_altitude=%d", - polar_pan_start_payload->delta_azimuth, polar_pan_start_payload->delta_altitude); + s_motor_interface.polar_pan_start( + polar_pan_start_payload->delta_azimuth, + polar_pan_start_payload->delta_altitude); break; case DRIVER_SOCKET_API_CMD_ID_POLAR_PAN_STOP: - ESP_LOGI(TAG, "Processing POLAR_PAN_STOP command"); + s_motor_interface.polar_pan_stop(); break; default: ESP_LOGW(TAG, "Unknown command ID: %d", wrapper->header.cmd_id); diff --git a/components/driver_socket/include/driver_socket.h b/components/driver_socket/include/driver_socket.h index d047ac9..0a09b86 100644 --- a/components/driver_socket/include/driver_socket.h +++ b/components/driver_socket/include/driver_socket.h @@ -1,10 +1,10 @@ #ifndef _DRIVER_SOCKET_H_ #define _DRIVER_SOCKET_H_ +#include "driver_socket_api.h" #include "freertos/FreeRTOS.h" #include "sys/socket.h" - /** * @brief Socket driver handle and configuration structures */ @@ -13,11 +13,10 @@ typedef struct { } driver_socket_handle_t; typedef struct { - const char *ip; - const char *port; + const char* ip; + const char* port; } driver_socket_config_t; - /** * @brief Task arguments structure for the socket task */ @@ -33,13 +32,17 @@ typedef struct { * * @param[out] socket_handle Socket handle to be initialized by this function * @param[in] config Socket configuration + * @param[in] motor_interface Motor control interface to be used by + * driver_socket_api * @return * ESP_OK : Socket driver initialized successfully + * ESP_ERR_INVALID_ARG : Invalid argument * ESP_ERR_NO_MEM : Unable to allocate memory for task arguments or * server_ready semaphore */ -esp_err_t driver_socket_init(driver_socket_handle_t *socket_handle, - const driver_socket_config_t *config); +esp_err_t driver_socket_init( + driver_socket_handle_t* socket_handle, const driver_socket_config_t* config, + const driver_socket_api_motor_interface_t* motor_interface); /** * @brief Utility to log socket errors @@ -49,14 +52,14 @@ esp_err_t driver_socket_init(driver_socket_handle_t *socket_handle, * @param[in] err Socket errno * @param[in] message Message to print */ -void log_socket_error(const char *tag, const int sock, const int err, - const char *message); +void log_socket_error(const char* tag, const int sock, const int err, + const char* message); /** * @brief Returns the string representation of client's address (accepted on * this server) */ -char *get_clients_address(struct sockaddr_storage *source_addr); +char* get_clients_address(struct sockaddr_storage* source_addr); /** * @brief Tries to receive data from specified sockets in a non-blocking way, @@ -73,8 +76,7 @@ char *get_clients_address(struct sockaddr_storage *source_addr); * -2 : Socket is not connected, to distinguish between an actual * socket error and active disconnection */ -int try_receive(const char *tag, const int sock, uint8_t *data, - size_t max_len); +int try_receive(const char* tag, const int sock, uint8_t* data, size_t max_len); /** * @brief Sends the specified data to the socket. This function blocks until all @@ -88,7 +90,7 @@ int try_receive(const char *tag, const int sock, uint8_t *data, * >0 : Size the written data * -1 : Error occurred during socket write operation */ -int socket_send(const char *tag, const int sock, const uint8_t *data, - const size_t len); +int socket_send(const char* tag, const int sock, const uint8_t* data, + const size_t len); -#endif // _DRIVER_SOCKET_H_ +#endif // _DRIVER_SOCKET_H_ diff --git a/components/driver_socket/include/driver_socket_api.h b/components/driver_socket/include/driver_socket_api.h index 73ad680..a8b8bc2 100644 --- a/components/driver_socket/include/driver_socket_api.h +++ b/components/driver_socket/include/driver_socket_api.h @@ -1,9 +1,10 @@ #ifndef _DRIVER_SOCKET_API_H_ #define _DRIVER_SOCKET_API_H_ -#include "freertos/FreeRTOS.h" #include +#include "freertos/FreeRTOS.h" + /** Command IDs for the socket API */ typedef enum { DRIVER_SOCKET_API_CMD_ID_HANDSHAKE = 0x0000, @@ -21,9 +22,10 @@ typedef struct { uint16_t len; } __attribute__((packed)) driver_socket_api_header_t; -/** Wrapper struct for the entire message, including header and payload. The payload - * is variable length, so we can't define it as a fixed array. Instead, we can - * use a flexible array member or just treat it as a pointer when processing. +/** Wrapper struct for the entire message, including header and payload. The + * payload is variable length, so we can't define it as a fixed array. Instead, + * we can use a flexible array member or just treat it as a pointer when + * processing. */ typedef struct { driver_socket_api_header_t header; @@ -34,7 +36,6 @@ typedef struct { // uint_t checksum; } __attribute__((packed)) driver_socket_api_wrapper_t; - /** Payload structures for different command types */ typedef struct { int32_t delta_azimuth; /** Requested change in azimuth */ @@ -52,20 +53,47 @@ typedef struct { int8_t delta_altitude; /** Requested change in altitude */ } __attribute__((packed)) driver_socket_api_polar_pan_start_payload_t; +/** Callback struct that allows the driver socket api to access motorhat motion commands */ +typedef struct { + esp_err_t (*polar_pan)(int16_t delta_azimuth, int16_t delta_altitude, + uint16_t delay_ms, uint16_t time_ms); + esp_err_t (*polar_pan_start)(int8_t delta_azimuth, int8_t delta_altitude); + esp_err_t (*polar_pan_stop)(void); + esp_err_t (*home)(uint16_t delay_ms); +} driver_socket_api_motor_interface_t; /** -* @brief Process incoming socket API messages. This function should be called -* from the socket task when a complete message has been received. It will parse -* the message, validate it, and execute the corresponding command. -* @param[in] buffer Pointer to the received message buffer -* @param[in] buffer_size Size of the received message buffer -* @return -* ESP_OK : Message processed successfully -* ESP_ERR_INVALID_ARG : Invalid message format or command ID -* ESP_ERR_INVALID_SIZE : Message size does not match expected size for the command -* ESP_ERR_INVALID_CRC : Checksum validation failed (if implemented) -* ESP_ERR_FAIL : Command execution failed (e.g., invalid parameters) -*/ -esp_err_t driver_socket_api_process(uint8_t *buffer, size_t buffer_size); + * @brief Initializes the socket API module with the provided motor control + * interface. + * + * This function must be called before processing any incoming messages with + * driver_socket_api_process(). It sets up the internal function pointers for + * motor control commands that will be invoked when corresponding messages are + * received. + * + * @param[in] motor_interface Pointer to a structure containing function + * pointers for motor control operations. + * + * @return + * ESP_OK : Initialization successful + * ESP_ERR_INVALID_ARG : Invalid argument (e.g., NULL pointer) + */ +esp_err_t driver_socket_api_init( + const driver_socket_api_motor_interface_t* motor_interface); + +/** + * @brief Process incoming socket API messages. This function should be called + * from the socket task when a complete message has been received. It will parse + * the message, validate it, and execute the corresponding command. + * @param[in] buffer Pointer to the received message buffer + * @param[in] buffer_size Size of the received message buffer + * @return + * ESP_OK : Message processed successfully + * ESP_ERR_INVALID_ARG : Invalid message format or command ID + * ESP_ERR_INVALID_SIZE : Message size does not match expected size for + * the command ESP_ERR_INVALID_CRC : Checksum validation failed (if implemented) + * ESP_ERR_FAIL : Command execution failed (e.g., invalid parameters) + */ +esp_err_t driver_socket_api_process(uint8_t* buffer, size_t buffer_size); -#endif // _DRIVER_SOCKET_API_H_ +#endif // _DRIVER_SOCKET_API_H_ diff --git a/components/driver_wifi/driver_wifi.c b/components/driver_wifi/driver_wifi.c index 4311fbf..4dbaa10 100644 --- a/components/driver_wifi/driver_wifi.c +++ b/components/driver_wifi/driver_wifi.c @@ -1,4 +1,5 @@ #include "driver_wifi.h" + #include "esp_err.h" #include "esp_log.h" #include "esp_wifi.h" @@ -18,13 +19,13 @@ #elif CONFIG_DRIVER_WIFI_SAE_PWE_H2E #define DRIVER_WIFI_SAE_MODE WPA3_SAE_PWE_HASH_TO_ELEMENT -#define DRIVER_WIFI_SAE_PASSWORD_IDENTIFIER \ +#define DRIVER_WIFI_SAE_PASSWORD_IDENTIFIER \ CONFIG_DRIVER_WIFI_SAE_PASSWORD_IDENTIFIER #elif CONFIG_DRIVER_WIFI_SAE_PWE_BOTH #define DRIVER_WIFI_SAE_MODE WPA3_SAE_PWE_BOTH -#define DRIVER_WIFI_SAE_PASSWORD_IDENTIFIER \ +#define DRIVER_WIFI_SAE_PASSWORD_IDENTIFIER \ CONFIG_DRIVER_WIFI_SAE_PASSWORD_IDENTIFIER #endif @@ -59,7 +60,7 @@ #endif -static const char *TAG = "DRIVER_WIFI"; +static const char* TAG = "DRIVER_WIFI"; /* FreeRTOS event group to signal when we are connected*/ static EventGroupHandle_t s_wifi_event_group; @@ -73,8 +74,8 @@ static EventGroupHandle_t s_wifi_event_group; static int s_retry_num = 0; -static void event_handler(void *arg, esp_event_base_t event_base, - int32_t event_id, void *event_data) { +static void event_handler(void* arg, esp_event_base_t event_base, + int32_t event_id, void* event_data) { if (event_base == WIFI_EVENT && event_id == WIFI_EVENT_STA_START) { esp_wifi_connect(); } else if (event_base == WIFI_EVENT && @@ -88,15 +89,14 @@ static void event_handler(void *arg, esp_event_base_t event_base, } ESP_LOGI(TAG, "connect to the AP fail"); } else if (event_base == IP_EVENT && event_id == IP_EVENT_STA_GOT_IP) { - ip_event_got_ip_t *event = (ip_event_got_ip_t *)event_data; + ip_event_got_ip_t* event = (ip_event_got_ip_t*)event_data; ESP_LOGI(TAG, "got ip:" IPSTR, IP2STR(&event->ip_info.ip)); s_retry_num = 0; xEventGroupSetBits(s_wifi_event_group, WIFI_CONNECTED_BIT); } } -esp_err_t wifi_init(driver_wifi_config_t *config) { - +esp_err_t wifi_init(driver_wifi_config_t* config) { s_wifi_event_group = xEventGroupCreate(); if (s_wifi_event_group == NULL) { ESP_LOGE(TAG, "Unable to create wifi event group"); diff --git a/components/driver_wifi/include/driver_wifi.h b/components/driver_wifi/include/driver_wifi.h index dbb096a..d63d724 100644 --- a/components/driver_wifi/include/driver_wifi.h +++ b/components/driver_wifi/include/driver_wifi.h @@ -10,26 +10,26 @@ typedef struct { } driver_wifi_config_t; /** - * @brief Initializes the WiFi driver and connects to the specified network. This - * does not block until the connection is established, so you should call - * wait_for_wifi_connection() after this to block until the connection is ready. - * @param[in] config WiFi configuration (SSID and password) - * @return - * ESP_OK : WiFi initialization started successfully - * ESP_ERR_INVALID_ARG : Invalid configuration (e.g., empty SSID) - * ESP_ERR_WIFI_NOT_INIT : WiFi driver not initialized - * ESP_ERR_WIFI_CONN : Failed to start WiFi connection process - */ -esp_err_t wifi_init(driver_wifi_config_t *config); + * @brief Initializes the WiFi driver and connects to the specified network. + * This does not block until the connection is established, so you should call + * wait_for_wifi_connection() after this to block until the connection is ready. + * @param[in] config WiFi configuration (SSID and password) + * @return + * ESP_OK : WiFi initialization started successfully + * ESP_ERR_INVALID_ARG : Invalid configuration (e.g., empty SSID) + * ESP_ERR_WIFI_NOT_INIT : WiFi driver not initialized + * ESP_ERR_WIFI_CONN : Failed to start WiFi connection process + */ +esp_err_t wifi_init(driver_wifi_config_t* config); /** - * @brief Blocks until the WiFi connection is established. This should be called - * after wifi_init() to wait for the connection to be ready before proceeding. - * @return - * ESP_OK : WiFi connected successfully - * ESP_ERR_WIFI_NOT_INIT : WiFi driver not initialized - * ESP_ERR_WIFI_CONN : Failed to connect to WiFi (e.g., wrong password) - */ + * @brief Blocks until the WiFi connection is established. This should be called + * after wifi_init() to wait for the connection to be ready before proceeding. + * @return + * ESP_OK : WiFi connected successfully + * ESP_ERR_WIFI_NOT_INIT : WiFi driver not initialized + * ESP_ERR_WIFI_CONN : Failed to connect to WiFi (e.g., wrong password) + */ esp_err_t wait_for_wifi_connection(); -#endif // _WIFI_H +#endif // _WIFI_H diff --git a/components/driver_wifi/test/test_driver_wifi.c b/components/driver_wifi/test/test_driver_wifi.c index f730f5e..819411b 100644 --- a/components/driver_wifi/test/test_driver_wifi.c +++ b/components/driver_wifi/test/test_driver_wifi.c @@ -8,6 +8,4 @@ TEST_SETUP(Driver_WiFi) {} TEST_TEAR_DOWN(Driver_WiFi) {} -TEST(Driver_WiFi, Driver_WiFi_Init) { - TEST_ASSERT_EQUAL(ESP_OK, ESP_FAIL); -} +TEST(Driver_WiFi, Driver_WiFi_Init) { TEST_ASSERT_EQUAL(ESP_OK, ESP_FAIL); } diff --git a/components/encoder/CMakeLists.txt b/components/encoder/CMakeLists.txt index 0b9d715..a7f6abc 100644 --- a/components/encoder/CMakeLists.txt +++ b/components/encoder/CMakeLists.txt @@ -1,4 +1,3 @@ idf_component_register(SRCS "encoder.c" - PRIV_REQUIRES esp_driver_gpio - REQUIRES esp_driver_pcnt + REQUIRES esp_driver_pcnt esp_driver_gpio INCLUDE_DIRS "include") diff --git a/components/encoder/encoder.c b/components/encoder/encoder.c index 464f0e8..cad937d 100644 --- a/components/encoder/encoder.c +++ b/components/encoder/encoder.c @@ -1,158 +1,173 @@ #include "encoder.h" + +#include "driver/pulse_cnt.h" #include "esp_log.h" #include "sdkconfig.h" -#include "driver/pulse_cnt.h" - -static const char *TAG = "encoder"; -esp_err_t encoder_init(encoder_handle_t *handle, encoder_config_t *config) -{ - if (handle == NULL || config == NULL) { - return ESP_ERR_INVALID_ARG; - } - - ESP_LOGI(TAG, "install pcnt unit"); - pcnt_unit_config_t unit_config = { - .high_limit = ENCODER_HIGH_LIMIT, - .low_limit = ENCODER_LOW_LIMIT, - .flags.accum_count = 1 // accumulate the count in the event the count goes past limits (count is reset to 0) - }; - - esp_err_t ret = pcnt_new_unit(&unit_config, &handle->pcnt_unit); +static const char* TAG = "encoder"; + +esp_err_t encoder_init(encoder_handle_t* handle, encoder_config_t* config) { + if (handle == NULL || config == NULL) { + return ESP_ERR_INVALID_ARG; + } + + ESP_LOGI(TAG, "install pcnt unit"); + pcnt_unit_config_t unit_config = { + .high_limit = ENCODER_HIGH_LIMIT, + .low_limit = ENCODER_LOW_LIMIT, + .flags.accum_count = 1 // accumulate the count in the event the count + // goes past limits (count is reset to 0) + }; + + esp_err_t ret = pcnt_new_unit(&unit_config, &handle->pcnt_unit); + if (ret != ESP_OK) { + return ret; + } + + ESP_LOGI(TAG, "set glitch filter"); + pcnt_glitch_filter_config_t filter_config = { + .max_glitch_ns = config->glitch_filter_ns, + }; + ret = pcnt_unit_set_glitch_filter(handle->pcnt_unit, &filter_config); + if (ret != ESP_OK) { + return ret; + } + + ESP_LOGI(TAG, "install pcnt channels"); + pcnt_chan_config_t chan_a_config = { + .edge_gpio_num = config->P0_pin, + .level_gpio_num = config->P1_pin, + }; + pcnt_channel_handle_t pcnt_chan_a = NULL; + ret = pcnt_new_channel(handle->pcnt_unit, &chan_a_config, &pcnt_chan_a); + if (ret != ESP_OK) { + return ret; + } + + pcnt_chan_config_t chan_b_config = { + .edge_gpio_num = config->P1_pin, + .level_gpio_num = config->P0_pin, + }; + pcnt_channel_handle_t pcnt_chan_b = NULL; + ret = pcnt_new_channel(handle->pcnt_unit, &chan_b_config, &pcnt_chan_b); + if (ret != ESP_OK) { + return ret; + } + + ESP_LOGI(TAG, "set edge and level actions for pcnt channels"); + if (config->invert_angle) { + ret = pcnt_channel_set_edge_action(pcnt_chan_a, + PCNT_CHANNEL_EDGE_ACTION_INCREASE, + PCNT_CHANNEL_EDGE_ACTION_DECREASE); if (ret != ESP_OK) { - return ret; + return ret; } - - ESP_LOGI(TAG, "set glitch filter"); - pcnt_glitch_filter_config_t filter_config = { - .max_glitch_ns = config->glitch_filter_ns, - }; - ret = pcnt_unit_set_glitch_filter(handle->pcnt_unit, &filter_config); + ret = pcnt_channel_set_edge_action(pcnt_chan_b, + PCNT_CHANNEL_EDGE_ACTION_DECREASE, + PCNT_CHANNEL_EDGE_ACTION_INCREASE); if (ret != ESP_OK) { - return ret; + return ret; } - - ESP_LOGI(TAG, "install pcnt channels"); - pcnt_chan_config_t chan_a_config = { - .edge_gpio_num = config->P0_pin, - .level_gpio_num = config->P1_pin, - }; - pcnt_channel_handle_t pcnt_chan_a = NULL; - ret = pcnt_new_channel(handle->pcnt_unit, &chan_a_config, &pcnt_chan_a); + } else { + ret = pcnt_channel_set_edge_action(pcnt_chan_a, + PCNT_CHANNEL_EDGE_ACTION_DECREASE, + PCNT_CHANNEL_EDGE_ACTION_INCREASE); if (ret != ESP_OK) { - return ret; + return ret; } - - pcnt_chan_config_t chan_b_config = { - .edge_gpio_num = config->P1_pin, - .level_gpio_num = config->P0_pin, - }; - pcnt_channel_handle_t pcnt_chan_b = NULL; - ret = pcnt_new_channel(handle->pcnt_unit, &chan_b_config, &pcnt_chan_b); + ret = pcnt_channel_set_edge_action(pcnt_chan_b, + PCNT_CHANNEL_EDGE_ACTION_INCREASE, + PCNT_CHANNEL_EDGE_ACTION_DECREASE); if (ret != ESP_OK) { - return ret; - } - - ESP_LOGI(TAG, "set edge and level actions for pcnt channels"); - if (config->invert_angle) { - ret = pcnt_channel_set_edge_action(pcnt_chan_a, PCNT_CHANNEL_EDGE_ACTION_INCREASE, PCNT_CHANNEL_EDGE_ACTION_DECREASE); - if (ret != ESP_OK) { - return ret; - } - ret = pcnt_channel_set_edge_action(pcnt_chan_b, PCNT_CHANNEL_EDGE_ACTION_DECREASE, PCNT_CHANNEL_EDGE_ACTION_INCREASE); - if (ret != ESP_OK) { - return ret; - } - } else { - ret = pcnt_channel_set_edge_action(pcnt_chan_a, PCNT_CHANNEL_EDGE_ACTION_DECREASE, PCNT_CHANNEL_EDGE_ACTION_INCREASE); - if (ret != ESP_OK) { - return ret; - } - ret = pcnt_channel_set_edge_action(pcnt_chan_b, PCNT_CHANNEL_EDGE_ACTION_INCREASE, PCNT_CHANNEL_EDGE_ACTION_DECREASE); - if (ret != ESP_OK) { - return ret; - } + return ret; } - ret = pcnt_channel_set_level_action(pcnt_chan_a, PCNT_CHANNEL_LEVEL_ACTION_KEEP, PCNT_CHANNEL_LEVEL_ACTION_INVERSE); + } + ret = + pcnt_channel_set_level_action(pcnt_chan_a, PCNT_CHANNEL_LEVEL_ACTION_KEEP, + PCNT_CHANNEL_LEVEL_ACTION_INVERSE); + if (ret != ESP_OK) { + return ret; + } + ret = + pcnt_channel_set_level_action(pcnt_chan_b, PCNT_CHANNEL_LEVEL_ACTION_KEEP, + PCNT_CHANNEL_LEVEL_ACTION_INVERSE); + if (ret != ESP_OK) { + return ret; + } + + ESP_LOGI(TAG, "add limit watch points for count accumulation"); + int watch_points[] = {ENCODER_LOW_LIMIT, ENCODER_HIGH_LIMIT}; + for (size_t i = 0; i < sizeof(watch_points) / sizeof(watch_points[0]); i++) { + ret = pcnt_unit_add_watch_point(handle->pcnt_unit, watch_points[i]); if (ret != ESP_OK) { - return ret; - } - ret = pcnt_channel_set_level_action(pcnt_chan_b, PCNT_CHANNEL_LEVEL_ACTION_KEEP, PCNT_CHANNEL_LEVEL_ACTION_INVERSE); - if (ret != ESP_OK) { - return ret; + return ret; } + } - ESP_LOGI(TAG, "add limit watch points for count accumulation"); - int watch_points[] = {ENCODER_LOW_LIMIT, ENCODER_HIGH_LIMIT}; - for (size_t i = 0; i < sizeof(watch_points) / sizeof(watch_points[0]); i++) { - ret = pcnt_unit_add_watch_point(handle->pcnt_unit, watch_points[i]); - if (ret != ESP_OK) {return ret;} - } - - return ESP_OK ; + return ESP_OK; } -esp_err_t encoder_start(encoder_handle_t *handle) { +esp_err_t encoder_start(encoder_handle_t* handle) { + pcnt_unit_handle_t pcnt_unit = handle->pcnt_unit; - pcnt_unit_handle_t pcnt_unit = handle->pcnt_unit; + ESP_LOGI(TAG, "enable pcnt unit"); + esp_err_t ret = pcnt_unit_enable(pcnt_unit); + if (ret != ESP_OK) { + return ret; + } - ESP_LOGI(TAG, "enable pcnt unit"); - esp_err_t ret = pcnt_unit_enable(pcnt_unit); - if (ret != ESP_OK) { - return ret; - } + ESP_LOGI(TAG, "clear pcnt unit"); + ret = pcnt_unit_clear_count(pcnt_unit); + if (ret != ESP_OK) { + return ret; + } - ESP_LOGI(TAG, "clear pcnt unit"); - ret = pcnt_unit_clear_count(pcnt_unit); - if (ret != ESP_OK) { - return ret; - } - - ESP_LOGI(TAG, "start pcnt unit"); - ret = pcnt_unit_start(pcnt_unit); - if (ret != ESP_OK) { - return ret; - } + ESP_LOGI(TAG, "start pcnt unit"); + ret = pcnt_unit_start(pcnt_unit); + if (ret != ESP_OK) { + return ret; + } - return ESP_OK ; + return ESP_OK; } - -esp_err_t encoder_get_raw_count(encoder_handle_t *handle, int *pulse_count) { - return pcnt_unit_get_count(handle->pcnt_unit, pulse_count); +esp_err_t encoder_get_raw_count(encoder_handle_t* handle, int* pulse_count) { + return pcnt_unit_get_count(handle->pcnt_unit, pulse_count); } -esp_err_t encoder_clear_count(encoder_handle_t *handle) { - return pcnt_unit_clear_count(handle->pcnt_unit); +esp_err_t encoder_clear_count(encoder_handle_t* handle) { + return pcnt_unit_clear_count(handle->pcnt_unit); } -esp_err_t encoder_get_wheel_angle(encoder_handle_t *handle, encoder_config_t *config, float *wheel_angle) { - - int pulse_count = 0; - esp_err_t ret = encoder_get_raw_count(handle, &pulse_count); - if (ret != ESP_OK) { - return ret; - } +esp_err_t encoder_get_wheel_angle(encoder_handle_t* handle, + encoder_config_t* config, + float* wheel_angle) { + int pulse_count = 0; + esp_err_t ret = encoder_get_raw_count(handle, &pulse_count); + if (ret != ESP_OK) { + return ret; + } - int resolution = config->resolution * 4; // Due to x4 quadrature encoding. + int resolution = config->resolution * 4; // Due to x4 quadrature encoding. - *wheel_angle = ((float)pulse_count * 360.0f) / resolution ; // Due to 360 degrees in 1 rotation. - - return ESP_OK ; + *wheel_angle = ((float)pulse_count * 360.0f) / + resolution; // Due to 360 degrees in 1 rotation. + return ESP_OK; } -esp_err_t encoder_get_limb_angle(encoder_handle_t *handle, encoder_config_t *config, float *limb_angle) { - float wheel_angle = 0.0f ; - esp_err_t ret = encoder_get_wheel_angle(handle, config, &wheel_angle); - if (ret != ESP_OK) { - return ret; - } +esp_err_t encoder_get_limb_angle(encoder_handle_t* handle, + encoder_config_t* config, float* limb_angle) { + float wheel_angle = 0.0f; + esp_err_t ret = encoder_get_wheel_angle(handle, config, &wheel_angle); + if (ret != ESP_OK) { + return ret; + } - float gear_ratio = config->gear_ratio ; - *limb_angle = config->limb_default ; + float gear_ratio = config->gear_ratio; + *limb_angle = config->limb_default; - *limb_angle += wheel_angle / gear_ratio ; + *limb_angle += wheel_angle / gear_ratio; - return ESP_OK ; + return ESP_OK; } \ No newline at end of file diff --git a/components/encoder/include/encoder.h b/components/encoder/include/encoder.h index 8f97d1a..4a4a761 100644 --- a/components/encoder/include/encoder.h +++ b/components/encoder/include/encoder.h @@ -1,31 +1,37 @@ -#include "esp_err.h" +#ifndef _ENCODER_H_ +#define _ENCODER_H_ -#include "driver/pulse_cnt.h" #include "driver/gpio.h" +#include "driver/pulse_cnt.h" +#include "esp_err.h" -#define ENCODER_LOW_LIMIT INT16_MIN // These are not configurable due to overflow functionality! -#define ENCODER_HIGH_LIMIT INT16_MAX // These are not configurable due to overflow functionality! - +#define ENCODER_LOW_LIMIT \ + INT16_MIN // These are not configurable due to overflow functionality! +#define ENCODER_HIGH_LIMIT \ + INT16_MAX // These are not configurable due to overflow functionality! typedef struct { - int P0_pin ; - int P1_pin ; - int glitch_filter_ns ; /**< Pulses shorter than this will not inc/dec the count */ - int resolution ; /**< Number of slots in the encoder wheel, used to calculate the real world angle */ - float gear_ratio ; /**< For every X number of rotations of the encoder wheel, the limb it controls moves once */ - float limb_default ; /**< Angle at which the limb hits the end stop switch */ - int invert_angle ; /**< Boolean flip sign of angle and count measurements */ -} encoder_config_t ; + int P0_pin; + int P1_pin; + int glitch_filter_ns; /**< Pulses shorter than this will not inc/dec the count + */ + int resolution; /**< Number of slots in the encoder wheel, used to calculate + the real world angle */ + float gear_ratio; /**< For every X number of rotations of the encoder wheel, + the limb it controls moves once */ + float limb_default; /**< Angle at which the limb hits the end stop switch */ + int invert_angle; /**< Boolean flip sign of angle and count measurements */ +} encoder_config_t; typedef struct { - pcnt_unit_handle_t pcnt_unit ; /**< specific internal PCNT hardware unit */ -} encoder_handle_t ; - + pcnt_unit_handle_t pcnt_unit; /**< specific internal PCNT hardware unit */ +} encoder_handle_t; /** * @brief Initialize an encoder * - * This function initializes the Pulse Counter hardware module for a specific encoder. + * This function initializes the Pulse Counter hardware module for a specific + * encoder. * * @param[out] handle Pointer to encoder handle structure * @param[in] config Pointer to configuration structure @@ -35,12 +41,13 @@ typedef struct { * - ESP_ERR_INVALID_ARG: Invalid argument (NULL pointer) * - ESP_ERR_*: Other ESP-IDF error codes */ -esp_err_t encoder_init(encoder_handle_t *handle, encoder_config_t *config) ; +esp_err_t encoder_init(encoder_handle_t* handle, encoder_config_t* config); /** * @brief Start an encoder * - * Starts the Pulse Counter module for the specified encoder. The count is set to 0. + * Starts the Pulse Counter module for the specified encoder. The count is set + * to 0. * * @param[out] handle Pointer to encoder handle structure * @@ -49,13 +56,14 @@ esp_err_t encoder_init(encoder_handle_t *handle, encoder_config_t *config) ; * - ESP_ERR_INVALID_ARG: Invalid argument (NULL pointer) * - ESP_ERR_*: Other ESP-IDF error codes */ -esp_err_t encoder_start(encoder_handle_t *handle) ; +esp_err_t encoder_start(encoder_handle_t* handle); /** * @brief Get pulse count of encoder * - * Retrieves the raw pulse count of the encoder. This count is signed, for every slot in - * the encoder wheel this count will increment or decrement (based on direction) by 4. + * Retrieves the raw pulse count of the encoder. This count is signed, for every + * slot in the encoder wheel this count will increment or decrement (based on + * direction) by 4. * * @param[out] handle Pointer to encoder handle structure * @param[out] pulse_count Pointer to pulse count value desired @@ -65,14 +73,14 @@ esp_err_t encoder_start(encoder_handle_t *handle) ; * - ESP_ERR_INVALID_ARG: Invalid argument (NULL pointer) * - ESP_ERR_*: Other ESP-IDF error codes */ -esp_err_t encoder_get_raw_count(encoder_handle_t *handle, - int *pulse_count) ; +esp_err_t encoder_get_raw_count(encoder_handle_t* handle, int* pulse_count); /** * @brief Clear pulse count of encoder * - * Resets pulse count of the encoder to 0. This function should only be called when - * an end stop has triggered. If called otherwise, all absolute angles will be incorrect. + * Resets pulse count of the encoder to 0. This function should only be called + * when an end stop has triggered. If called otherwise, all absolute angles will + * be incorrect. * * @param[out] handle Pointer to encoder handle structure * @@ -81,14 +89,14 @@ esp_err_t encoder_get_raw_count(encoder_handle_t *handle, * - ESP_ERR_INVALID_ARG: Invalid argument (NULL pointer) * - ESP_ERR_*: Other ESP-IDF error codes */ -esp_err_t encoder_clear_count(encoder_handle_t *handle) ; +esp_err_t encoder_clear_count(encoder_handle_t* handle); /** * @brief Get wheel angle of encoder * - * Retrieves the angle of the encoder wheel. This angle is calculated using the - * current pulse count, default pulse count (0), and wheel resolution. This angle is - * signed, and can go outside 360. + * Retrieves the angle of the encoder wheel. This angle is calculated using the + * current pulse count, default pulse count (0), and wheel resolution. This + * angle is signed, and can go outside 360. * * @param[out] handle Pointer to encoder handle structure * @param[in] config Pointer to configuration structure @@ -99,16 +107,16 @@ esp_err_t encoder_clear_count(encoder_handle_t *handle) ; * - ESP_ERR_INVALID_ARG: Invalid argument (NULL pointer) * - ESP_ERR_*: Other ESP-IDF error codes */ -esp_err_t encoder_get_wheel_angle(encoder_handle_t *handle, - encoder_config_t *config, float *wheel_angle) ; +esp_err_t encoder_get_wheel_angle(encoder_handle_t* handle, + encoder_config_t* config, float* wheel_angle); /** * @brief Get limb angle of encoder * - * Retrieves the angle of the encoder wheel. This angle is calculated using the - * current pulse count, default pulse count (0), wheel resolution, limb gear ratio, and - * default limb angle. This angle is signed, and can go outside 360(though it may - * not depending on the robot anatomy). + * Retrieves the angle of the encoder wheel. This angle is calculated using the + * current pulse count, default pulse count (0), wheel resolution, limb gear + * ratio, and default limb angle. This angle is signed, and can go outside + * 360(though it may not depending on the robot anatomy). * * @param[out] handle Pointer to encoder handle structure * @param[in] config Pointer to configuration structure @@ -118,8 +126,8 @@ esp_err_t encoder_get_wheel_angle(encoder_handle_t *handle, * - ESP_OK: Success * - ESP_ERR_INVALID_ARG: Invalid argument (NULL pointer) * - ESP_ERR_*: Other ESP-IDF error codes - */ -esp_err_t encoder_get_limb_angle(encoder_handle_t *handle, - encoder_config_t *config, float *limb_angle) ; - + */ +esp_err_t encoder_get_limb_angle(encoder_handle_t* handle, + encoder_config_t* config, float* limb_angle); +#endif /* _ENCODER_H_ */ diff --git a/components/encoder/test/test_encoder.c b/components/encoder/test/test_encoder.c index 9800710..cd3647f 100644 --- a/components/encoder/test/test_encoder.c +++ b/components/encoder/test/test_encoder.c @@ -1,34 +1,28 @@ #include "encoder.h" -#include "unity_fixture.h" #include "sdkconfig.h" +#include "unity_fixture.h" -TEST_GROUP(ENCODER) ; +TEST_GROUP(ENCODER); -static encoder_handle_t handle ; +static encoder_handle_t handle; static encoder_config_t config = { - .P0_pin = CONFIG_ENCODER_P0_TEST_PIN , - .P1_pin = CONFIG_ENCODER_P1_TEST_PIN , - .glitch_filter_ns = CONFIG_ENCODER_GLITCH_FILTER , - .resolution = CONFIG_ENCODER_0_RESOLUTION , - .gear_ratio = (float)CONFIG_ENCODER_0_LIMB_GEAR_RATIO_X / (float)CONFIG_ENCODER_0_LIMB_GEAR_RATIO_Y , - .limb_default = CONFIG_ENCODER_0_LIMB_REFERENCE, - .invert_angle = CONFIG_ENCODER_0_ANGLE_INVERT - } ; - -TEST_SETUP(ENCODER) { - -} + .P0_pin = CONFIG_ENCODER_P0_TEST_PIN, + .P1_pin = CONFIG_ENCODER_P1_TEST_PIN, + .glitch_filter_ns = CONFIG_ENCODER_GLITCH_FILTER, + .resolution = CONFIG_ENCODER_0_RESOLUTION, + .gear_ratio = (float)CONFIG_ENCODER_0_LIMB_GEAR_RATIO_X / + (float)CONFIG_ENCODER_0_LIMB_GEAR_RATIO_Y, + .limb_default = CONFIG_ENCODER_0_LIMB_REFERENCE, + .invert_angle = CONFIG_ENCODER_0_ANGLE_INVERT}; -TEST_TEAR_DOWN(ENCODER) { +TEST_SETUP(ENCODER) {} -} +TEST_TEAR_DOWN(ENCODER) {} TEST(ENCODER, Encoder_True_Initialization) { - - - esp_err_t err = encoder_init(&handle, &config); - TEST_ASSERT_EQUAL(ESP_OK, err); - TEST_ASSERT_NOT_EQUAL(NULL, &handle.pcnt_unit); + esp_err_t err = encoder_init(&handle, &config); + TEST_ASSERT_EQUAL(ESP_OK, err); + TEST_ASSERT_NOT_EQUAL(NULL, &handle.pcnt_unit); } // TEST(ENCODER, Encoder_Test_Initialization) { @@ -37,9 +31,10 @@ TEST(ENCODER, Encoder_True_Initialization) { // .P1_pin = -1 , // .glitch_filter_ns = CONFIG_ENCODER_GLITCH_FILTER , // .resolution = CONFIG_ENCODER_0_RESOLUTION , -// .gear_ratio = (float)CONFIG_ENCODER_0_LIMB_GEAR_RATIO_X / (float)CONFIG_ENCODER_0_LIMB_GEAR_RATIO_Y , -// .limb_default = CONFIG_ENCODER_0_LIMB_REFERENCE, -// .invert_angle = CONFIG_ENCODER_0_ANGLE_INVERT +// .gear_ratio = (float)CONFIG_ENCODER_0_LIMB_GEAR_RATIO_X / +// (float)CONFIG_ENCODER_0_LIMB_GEAR_RATIO_Y , .limb_default = +// CONFIG_ENCODER_0_LIMB_REFERENCE, .invert_angle = +// CONFIG_ENCODER_0_ANGLE_INVERT // } ; // esp_err_t err = encoder_init(&handle, &virtual_config); @@ -47,24 +42,18 @@ TEST(ENCODER, Encoder_True_Initialization) { // TEST_ASSERT_NOT_EQUAL(NULL, &handle.pcnt_unit); // } - - - -TEST_GROUP(ENCODER_INFINITE) ; +TEST_GROUP(ENCODER_INFINITE); TEST_SETUP(ENCODER_INFINITE) {} TEST_TEAR_DOWN(ENCODER_INFINITE) {} TEST(ENCODER_INFINITE, Encoder_Run_Infinite) { - - encoder_init(&handle, &config); - esp_err_t err = encoder_start(&handle) ; - TEST_ASSERT_EQUAL(ESP_OK, err) ; - - int pulse_count = 0; - while (true) { - TEST_ASSERT_EQUAL(encoder_get_raw_count(&handle, &pulse_count), ESP_OK); - printf("Pulse count: %d\n", pulse_count); - } - - + encoder_init(&handle, &config); + esp_err_t err = encoder_start(&handle); + TEST_ASSERT_EQUAL(ESP_OK, err); + + int pulse_count = 0; + while (true) { + TEST_ASSERT_EQUAL(encoder_get_raw_count(&handle, &pulse_count), ESP_OK); + printf("Pulse count: %d\n", pulse_count); + } } \ No newline at end of file diff --git a/components/encoder/test/test_runner.c b/components/encoder/test/test_runner.c index 6218969..83786a1 100644 --- a/components/encoder/test/test_runner.c +++ b/components/encoder/test/test_runner.c @@ -9,5 +9,4 @@ TEST_GROUP_RUNNER(ENCODER) { TEST_GROUP_RUNNER(ENCODER_INFINITE) { RUN_TEST_CASE(ENCODER_INFINITE, Encoder_Run_Infinite); - } diff --git a/components/i2c_bus/i2c_bus.c b/components/i2c_bus/i2c_bus.c index 2bc8a27..a2f334b 100644 --- a/components/i2c_bus/i2c_bus.c +++ b/components/i2c_bus/i2c_bus.c @@ -1,8 +1,8 @@ #include "i2c_bus.h" -#include "driver/i2c_master.h" -esp_err_t i2c_bus_init(i2c_bus_t *bus, const i2c_bus_config_t *config) { +#include "driver/i2c_master.h" +esp_err_t i2c_bus_init(i2c_bus_t* bus, const i2c_bus_config_t* config) { i2c_master_bus_config_t bus_config = { .i2c_port = config->port, .sda_io_num = config->sda_io_num, diff --git a/components/i2c_bus/include/i2c_bus.h b/components/i2c_bus/include/i2c_bus.h index 7041b48..f4a8218 100644 --- a/components/i2c_bus/include/i2c_bus.h +++ b/components/i2c_bus/include/i2c_bus.h @@ -2,8 +2,8 @@ #define _I2C_BUS_H_ #include "driver/i2c_types.h" -#include "soc/gpio_num.h" #include "esp_err.h" +#include "soc/gpio_num.h" /** * @brief I2C bus configuration structure @@ -12,9 +12,9 @@ * including the I2C port number and GPIO pin assignments. */ typedef struct { - i2c_port_t port; /**< I2C port number (I2C_NUM_0, I2C_NUM_1, etc.) */ - gpio_num_t sda_io_num; /**< GPIO number for SDA (data line) */ - gpio_num_t scl_io_num; /**< GPIO number for SCL (clock line) */ + i2c_port_t port; /**< I2C port number (I2C_NUM_0, I2C_NUM_1, etc.) */ + gpio_num_t sda_io_num; /**< GPIO number for SDA (data line) */ + gpio_num_t scl_io_num; /**< GPIO number for SCL (clock line) */ } i2c_bus_config_t; /** @@ -24,7 +24,7 @@ typedef struct { * for all subsequent operations on the I2C bus, including adding devices. */ typedef struct { - i2c_master_bus_handle_t handle; /**< ESP-IDF I2C master bus handle */ + i2c_master_bus_handle_t handle; /**< ESP-IDF I2C master bus handle */ } i2c_bus_t; /** @@ -40,11 +40,13 @@ typedef struct { * devices using the ESP-IDF i2c_master_bus_add_device() function. * * @param[out] bus Pointer to I2C bus structure to initialize - * @param[in] config Pointer to configuration structure with port and GPIO settings + * @param[in] config Pointer to configuration structure with port and GPIO + * settings * * @return * - ESP_OK: Success - * - ESP_ERR_INVALID_ARG: Invalid argument (NULL pointer, invalid port, or invalid GPIO) + * - ESP_ERR_INVALID_ARG: Invalid argument (NULL pointer, invalid port, or + * invalid GPIO) * - ESP_ERR_NO_MEM: Memory allocation failed * - ESP_ERR_INVALID_STATE: I2C driver already installed on this port * - ESP_ERR_*: Other ESP-IDF error codes from I2C driver initialization @@ -68,6 +70,6 @@ typedef struct { * } * @endcode */ -esp_err_t i2c_bus_init(i2c_bus_t *bus, const i2c_bus_config_t *config); +esp_err_t i2c_bus_init(i2c_bus_t* bus, const i2c_bus_config_t* config); -#endif // _I2C_BUS_H_ +#endif // _I2C_BUS_H_ diff --git a/components/i2c_bus/test/test_i2c_bus.c b/components/i2c_bus/test/test_i2c_bus.c index 9d518b7..d56b731 100644 --- a/components/i2c_bus/test/test_i2c_bus.c +++ b/components/i2c_bus/test/test_i2c_bus.c @@ -16,8 +16,9 @@ TEST_TEAR_DOWN(I2C_Bus) { } TEST(I2C_Bus, I2C_Bus_Initialization) { - i2c_bus_config_t config = { - .port = I2C_NUM_0, .sda_io_num = CONFIG_I2CBUS_TEST_SDA_PIN, .scl_io_num = CONFIG_I2CBUS_TEST_SCL_PIN}; + i2c_bus_config_t config = {.port = I2C_NUM_0, + .sda_io_num = CONFIG_I2CBUS_TEST_SDA_PIN, + .scl_io_num = CONFIG_I2CBUS_TEST_SCL_PIN}; esp_err_t err = i2c_bus_init(&bus_handle, &config); TEST_ASSERT_EQUAL(ESP_OK, err); diff --git a/components/limit_switch/CMakeLists.txt b/components/limit_switch/CMakeLists.txt index 357cf8f..ec3086a 100644 --- a/components/limit_switch/CMakeLists.txt +++ b/components/limit_switch/CMakeLists.txt @@ -1,5 +1,5 @@ idf_component_register( SRCS "limit_switch.c" INCLUDE_DIRS "include" - REQUIRES esp_driver_gpio + REQUIRES esp_driver_gpio signal_bus ) diff --git a/components/limit_switch/include/limit_switch.h b/components/limit_switch/include/limit_switch.h index 9cac676..0d6cf0a 100644 --- a/components/limit_switch/include/limit_switch.h +++ b/components/limit_switch/include/limit_switch.h @@ -11,13 +11,14 @@ * @brief limit_switch configuration structure */ typedef struct { - gpio_num_t limit_gpio; /**< GPIO number for limit switch pin */ + gpio_num_t limit_gpio; /**< GPIO number for limit switch pin */ } limit_switch_config_t; /** * @brief Initialize the limit switch task * - * This function initializes the limit switch task and sets up the limit switch GPIO. + * This function initializes the limit switch task and sets up the limit switch + * GPIO. * * @param[in] config Pointer to configuration structure * @@ -25,6 +26,6 @@ typedef struct { * - ESP_OK: Success * - ESP_ERR_INVALID_ARG: Invalid argument */ -esp_err_t limit_switch_init(const limit_switch_config_t *config); +esp_err_t limit_switch_init(const limit_switch_config_t* config); -#endif // _LIMIT_SWITCH_H \ No newline at end of file +#endif // _LIMIT_SWITCH_H \ No newline at end of file diff --git a/components/limit_switch/limit_switch.c b/components/limit_switch/limit_switch.c index fe0a462..76bdad8 100644 --- a/components/limit_switch/limit_switch.c +++ b/components/limit_switch/limit_switch.c @@ -1,61 +1,44 @@ #include "limit_switch.h" + #include "esp_log.h" #include "freertos/FreeRTOS.h" +#include "signal_bus.h" #define TAG "limit_switch" -static TaskHandle_t limit_switch_task_handle = NULL; +static void IRAM_ATTR limit_switch_isr(void* arg) { + limit_switch_config_t* config = (limit_switch_config_t*)arg; + BaseType_t higher_priority_task_woken = pdFALSE; + + int level = gpio_get_level(config->limit_gpio); + if (level) { + // Limit switch released, clear corresponding bit + xEventGroupClearBitsFromISR(g_motor_events, LIMIT_0); + } else { + // Limit switch triggered, set corresponding bit + xEventGroupSetBitsFromISR(g_motor_events, LIMIT_0, + &higher_priority_task_woken); + } + + portYIELD_FROM_ISR(higher_priority_task_woken); +} -static void IRAM_ATTR limit_switch_isr(void *arg){ - BaseType_t higher_priority_task_woken = pdFALSE; +esp_err_t limit_switch_init(const limit_switch_config_t* config) { + if (!config) { + return ESP_ERR_INVALID_ARG; + } - if (limit_switch_task_handle){ - xTaskNotifyFromISR( - limit_switch_task_handle, - 0, - eNoAction, - &higher_priority_task_woken - ); - } + // Configure GPIO interrupt service + gpio_config_t io_conf = { + .intr_type = GPIO_INTR_ANYEDGE, + .mode = GPIO_MODE_INPUT, + .pin_bit_mask = 1ULL << config->limit_gpio, + .pull_up_en = GPIO_PULLUP_ENABLE, + .pull_down_en = GPIO_PULLDOWN_DISABLE, + }; - if (higher_priority_task_woken) { - portYIELD_FROM_ISR(); - } -} - -void limit_switch_task(void *arg){ - while (1) { - // Wait until GPIO interrupt fires - ulTaskNotifyTake(pdTRUE, portMAX_DELAY); - - ESP_LOGW(TAG,"Limit switch hit!"); - // TODO: React to limit switch - } -} + ESP_ERROR_CHECK(gpio_config(&io_conf)); + ESP_ERROR_CHECK(gpio_isr_handler_add(config->limit_gpio, limit_switch_isr, (void*)config)); -esp_err_t limit_switch_init(const limit_switch_config_t *config) { - ESP_LOGI(TAG, "Initializing limit switches..."); - - if (!config) { - return ESP_ERR_INVALID_ARG; - } - - // Configure GPIO interrupt service - gpio_config_t io_conf = { - .intr_type = GPIO_INTR_NEGEDGE, - .mode = GPIO_MODE_INPUT, - .pin_bit_mask = 1ULL << config->limit_gpio, - .pull_up_en = GPIO_PULLUP_ENABLE, - .pull_down_en = GPIO_PULLDOWN_DISABLE, - }; - - ESP_ERROR_CHECK(gpio_config(&io_conf)); - ESP_ERROR_CHECK(gpio_isr_handler_add(config->limit_gpio, limit_switch_isr, NULL)); - - // Start waiting task - xTaskCreate(limit_switch_task, "limit_switch_task", 4096, NULL, 7, &limit_switch_task_handle); - - ESP_LOGI(TAG, "limit_switch initialized"); - - return ESP_OK; + return ESP_OK; } \ No newline at end of file diff --git a/components/motorhat/CMakeLists.txt b/components/motorhat/CMakeLists.txt index 5475637..7c8ca4d 100644 --- a/components/motorhat/CMakeLists.txt +++ b/components/motorhat/CMakeLists.txt @@ -1,2 +1,2 @@ idf_component_register(SRCS "motorhat.c" "pca9685.c" - INCLUDE_DIRS "include" REQUIRES esp_driver_i2c i2c_bus) + INCLUDE_DIRS "include" REQUIRES esp_driver_i2c i2c_bus signal_bus esp_driver_gpio) diff --git a/components/motorhat/include/motorhat.h b/components/motorhat/include/motorhat.h index eee26d0..d280be9 100644 --- a/components/motorhat/include/motorhat.h +++ b/components/motorhat/include/motorhat.h @@ -1,11 +1,18 @@ #ifndef _MOTORHAT_H_ #define _MOTORHAT_H_ -#include "pca9685.h" #include +#include "freertos/FreeRTOS.h" +#include "freertos/event_groups.h" +#include "driver/gpio.h" +#include "pca9685.h" + #define DEFAULT_FREQUENCY_HZ 1526.0f +/** + * @brief Motor enumeration + */ typedef enum { MOTORHAT_MOTOR1 = 0, MOTORHAT_MOTOR2, @@ -14,27 +21,49 @@ typedef enum { MOTORHAT_NUM_MOTORS } motorhat_motor_t; +/** + * @brief Axis enumeration + */ +typedef enum { + MOTORHAT_AXIS_AZIMUTH = 0, + MOTORHAT_AXIS_ALTITUDE, + MOTORHAT_NUM_AXES +} motorhat_axis_t; + +/** + * @brief Mapping of axes to motors + */ +static const motorhat_motor_t axis_motor[MOTORHAT_NUM_AXES] = { + [MOTORHAT_AXIS_AZIMUTH] = MOTORHAT_MOTOR3, + [MOTORHAT_AXIS_ALTITUDE] = MOTORHAT_MOTOR4, +}; + +/** + * @brief Function callback for clearing encoder count + */ +typedef void (*motorhat_encoder_cb_t)(void* ctx); + /** * @brief Motor direction and state control */ typedef enum { - MOTORHAT_DIRECTION_FORWARD = 0, /**< Motor rotates forward */ - MOTORHAT_DIRECTION_BACKWARD, /**< Motor rotates backward */ - MOTORHAT_DIRECTION_BRAKE, /**< Motor brakes (both inputs high) */ - MOTORHAT_DIRECTION_RELEASE /**< Motor released/coasting (both inputs low) */ + MOTORHAT_DIRECTION_FORWARD = 0, /**< Motor rotates forward */ + MOTORHAT_DIRECTION_BACKWARD, /**< Motor rotates backward */ + MOTORHAT_DIRECTION_BRAKE, /**< Motor brakes (both inputs high) */ + MOTORHAT_DIRECTION_RELEASE /**< Motor released/coasting (both inputs low) */ } motorhat_direction_t; /** * @brief Channel mapping for a single motor - * + * * Maps the three PCA9685 channels required to control one motor: * - IN1 and IN2: Direction control inputs to the H-bridge * - PWM: Speed control via pulse-width modulation */ typedef struct { - pca9685_channel_t in1_channel; /**< H-bridge IN1 control channel */ - pca9685_channel_t in2_channel; /**< H-bridge IN2 control channel */ - pca9685_channel_t pwm_channel; /**< PWM speed control channel */ + pca9685_channel_t in1_channel; /**< H-bridge IN1 control channel */ + pca9685_channel_t in2_channel; /**< H-bridge IN2 control channel */ + pca9685_channel_t pwm_channel; /**< PWM speed control channel */ } motorhat_motor_channels_t; /** @@ -42,18 +71,26 @@ typedef struct { */ typedef struct { pca9685_config_t pca9685_config; + uint16_t polar_pan_speed; /**< Speed to use for polar pan movements */ + motorhat_encoder_cb_t encoder_cb; /**< Clear encoder count function callback */ + void* encoder_ctx; /** < Clear encoder count callback context (the encoder handle) */ + gpio_num_t limit_gpio; /**< GPIO number for limit switch pin */ } motorhat_config_t; /** * @brief Motor HAT device handle */ typedef struct { - pca9685_handle_t *pca9685; + pca9685_handle_t pca9685; /**< Handle for the underlying PCA9685 controller */ + uint16_t polar_pan_speed; /**< Speed to use for polar pan movements */ + motorhat_encoder_cb_t encoder_cb; /**< Clear encoder count function callback */ + void* encoder_ctx; /** < Clear encoder count callback context (the encoder handle) */ + gpio_num_t limit_gpio; /**< GPIO number for limit switch pin */ } motorhat_handle_t; /** * @brief Channel assignments for each motor on the Adafruit Motor HAT - * + * * This mapping is specific to the Adafruit Motor HAT v2 pinout: * - Motor 1: IN1=CH10, IN2=CH9, PWM=CH8 * - Motor 2: IN1=CH11, IN2=CH12, PWM=CH13 @@ -61,21 +98,22 @@ typedef struct { * - Motor 4: IN1=CH5, IN2=CH6, PWM=CH7 */ static const motorhat_motor_channels_t motor_channels[MOTORHAT_NUM_MOTORS] = { - {PCA9685_CHANNEL10, PCA9685_CHANNEL9, PCA9685_CHANNEL8}, // Motor 1 - {PCA9685_CHANNEL11, PCA9685_CHANNEL12, PCA9685_CHANNEL13}, // Motor 2 - {PCA9685_CHANNEL4, PCA9685_CHANNEL3, PCA9685_CHANNEL2}, // Motor 3 - {PCA9685_CHANNEL5, PCA9685_CHANNEL6, PCA9685_CHANNEL7} // Motor 4 + {PCA9685_CHANNEL10, PCA9685_CHANNEL9, PCA9685_CHANNEL8}, // Motor 1 + {PCA9685_CHANNEL11, PCA9685_CHANNEL12, PCA9685_CHANNEL13}, // Motor 2 + {PCA9685_CHANNEL4, PCA9685_CHANNEL3, PCA9685_CHANNEL2}, // Motor 3 + {PCA9685_CHANNEL5, PCA9685_CHANNEL6, PCA9685_CHANNEL7} // Motor 4 }; /** * @brief Initialize the Motor HAT controller * - * Initializes the underlying PCA9685 PWM controller with the provided configuration. - * After successful initialization, motors can be controlled using the speed and - * direction functions. + * Initializes the underlying PCA9685 PWM controller with the provided + * configuration. After successful initialization, motors can be controlled + * using the speed and direction functions. * * @param[out] handle Pointer to Motor HAT handle structure - * @param[in] config Pointer to configuration structure containing PCA9685 settings + * @param[in] config Pointer to configuration structure containing PCA9685 + * settings * * @return * - ESP_OK: Success @@ -85,8 +123,84 @@ static const motorhat_motor_channels_t motor_channels[MOTORHAT_NUM_MOTORS] = { * @note The handle->pca9685 pointer must be allocated and point to a valid * pca9685_handle_t structure before calling this function */ -esp_err_t motorhat_init(motorhat_handle_t *handle, - const motorhat_config_t *config); +esp_err_t motorhat_init(motorhat_handle_t* handle, + const motorhat_config_t* config); + +/** + * @brief Polar pan command + * + * Triggers a polar pan movement based on the specified azimuth and altitude + * deltas, with timing parameters for delay and duration. This is a high-level + * command that can be implemented to control multiple motors in coordination to + * achieve the desired pan movement. + * + * @param[in] delta_azimuth Azimuth delta for the pan movement + * @param[in] delta_altitude Altitude delta for the pan movement + * @param[in] delay_ms Delay before starting the movement + * @param[in] time_ms Duration of the movement + * + * @return + * - ESP_OK: Success + * - ESP_ERR_*: Other ESP-IDF error codes propogated from motor control + * functions + */ +esp_err_t motorhat_polar_pan(int16_t delta_azimuth, int16_t delta_altitude, + uint16_t delay_ms, uint16_t time_ms); + +/** + * @brief Polar pan start command + * + * Starts a continuous pan movement that will run until a stop command is + * received. Only uses two axes currently, directly relating azimuth to axis 1 and altitude to axis 2. + * + * @param[in] delta_azimuth Azimuth delta for the pan movement + * @param[in] delta_altitude Altitude delta for the pan movement + * + * @return + * - ESP_OK: Success + * - ESP_ERR_*: Other ESP-IDF error codes propogated from motor control + * functions + */ +esp_err_t motorhat_polar_pan_start(int8_t delta_azimuth, int8_t delta_altitude); + +/** + * @brief Polar pan stop command + * + * Stops any ongoing polar pan movement initiated by polar_pan_start. + * + * @return + * - ESP_OK: Success + * - ESP_ERR_*: Other ESP-IDF error codes propogated from motor control + * functions + */ +esp_err_t motorhat_polar_pan_stop(void); + +/** + * @brief Home command + * + * Moves the motors to a predefined home position after an optional delay. Blocks high level movement commands + * such as polar pan, setting the motor speed and direction directly to operate around these commands. + * + * The homing sequence is modeled after the sequence demonstrated by the SCORBOT ER-V, since both robots + * have the unusual placement of their limit switches at the centers of their corresponding axis's range, + * rather than at the edges. The one difference is that this homing sequence starts with moving each motor FORWARD, + * and ends with it moving BACKWARD to release the limit switch. This is because for the vertical axis, the slowed + * down motion to release the limit switch and reach the final homed position currently stalls the motor when moving upward, + * so I've reversed the order so that it happens while moving downward. This should be fixed by replacing the + * current motor drivers with ones capable of pushing the proper amount of amps to drive the motors, + * but in the meantime reversing the order of FORWARD and BACKWArd motion in homing avoids this issue. + * + * IMPORTANT: until current sensing is fully functional, the robot can't detect reaching the edge of it's range and will + * break if you attempt to home it while the arm is on the wrong side of the limit switch. + * + * @param[in] delay_ms Delay before starting the homing movement + * + * @return + * - ESP_OK: Success + * - ESP_ERR_*: Other ESP-IDF error codes propogated from motor control + * functions + */ +esp_err_t motorhat_home(uint16_t delay_ms); /** * @brief Set motor speed @@ -100,13 +214,15 @@ esp_err_t motorhat_init(motorhat_handle_t *handle, * * @return * - ESP_OK: Success - * - ESP_ERR_INVALID_ARG: Invalid argument (NULL handle, invalid motor, or speed > 4096) + * - ESP_ERR_INVALID_ARG: Invalid argument (NULL handle, invalid motor, or + * speed > 4096) + * - ESP_ERR_INVALID_STATE: Motor is in fault state (stop bits set) * - ESP_ERR_*: Other ESP-IDF error codes from I2C operations * * @note For typical usage with 0-255 speed range, scale your value: * actual_speed = (your_speed * PCA9685_PWM_MAX) / 255 */ -esp_err_t motorhat_set_motor_speed(motorhat_handle_t *handle, +esp_err_t motorhat_set_motor_speed(motorhat_handle_t* handle, motorhat_motor_t motor, uint16_t speed); /** @@ -115,7 +231,7 @@ esp_err_t motorhat_set_motor_speed(motorhat_handle_t *handle, * Controls the motor's direction and braking state by setting the IN1 and IN2 * control signals appropriately: * - FORWARD: IN1=HIGH, IN2=LOW - * - BACKWARD: IN1=LOW, IN2=HIGH + * - BACKWARD: IN1=LOW, IN2=HIGH * - BRAKE: IN1=HIGH, IN2=HIGH (active braking) * - RELEASE: IN1=LOW, IN2=LOW (coasting/free-running) * @@ -125,14 +241,30 @@ esp_err_t motorhat_set_motor_speed(motorhat_handle_t *handle, * * @return * - ESP_OK: Success - * - ESP_ERR_INVALID_ARG: Invalid argument (NULL handle, invalid motor, or invalid direction) + * - ESP_ERR_INVALID_ARG: Invalid argument (NULL handle, invalid motor, or + * invalid direction) + * - ESP_ERR_INVALID_STATE: Motor is in fault state (stop bits set) * - ESP_ERR_*: Other ESP-IDF error codes from I2C operations * * @note Setting direction does not affect speed. Set speed separately using * motorhat_set_motor_speed() */ -esp_err_t motorhat_set_motor_direction(motorhat_handle_t *handle, +esp_err_t motorhat_set_motor_direction(motorhat_handle_t* handle, motorhat_motor_t motor, motorhat_direction_t direction); -#endif // _MOTORHAT_H_ +/** + * @brief Emergency stop all motors + * + * Immediately stops all motors and releases control signals. + * + * @param[in] handle Pointer to Motor HAT handle + * + * @return + * - ESP_OK: Success + * - ESP_ERR_INVALID_ARG: Invalid argument (NULL handle) + */ + +esp_err_t motorhat_emergency_stop(motorhat_handle_t* handle); + +#endif // _MOTORHAT_H_ diff --git a/components/motorhat/include/pca9685.h b/components/motorhat/include/pca9685.h index c05cccf..fdb2803 100644 --- a/components/motorhat/include/pca9685.h +++ b/components/motorhat/include/pca9685.h @@ -1,9 +1,10 @@ #ifndef _PCA9685_H_ #define _PCA9685_H_ +#include + #include "driver/i2c_types.h" #include "esp_err.h" -#include #define PCA9685_PWM_MAX 4096 #define PCA9685_I2C_MASTER_TIMEOUT_MS 1000 @@ -16,8 +17,8 @@ // Calculate pre-scale value for a desired PWM frequency // See PCA9685 datasheet section 7.3.5 // The + 0.5f is for rounding to nearest integer without truncation -#define PCA9685_CALC_PRE_SCALE(freq_hz) \ - ((uint8_t)((PCA9685_OSCILLATOR_FREQ_HZ / (4096.0f * (freq_hz))) - 1.0f + \ +#define PCA9685_CALC_PRE_SCALE(freq_hz) \ + ((uint8_t)((PCA9685_OSCILLATOR_FREQ_HZ / (4096.0f * (freq_hz))) - 1.0f + \ 0.5f)) /** @@ -204,13 +205,14 @@ typedef struct { * frequency) * - ESP_ERR_*: Other ESP-IDF error codes from I2C operations */ -esp_err_t pca9685_init(pca9685_handle_t *handle, - const pca9685_config_t *config); +esp_err_t pca9685_init(pca9685_handle_t* handle, + const pca9685_config_t* config); /** * @brief Deinitialize the PCA9685 device * - * This function releases the I2C device handle and performs any necessary cleanup. + * This function releases the I2C device handle and performs any necessary + * cleanup. * * @param[in] handle Pointer to PCA9685 handle structure * @return @@ -218,7 +220,7 @@ esp_err_t pca9685_init(pca9685_handle_t *handle, * - ESP_ERR_INVALID_ARG: Invalid argument (NULL pointer) * - ESP_ERR_*: Other ESP-IDF error codes from I2C operations */ - esp_err_t pca9685_deinit(pca9685_handle_t *handle); +esp_err_t pca9685_deinit(pca9685_handle_t* handle); /** * @brief Set PWM duty cycle for a specific channel @@ -238,7 +240,7 @@ esp_err_t pca9685_init(pca9685_handle_t *handle, * duty cycle > 4096) * - ESP_ERR_*: Other ESP-IDF error codes from I2C operations */ -esp_err_t pca9685_set_duty_cycle(pca9685_handle_t *handle, +esp_err_t pca9685_set_duty_cycle(pca9685_handle_t* handle, pca9685_channel_t channel, uint16_t duty_cycle); @@ -257,7 +259,7 @@ esp_err_t pca9685_set_duty_cycle(pca9685_handle_t *handle, * - ESP_ERR_INVALID_ARG: Invalid argument (NULL handle or invalid channel) * - ESP_ERR_*: Other ESP-IDF error codes from I2C operations */ -esp_err_t pca9685_digital_write(pca9685_handle_t *handle, +esp_err_t pca9685_digital_write(pca9685_handle_t* handle, pca9685_channel_t channel, bool level); /** @@ -279,7 +281,7 @@ esp_err_t pca9685_digital_write(pca9685_handle_t *handle, * values > 4096) * - ESP_ERR_*: Other ESP-IDF error codes from I2C operations */ -esp_err_t pca9685_write_channel_registers(pca9685_handle_t *handle, +esp_err_t pca9685_write_channel_registers(pca9685_handle_t* handle, pca9685_channel_t channel, uint16_t on, uint16_t off); @@ -295,8 +297,8 @@ esp_err_t pca9685_write_channel_registers(pca9685_handle_t *handle, * - ESP_ERR_INVALID_ARG: Invalid argument (NULL pointer) * - ESP_ERR_*: Other ESP-IDF error codes from I2C operations */ -esp_err_t pca9685_read_register(pca9685_handle_t *handle, - pca9685_register_t reg, uint8_t *data); +esp_err_t pca9685_read_register(pca9685_handle_t* handle, + pca9685_register_t reg, uint8_t* data); /** * @brief Write a single byte to a PCA9685 register @@ -310,7 +312,7 @@ esp_err_t pca9685_read_register(pca9685_handle_t *handle, * - ESP_ERR_INVALID_ARG: Invalid argument (NULL handle) * - ESP_ERR_*: Other ESP-IDF error codes from I2C operations */ -esp_err_t pca9685_write_register(pca9685_handle_t *handle, +esp_err_t pca9685_write_register(pca9685_handle_t* handle, pca9685_register_t reg, uint8_t data); -#endif // _PCA9685_H_ +#endif // _PCA9685_H_ diff --git a/components/motorhat/motorhat.c b/components/motorhat/motorhat.c index ff68113..952e69e 100644 --- a/components/motorhat/motorhat.c +++ b/components/motorhat/motorhat.c @@ -1,32 +1,285 @@ #include "motorhat.h" + #include "esp_err.h" +#include "esp_log.h" +#include "signal_bus.h" +#include "driver/gpio.h" + +#define TAG "motorhat" + +static motorhat_handle_t* s_handle = NULL; + +static motorhat_direction_t delta_to_direction(int8_t delta) { + if (delta > 0) return MOTORHAT_DIRECTION_FORWARD; + if (delta < 0) return MOTORHAT_DIRECTION_BACKWARD; + + return MOTORHAT_DIRECTION_RELEASE; +} + +void motor_stop_task(void* args) { + while (1) { + // Block indefinitely until any stop bit is set + xEventGroupWaitBits(g_motor_events, CURRENT_ANY, + pdFALSE, // don't clear bits on exit + pdFALSE, // any bit (OR) + portMAX_DELAY); + + // Check for homing sequence before reacting + if (!(xEventGroupGetBits(g_motor_events) & HOMING_FLAG)){ + motorhat_emergency_stop(s_handle); + } else { + // Homing sequence will handle this, yield and check again shortly + vTaskDelay(pdMS_TO_TICKS(10)); +} + } +} -esp_err_t motorhat_init(motorhat_handle_t *handle, - const motorhat_config_t *config) { +esp_err_t motorhat_init(motorhat_handle_t* handle, + const motorhat_config_t* config) { if (handle == NULL || config == NULL) { return ESP_ERR_INVALID_ARG; } - return pca9685_init(handle->pca9685, &config->pca9685_config); + s_handle = handle; + handle->polar_pan_speed = config->polar_pan_speed; + handle->encoder_cb = config->encoder_cb; + handle->encoder_ctx = config->encoder_ctx; + handle->limit_gpio = config->limit_gpio; + + xTaskCreate(motor_stop_task, "motor_stop_task", 4096, handle, 8, NULL); + + return pca9685_init(&handle->pca9685, &config->pca9685_config); + return ESP_OK; +} + +// Socket commands +esp_err_t motorhat_home(uint16_t delay_ms) { + if (xEventGroupGetBits(g_motor_events) & HOMING_FLAG) { + ESP_LOGW(TAG, "Homing in progress, cannot send commands"); + return ESP_ERR_INVALID_STATE; + } + + if (s_handle == NULL) return ESP_ERR_INVALID_STATE; + + ESP_LOGI(TAG, "Received home command: delay_ms=%d", delay_ms); + + // Set homing flag + xEventGroupSetBits(g_motor_events, HOMING_FLAG); + + // Add delay + vTaskDelay(pdMS_TO_TICKS(delay_ms)); + + // Stop any movement before homing + for (int m = MOTORHAT_MOTOR1; m < MOTORHAT_NUM_MOTORS; m++) { + motorhat_set_motor_speed(s_handle, m, 0); + motorhat_set_motor_direction(s_handle, m, MOTORHAT_DIRECTION_RELEASE); + } + + // Home each axis individually until we candetermine which motor triggers limit switch and current events + for (int axis = MOTORHAT_AXIS_AZIMUTH; axis < MOTORHAT_NUM_AXES; axis++) { + int m = axis_motor[axis]; + ESP_LOGI(TAG, "Homing motor %d", m); + + motorhat_set_motor_direction(s_handle, m, MOTORHAT_DIRECTION_FORWARD); + motorhat_set_motor_speed(s_handle, m, s_handle->polar_pan_speed); + + EventBits_t bits = xEventGroupWaitBits( + g_motor_events, + HOME_EVENT, + pdTRUE, + pdFALSE, + portMAX_DELAY + ); + + // Stop when a homing event is detected + motorhat_set_motor_speed(s_handle, m, 0); + + if (bits & CURRENT_ANY) { + ESP_LOGI(TAG, "Motor %d hit the forward end of its range", m); + + // Small pause before going opposite direction + vTaskDelay(pdMS_TO_TICKS(200)); + + // Switch directions and move towards limit switch + motorhat_set_motor_direction(s_handle, m, MOTORHAT_DIRECTION_BACKWARD); + motorhat_set_motor_speed(s_handle, m, s_handle->polar_pan_speed); + + bits = xEventGroupWaitBits( + g_motor_events, + HOME_EVENT, + pdTRUE, + pdFALSE, + portMAX_DELAY + ); + + // Stop when a homing event is detected + motorhat_set_motor_speed(s_handle, m, 0); + + if (bits & LIMIT_ANY) { + ESP_LOGI(TAG, "Motor %d hit a limit switch moving backward", m); + + // Continue forward slowly until limit switch releases + motorhat_set_motor_speed(s_handle, m, s_handle->polar_pan_speed / 3); + + // Detect limit switch release by polling + while (!gpio_get_level(s_handle->limit_gpio)) { + vTaskDelay(pdMS_TO_TICKS(10)); + } + ESP_LOGI(TAG, "Motor %d finished homing", m); + + // Homing finished, stop the motor and continue sequence + motorhat_set_motor_speed(s_handle, m, 0); + motorhat_set_motor_direction(s_handle, m, MOTORHAT_DIRECTION_RELEASE); + + } else { + ESP_LOGW(TAG, "Motor %d unexpected homing event detected", m); + motorhat_emergency_stop(s_handle); + } + + } else if (bits & LIMIT_ANY) { + ESP_LOGI(TAG, "Motor %d hit a limit switch moving forward", m); + + // Small pause before going the opposite direction + vTaskDelay(pdMS_TO_TICKS(200)); + + // Turn around and move slowly until limit switch releases + motorhat_set_motor_direction(s_handle, m, MOTORHAT_DIRECTION_BACKWARD); + motorhat_set_motor_speed(s_handle, m, s_handle->polar_pan_speed / 3); + + // Detect limit switch release by polling + while (!gpio_get_level(s_handle->limit_gpio)) { + vTaskDelay(pdMS_TO_TICKS(10)); + } + ESP_LOGI(TAG, "Motor %d finished homing", m); + + // Homing finished, stop the motor and continue sequence + motorhat_set_motor_speed(s_handle, m, 0); + motorhat_set_motor_direction(s_handle, m, MOTORHAT_DIRECTION_RELEASE); + + } else { + ESP_LOGW(TAG, "Motor %d unexpected homing event detected", m); + motorhat_emergency_stop(s_handle); + } + } + + // Reset encoder values + if (s_handle->encoder_cb) { + s_handle->encoder_cb(s_handle->encoder_ctx); + ESP_LOGI(TAG, "Homing successful, encoders reset"); + } else { + ESP_LOGW(TAG, "Encoder reset callback not found, homing unsuccessful"); + } + + // Clear homing flag + xEventGroupClearBits(g_motor_events, HOMING_FLAG); + + return ESP_OK; } -esp_err_t motorhat_set_motor_speed(motorhat_handle_t *handle, +esp_err_t motorhat_polar_pan(int16_t delta_azimuth, int16_t delta_altitude, + uint16_t delay_ms, uint16_t time_ms) { + if (xEventGroupGetBits(g_motor_events) & HOMING_FLAG) { + ESP_LOGW(TAG, "Homing in progress, cannot send commands"); + return ESP_ERR_INVALID_STATE; + } + + if (s_handle == NULL) return ESP_ERR_INVALID_STATE; + + ESP_LOGI(TAG, "Received polar pan command: delta_azimuth=%d, delta_altitude=%d, delay_ms=%d, time_ms=%d", + delta_azimuth, delta_altitude, delay_ms, time_ms); + + // TODO: Implement polar pan logic using motorhat_set_motor_direction and + // motorhat_set_motor_speed + return ESP_OK; +} + +esp_err_t motorhat_polar_pan_start(int8_t delta_azimuth, + int8_t delta_altitude) { + if (xEventGroupGetBits(g_motor_events) & HOMING_FLAG) { + ESP_LOGW(TAG, "Homing in progress, cannot send commands"); + return ESP_ERR_INVALID_STATE; + } + + if (s_handle == NULL) return ESP_ERR_INVALID_STATE; + + ESP_LOGI(TAG, "Received polar pan start command: delta_azimuth=%d, delta_altitude=%d", delta_azimuth, delta_altitude); + + // Set both axes speed to 0 to prevent direction change while moving + esp_err_t err = + motorhat_set_motor_speed(s_handle, axis_motor[MOTORHAT_AXIS_AZIMUTH], 0); + if (err != ESP_OK) { + return err; + } + err = + motorhat_set_motor_speed(s_handle, axis_motor[MOTORHAT_AXIS_ALTITUDE], 0); + if (err != ESP_OK) { + return err; + } + + // Set directions based on deltas + err = + motorhat_set_motor_direction(s_handle, axis_motor[MOTORHAT_AXIS_AZIMUTH], + delta_to_direction(delta_azimuth)); + if (err != ESP_OK) { + return err; + } + err = + motorhat_set_motor_direction(s_handle, axis_motor[MOTORHAT_AXIS_ALTITUDE], + delta_to_direction(delta_altitude)); + if (err != ESP_OK) { + return err; + } + + // Set speeds to a default value + err = motorhat_set_motor_speed(s_handle, axis_motor[MOTORHAT_AXIS_AZIMUTH], + s_handle->polar_pan_speed); + if (err != ESP_OK) { + return err; + } + return motorhat_set_motor_speed(s_handle, axis_motor[MOTORHAT_AXIS_ALTITUDE], + s_handle->polar_pan_speed); +} + +esp_err_t motorhat_polar_pan_stop(void) { + if (xEventGroupGetBits(g_motor_events) & HOMING_FLAG) { + ESP_LOGW(TAG, "Homing in progress, cannot send commands"); + return ESP_ERR_INVALID_STATE; + } + + if (s_handle == NULL) return ESP_ERR_INVALID_STATE; + + ESP_LOGI(TAG, "Received polar pan stop command"); + + for (int m = MOTORHAT_MOTOR1; m < MOTORHAT_NUM_MOTORS; m++) { + motorhat_set_motor_speed(s_handle, m, 0); + motorhat_set_motor_direction(s_handle, m, MOTORHAT_DIRECTION_BRAKE); + } + return ESP_OK; +} + +// Motor control to be used by socket command functions +esp_err_t motorhat_set_motor_speed(motorhat_handle_t* handle, motorhat_motor_t motor, uint16_t speed) { if (handle == NULL || motor < MOTORHAT_MOTOR1 || motor >= MOTORHAT_NUM_MOTORS) { return ESP_ERR_INVALID_ARG; } + if (xEventGroupGetBits(g_motor_events) & CURRENT_ANY) { + ESP_LOGW(TAG, "Motor %d in fault state, cannot set speed", motor); + return ESP_ERR_INVALID_STATE; + } + if (speed > PCA9685_PWM_MAX) { return ESP_ERR_INVALID_ARG; } - const motorhat_motor_channels_t *channels = &motor_channels[motor]; + const motorhat_motor_channels_t* channels = &motor_channels[motor]; - return pca9685_set_duty_cycle(handle->pca9685, channels->pwm_channel, speed); + return pca9685_set_duty_cycle(&handle->pca9685, channels->pwm_channel, speed); } -esp_err_t motorhat_set_motor_direction(motorhat_handle_t *handle, +esp_err_t motorhat_set_motor_direction(motorhat_handle_t* handle, motorhat_motor_t motor, motorhat_direction_t direction) { if (handle == NULL || motor < MOTORHAT_MOTOR1 || @@ -34,38 +287,75 @@ esp_err_t motorhat_set_motor_direction(motorhat_handle_t *handle, return ESP_ERR_INVALID_ARG; } - const motorhat_motor_channels_t *channels = &motor_channels[motor]; + if (xEventGroupGetBits(g_motor_events) & CURRENT_ANY) { + ESP_LOGW(TAG, "Motor %d in fault state, cannot set direction", motor); + return ESP_ERR_INVALID_STATE; + } + + const motorhat_motor_channels_t* channels = &motor_channels[motor]; esp_err_t err; switch (direction) { - case MOTORHAT_DIRECTION_FORWARD: - err = pca9685_digital_write(handle->pca9685, channels->in1_channel, true); - if (err != ESP_OK) - return err; - err = pca9685_digital_write(handle->pca9685, channels->in2_channel, false); - break; - case MOTORHAT_DIRECTION_BACKWARD: - err = pca9685_digital_write(handle->pca9685, channels->in1_channel, false); - if (err != ESP_OK) - return err; - err = pca9685_digital_write(handle->pca9685, channels->in2_channel, true); - break; - case MOTORHAT_DIRECTION_BRAKE: - err = pca9685_digital_write(handle->pca9685, channels->in1_channel, true); - if (err != ESP_OK) - return err; - err = pca9685_digital_write(handle->pca9685, channels->in2_channel, true); - break; - case MOTORHAT_DIRECTION_RELEASE: - err = pca9685_digital_write(handle->pca9685, channels->in1_channel, false); - if (err != ESP_OK) - return err; - err = pca9685_digital_write(handle->pca9685, channels->in2_channel, false); - break; - default: - return ESP_ERR_INVALID_ARG; + case MOTORHAT_DIRECTION_FORWARD: + err = + pca9685_digital_write(&handle->pca9685, channels->in1_channel, true); + if (err != ESP_OK) return err; + err = + pca9685_digital_write(&handle->pca9685, channels->in2_channel, false); + err = ESP_OK; + break; + case MOTORHAT_DIRECTION_BACKWARD: + err = + pca9685_digital_write(&handle->pca9685, channels->in1_channel, false); + if (err != ESP_OK) return err; + err = + pca9685_digital_write(&handle->pca9685, channels->in2_channel, true); + err = ESP_OK; + break; + case MOTORHAT_DIRECTION_BRAKE: + err = + pca9685_digital_write(&handle->pca9685, channels->in1_channel, true); + if (err != ESP_OK) return err; + err = + pca9685_digital_write(&handle->pca9685, channels->in2_channel, true); + err = ESP_OK; + break; + case MOTORHAT_DIRECTION_RELEASE: + err = + pca9685_digital_write(&handle->pca9685, channels->in1_channel, false); + if (err != ESP_OK) return err; + err = + pca9685_digital_write(&handle->pca9685, channels->in2_channel, false); + err = ESP_OK; + break; + default: + return ESP_ERR_INVALID_ARG; } return err; } + +// Emergency stop on all motors that bypasses normal logic flow +esp_err_t motorhat_emergency_stop(motorhat_handle_t* handle) { + if (handle == NULL) { + return ESP_ERR_INVALID_ARG; + } + + esp_err_t first_err = ESP_OK; + for (int m = MOTORHAT_MOTOR1; m < MOTORHAT_NUM_MOTORS; m++) { + const motorhat_motor_channels_t* ch = &motor_channels[m]; + + // Stop PWM cycle and release both control lines + esp_err_t err = + pca9685_set_duty_cycle(&handle->pca9685, ch->pwm_channel, 0); + if (err != ESP_OK && first_err == ESP_OK) first_err = err; + + err = pca9685_digital_write(&handle->pca9685, ch->in1_channel, false); + if (err != ESP_OK && first_err == ESP_OK) first_err = err; + + err = pca9685_digital_write(&handle->pca9685, ch->in2_channel, false); + if (err != ESP_OK && first_err == ESP_OK) first_err = err; + } + return first_err; +} diff --git a/components/motorhat/pca9685.c b/components/motorhat/pca9685.c index ba11317..1ca65d8 100644 --- a/components/motorhat/pca9685.c +++ b/components/motorhat/pca9685.c @@ -1,8 +1,10 @@ #include "pca9685.h" + +#include + #include "driver/i2c_master.h" #include "esp_err.h" #include "freertos/FreeRTOS.h" -#include static pca9685_channel_registers_t channel_regs[PCA9685_CHANNEL15 + 1] = { {PCA9685_LED0_ON_L, PCA9685_LED0_ON_H, PCA9685_LED0_OFF_L, @@ -39,8 +41,8 @@ static pca9685_channel_registers_t channel_regs[PCA9685_CHANNEL15 + 1] = { PCA9685_LED15_OFF_H}, }; -esp_err_t pca9685_init(pca9685_handle_t *handle, - const pca9685_config_t *config) { +esp_err_t pca9685_init(pca9685_handle_t* handle, + const pca9685_config_t* config) { if (handle == NULL || config == NULL) { return ESP_ERR_INVALID_ARG; } @@ -98,9 +100,9 @@ esp_err_t pca9685_init(pca9685_handle_t *handle, vTaskDelay(pdMS_TO_TICKS(5)); // Enable auto increment - ret = pca9685_write_register(handle, PCA9685_MODE1, - oldmode | PCA9685_MODE1_RESTART | - PCA9685_MODE1_AI); + ret = pca9685_write_register( + handle, PCA9685_MODE1, + oldmode | PCA9685_MODE1_RESTART | PCA9685_MODE1_AI); if (ret != ESP_OK) { return ret; } @@ -108,7 +110,7 @@ esp_err_t pca9685_init(pca9685_handle_t *handle, return ESP_OK; } -esp_err_t pca9685_deinit(pca9685_handle_t *handle) { +esp_err_t pca9685_deinit(pca9685_handle_t* handle) { if (handle == NULL || handle->dev_handle == NULL) { return ESP_ERR_INVALID_ARG; } @@ -122,7 +124,7 @@ esp_err_t pca9685_deinit(pca9685_handle_t *handle) { return ESP_OK; } -esp_err_t pca9685_set_duty_cycle(pca9685_handle_t *handle, +esp_err_t pca9685_set_duty_cycle(pca9685_handle_t* handle, pca9685_channel_t channel, uint16_t duty_cycle) { if (handle == NULL || channel < PCA9685_CHANNEL0 || @@ -149,7 +151,7 @@ esp_err_t pca9685_set_duty_cycle(pca9685_handle_t *handle, return pca9685_write_channel_registers(handle, channel, on, off); } -esp_err_t pca9685_digital_write(pca9685_handle_t *handle, +esp_err_t pca9685_digital_write(pca9685_handle_t* handle, pca9685_channel_t channel, bool level) { if (handle == NULL || channel < PCA9685_CHANNEL0 || channel > PCA9685_CHANNEL15) { @@ -163,7 +165,7 @@ esp_err_t pca9685_digital_write(pca9685_handle_t *handle, } } -esp_err_t pca9685_write_channel_registers(pca9685_handle_t *handle, +esp_err_t pca9685_write_channel_registers(pca9685_handle_t* handle, pca9685_channel_t channel, uint16_t on, uint16_t off) { if (handle == NULL || channel < PCA9685_CHANNEL0 || @@ -177,7 +179,6 @@ esp_err_t pca9685_write_channel_registers(pca9685_handle_t *handle, pca9685_channel_registers_t regs = channel_regs[channel]; - // The PCA9685 uses 12-bit PWM values for each channel (0–4095). // These 12 bits are split across two 8-bit registers: // LEDn_ON_L -> bits 7..0 (low byte) @@ -207,17 +208,17 @@ esp_err_t pca9685_write_channel_registers(pca9685_handle_t *handle, PCA9685_I2C_MASTER_TIMEOUT_MS); } -esp_err_t pca9685_read_register(pca9685_handle_t *handle, - pca9685_register_t reg, uint8_t *data) { +esp_err_t pca9685_read_register(pca9685_handle_t* handle, + pca9685_register_t reg, uint8_t* data) { if (handle == NULL || data == NULL) { return ESP_ERR_INVALID_ARG; } - return i2c_master_transmit_receive(handle->dev_handle, (uint8_t *)®, 1, + return i2c_master_transmit_receive(handle->dev_handle, (uint8_t*)®, 1, data, 1, PCA9685_I2C_MASTER_TIMEOUT_MS); } -esp_err_t pca9685_write_register(pca9685_handle_t *handle, +esp_err_t pca9685_write_register(pca9685_handle_t* handle, pca9685_register_t reg, uint8_t data) { if (handle == NULL) { return ESP_ERR_INVALID_ARG; diff --git a/components/motorhat/test/test_pca9685.c b/components/motorhat/test/test_pca9685.c index fe548b1..883a82b 100644 --- a/components/motorhat/test/test_pca9685.c +++ b/components/motorhat/test/test_pca9685.c @@ -40,9 +40,8 @@ TEST(PCA9685, PCA9685_Initialization) { } TEST(PCA9685, PCA9685_Wrong_Address) { - pca9685_config_t config = { - .i2c_addr = 0x00, // Invalid address + .i2c_addr = 0x00, // Invalid address .i2c_speed_hz = 400000, .pwm_freq_hz = 1000, .bus_handle = bus_handle.handle, @@ -67,10 +66,14 @@ TEST(PCA9685, PCA9685_Set_Duty_Cycle) { // Read back the registers to verify uint8_t on_l, on_h, off_l, off_h; - TEST_ASSERT_EQUAL(pca9685_read_register(&handle, PCA9685_LED0_ON_L, &on_l), ESP_OK); - TEST_ASSERT_EQUAL(pca9685_read_register(&handle, PCA9685_LED0_ON_H, &on_h), ESP_OK); - TEST_ASSERT_EQUAL(pca9685_read_register(&handle, PCA9685_LED0_OFF_L, &off_l), ESP_OK); - TEST_ASSERT_EQUAL(pca9685_read_register(&handle, PCA9685_LED0_OFF_H, &off_h), ESP_OK); + TEST_ASSERT_EQUAL(pca9685_read_register(&handle, PCA9685_LED0_ON_L, &on_l), + ESP_OK); + TEST_ASSERT_EQUAL(pca9685_read_register(&handle, PCA9685_LED0_ON_H, &on_h), + ESP_OK); + TEST_ASSERT_EQUAL(pca9685_read_register(&handle, PCA9685_LED0_OFF_L, &off_l), + ESP_OK); + TEST_ASSERT_EQUAL(pca9685_read_register(&handle, PCA9685_LED0_OFF_H, &off_h), + ESP_OK); TEST_ASSERT_EQUAL(0, on_l); uint16_t on = (on_h << 8) | on_l; uint16_t off = (off_h << 8) | off_l; @@ -90,27 +93,37 @@ TEST(PCA9685, PCA9685_Digital_Write) { TEST_ASSERT_EQUAL(pca9685_init(&handle, &config), ESP_OK); // Set channel high - TEST_ASSERT_EQUAL(pca9685_digital_write(&handle, PCA9685_CHANNEL1, true), ESP_OK); + TEST_ASSERT_EQUAL(pca9685_digital_write(&handle, PCA9685_CHANNEL1, true), + ESP_OK); // Read back the registers to verify uint8_t on_l, on_h, off_l, off_h; - TEST_ASSERT_EQUAL(pca9685_read_register(&handle, PCA9685_LED1_ON_L, &on_l), ESP_OK); - TEST_ASSERT_EQUAL(pca9685_read_register(&handle, PCA9685_LED1_ON_H, &on_h), ESP_OK); - TEST_ASSERT_EQUAL(pca9685_read_register(&handle, PCA9685_LED1_OFF_L, &off_l), ESP_OK); - TEST_ASSERT_EQUAL(pca9685_read_register(&handle, PCA9685_LED1_OFF_H, &off_h), ESP_OK); + TEST_ASSERT_EQUAL(pca9685_read_register(&handle, PCA9685_LED1_ON_L, &on_l), + ESP_OK); + TEST_ASSERT_EQUAL(pca9685_read_register(&handle, PCA9685_LED1_ON_H, &on_h), + ESP_OK); + TEST_ASSERT_EQUAL(pca9685_read_register(&handle, PCA9685_LED1_OFF_L, &off_l), + ESP_OK); + TEST_ASSERT_EQUAL(pca9685_read_register(&handle, PCA9685_LED1_OFF_H, &off_h), + ESP_OK); uint16_t on = (on_h << 8) | on_l; uint16_t off = (off_h << 8) | off_l; TEST_ASSERT_EQUAL(PCA9685_PWM_MAX, on); TEST_ASSERT_EQUAL(0, off); // Set channel low - TEST_ASSERT_EQUAL(pca9685_digital_write(&handle, PCA9685_CHANNEL1, false), ESP_OK); + TEST_ASSERT_EQUAL(pca9685_digital_write(&handle, PCA9685_CHANNEL1, false), + ESP_OK); // Read back the registers to verify - TEST_ASSERT_EQUAL(pca9685_read_register(&handle, PCA9685_LED1_ON_L, &on_l), ESP_OK); - TEST_ASSERT_EQUAL(pca9685_read_register(&handle, PCA9685_LED1_ON_H, &on_h), ESP_OK); - TEST_ASSERT_EQUAL(pca9685_read_register(&handle, PCA9685_LED1_OFF_L, &off_l), ESP_OK); - TEST_ASSERT_EQUAL(pca9685_read_register(&handle, PCA9685_LED1_OFF_H, &off_h), ESP_OK); + TEST_ASSERT_EQUAL(pca9685_read_register(&handle, PCA9685_LED1_ON_L, &on_l), + ESP_OK); + TEST_ASSERT_EQUAL(pca9685_read_register(&handle, PCA9685_LED1_ON_H, &on_h), + ESP_OK); + TEST_ASSERT_EQUAL(pca9685_read_register(&handle, PCA9685_LED1_OFF_L, &off_l), + ESP_OK); + TEST_ASSERT_EQUAL(pca9685_read_register(&handle, PCA9685_LED1_OFF_H, &off_h), + ESP_OK); on = (on_h << 8) | on_l; off = (off_h << 8) | off_l; TEST_ASSERT_EQUAL(0, on); diff --git a/components/signal_bus/CMakeLists.txt b/components/signal_bus/CMakeLists.txt new file mode 100644 index 0000000..49e88e9 --- /dev/null +++ b/components/signal_bus/CMakeLists.txt @@ -0,0 +1,4 @@ +idf_component_register( + SRCS "signal_bus.c" + INCLUDE_DIRS "include" +) \ No newline at end of file diff --git a/components/signal_bus/include/signal_bus.h b/components/signal_bus/include/signal_bus.h new file mode 100644 index 0000000..9520ffa --- /dev/null +++ b/components/signal_bus/include/signal_bus.h @@ -0,0 +1,46 @@ +#ifndef _SIGNAL_BUS_H +#define _SIGNAL_BUS_H + +#include "esp_err.h" +#include "freertos/FreeRTOS.h" +#include "freertos/event_groups.h" + +// Motor event bits +#define CURRENT_0 (1 << 0) +#define CURRENT_1 (1 << 1) +#define CURRENT_2 (1 << 2) +#define CURRENT_3 (1 << 3) +#define CURRENT_4 (1 << 4) +#define CURRENT_5 (1 << 5) +#define CURRENT_6 (1 << 6) + +#define LIMIT_0 (1 << 7) +#define LIMIT_1 (1 << 8) +#define LIMIT_2 (1 << 9) +#define LIMIT_3 (1 << 10) +#define LIMIT_4 (1 << 11) +#define LIMIT_5 (1 << 12) +#define LIMIT_6 (1 << 13) + +#define HOMING_FLAG (1 << 14) + +#define CURRENT_ANY (0x007F) // bits 0-6 +#define LIMIT_ANY (0x3F80) // bits 7-13 +#define HOME_EVENT (CURRENT_ANY | LIMIT_ANY) +#define EVENT_ANY (HOME_EVENT | HOMING_FLAG) + +extern EventGroupHandle_t g_motor_events; + +/** + * @brief Initialize the signal bus + * + * This function initializes the signal bus and sets up the necessary resources. + * + * @return + * - ESP_OK: Success + * - ESP_ERR_INVALID_ARG: Invalid argument + * - ESP_FAIL: Failed to initialize signal bus + */ +esp_err_t signal_bus_init(); + +#endif // _SIGNAL_BUS_H \ No newline at end of file diff --git a/components/signal_bus/signal_bus.c b/components/signal_bus/signal_bus.c new file mode 100644 index 0000000..f4b234b --- /dev/null +++ b/components/signal_bus/signal_bus.c @@ -0,0 +1,19 @@ +#include "signal_bus.h" + +#include "esp_log.h" + +#define TAG "signal_bus" + +EventGroupHandle_t g_motor_events = NULL; + +esp_err_t signal_bus_init(void) { + ESP_LOGI(TAG, "Initializing signal bus..."); + + g_motor_events = xEventGroupCreate(); + + if (g_motor_events == NULL) { + return ESP_FAIL; + } else { + return ESP_OK; + } +} diff --git a/main/CMakeLists.txt b/main/CMakeLists.txt index b3d7aba..ae02928 100644 --- a/main/CMakeLists.txt +++ b/main/CMakeLists.txt @@ -1,2 +1,2 @@ idf_component_register(SRCS "esp_driver.c" - INCLUDE_DIRS "." PRIV_REQUIRES i2c_bus motorhat driver_socket driver_wifi ads1015 encoder limit_switch) + INCLUDE_DIRS "." PRIV_REQUIRES i2c_bus motorhat driver_socket driver_wifi ads1015 encoder limit_switch signal_bus) diff --git a/main/Kconfig.projbuild b/main/Kconfig.projbuild index 625f71a..7e2c5f4 100644 --- a/main/Kconfig.projbuild +++ b/main/Kconfig.projbuild @@ -1,13 +1,13 @@ menu "Talos ESP Driver Configuration" config DRIVER_MOTORHAT_SDA_PIN int "MotorHAT SDA Pin" - default 16 + default 21 help GPIO pin number for MotorHAT SDA line. config DRIVER_MOTORHAT_SCL_PIN int "MotorHAT SCL Pin" - default 17 + default 22 help GPIO pin number for MotorHAT SCL line. @@ -23,33 +23,21 @@ menu "Talos ESP Driver Configuration" help I2C address for the MotorHAT device. - config DRIVER_ADS1015_SDA_PIN - int "ADS1015 SDA Pin" - default 18 - help - GPIO pin number for the ADS1015 SDA line. - - config DRIVER_ADS1015_SCL_PIN - int "ADS1015 SCL Pin" - default 19 + config DRIVER_MOTORHAT_PAN_SPEED + string "MotorHAT Pan Speed" + default "0.8" help - GPIO pin number for the ADS1015 SCL line. - - config DRIVER_ADS1015_I2C_BUS - int "ADS1015 I2C Bus Number" - default 1 - help - I2C bus number for ADS1015 communication. + The fraction of PCA9685's max PWM speed to use when driving polar pan. config DRIVER_ADS1015_ADDRESS hex "ADS1015 I2C Address" - default 0x48 + default 0x4a help I2C address for the ADS1015 device. config DRIVER_ADS1015_RDY_PIN int "ADS1015 RDY Pin" - default 21 + default 18 help GPIO pin number for the ADS1015 RDY line. @@ -62,6 +50,12 @@ menu "Talos ESP Driver Configuration" 0=128SPS, 1=250SPS, 2=490SPS, 3=920SPS, 4=1600SPS, 5=2400SPS, 6=3300SPS + config DRIVER_LIMIT_SWITCH_PIN + int "Limit Switch Pin" + default 23 + help + GPIO pin for limit switch alerts. + config DRIVER_WIFI_SSID string "WiFi SSID" default "YourSSID" @@ -86,10 +80,4 @@ menu "Talos ESP Driver Configuration" help Listening port for the server. - config DRIVER_LIMIT_SWITCH_PIN - int "Limit Switch Pin" - default 22 - help - GPIO pin for limit switch alerts. - endmenu diff --git a/main/esp_driver.c b/main/esp_driver.c index c5d2122..88efafe 100644 --- a/main/esp_driver.c +++ b/main/esp_driver.c @@ -1,26 +1,45 @@ +#include "ads1015.h" +#include "driver/gpio.h" #include "driver_socket.h" +#include "driver_socket_api.h" #include "driver_wifi.h" +#include "encoder.h" #include "esp_err.h" #include "esp_log.h" #include "freertos/FreeRTOS.h" #include "i2c_bus.h" - -#include "ads1015.h" +#include "limit_switch.h" #include "motorhat.h" #include "nvs_flash.h" -#include "limit_switch.h" +#include "pca9685.h" +#include "signal_bus.h" #define TAG "MAIN" -#include "encoder.h" -#define ENCODER_GLITCH_FILTER 100 //CONFIG_ENCODER_GLITCH_FILTER +#define ENCODER_GLITCH_FILTER 100 // CONFIG_ENCODER_GLITCH_FILTER #define ENCODER_STANDARD_RESOLUTION 20 +#define POLAR_PAN_SPEED + +static const driver_socket_api_motor_interface_t motor_interface = { + .polar_pan = motorhat_polar_pan, + .polar_pan_start = motorhat_polar_pan_start, + .polar_pan_stop = motorhat_polar_pan_stop, + .home = motorhat_home, +}; + +typedef struct { + encoder_handle_t* encoders; + int count; +} encoder_array_t; + +void clear_all_encoders(void* ctx) { + encoder_array_t* arr = (encoder_array_t*)ctx; + for (int i = 0; i < arr->count; i++) { + encoder_clear_count(&arr->encoders[i]); + } +} void app_main(void) { - - // Install GPIO interrupt service - ESP_ERROR_CHECK(gpio_install_isr_service(0)); - // Initialize NVS esp_err_t ret = nvs_flash_init(); if (ret == ESP_ERR_NVS_NO_FREE_PAGES || @@ -28,14 +47,15 @@ void app_main(void) { ESP_ERROR_CHECK(nvs_flash_erase()); ret = nvs_flash_init(); } - ESP_ERROR_CHECK(ret); - limit_switch_config_t limit_switch_config = { - .limit_gpio = CONFIG_DRIVER_LIMIT_SWITCH_PIN, - }; - ESP_ERROR_CHECK(limit_switch_init(&limit_switch_config)); + // Install GPIO interrupt service + ESP_ERROR_CHECK(gpio_install_isr_service(0)); + + // Initialize signal bus + ESP_ERROR_CHECK(signal_bus_init()); + // Initialize I2C bus i2c_bus_t bus; i2c_bus_config_t bus_config = { .port = I2C_NUM_0, @@ -44,26 +64,52 @@ void app_main(void) { }; ESP_ERROR_CHECK(i2c_bus_init(&bus, &bus_config)); - i2c_bus_t adc_bus; - i2c_bus_config_t adc_bus_config = { - .port = I2C_NUM_1, - .sda_io_num = CONFIG_DRIVER_ADS1015_SDA_PIN, - .scl_io_num = CONFIG_DRIVER_ADS1015_SCL_PIN, + // Initialize limit switches + limit_switch_config_t limit_switch_config = { + .limit_gpio = CONFIG_DRIVER_LIMIT_SWITCH_PIN, }; - ESP_ERROR_CHECK(i2c_bus_init(&adc_bus, &adc_bus_config)); + ESP_ERROR_CHECK(limit_switch_init(&limit_switch_config)); + // Initialize current sensing ads1015_handle_t ads; ads1015_config_t ads_config = { .i2c_addr = CONFIG_DRIVER_ADS1015_ADDRESS, .i2c_speed_hz = 400000, .rdy_gpio = CONFIG_DRIVER_ADS1015_RDY_PIN, - .bus_handle = adc_bus.handle, + .bus_handle = bus.handle, .adc_data_rate = CONFIG_DRIVER_ADS1015_DATA_RATE, }; ESP_ERROR_CHECK(ads1015_init(&ads, &ads_config)); - motorhat_handle_t motorhat; + // Initialize encoders + encoder_handle_t encoders[2]; + encoder_config_t encoder_axis1_config = { + .P0_pin = 36, + .P1_pin = 39, + .resolution = CONFIG_ENCODER_0_RESOLUTION, + .glitch_filter_ns = CONFIG_ENCODER_GLITCH_FILTER, + .invert_angle = CONFIG_ENCODER_0_ANGLE_INVERT, + }; + ESP_ERROR_CHECK(encoder_init(&encoders[0], &encoder_axis1_config)); + ESP_ERROR_CHECK(encoder_start(&encoders[0])); + + encoder_config_t encoder_axis2_config = { + .P0_pin = 34, + .P1_pin = 35, + .resolution = CONFIG_ENCODER_0_RESOLUTION, + .glitch_filter_ns = CONFIG_ENCODER_GLITCH_FILTER, + .invert_angle = CONFIG_ENCODER_0_ANGLE_INVERT, + }; + ESP_ERROR_CHECK(encoder_init(&encoders[1], &encoder_axis2_config)); + ESP_ERROR_CHECK(encoder_start(&encoders[1])); + encoder_array_t encoder_array = { + .encoders = encoders, + .count = 2, +}; + + // Initialize motor controller + motorhat_handle_t motorhat; motorhat_config_t motorhat_config = { .pca9685_config = { @@ -72,9 +118,15 @@ void app_main(void) { .pwm_freq_hz = DEFAULT_FREQUENCY_HZ, .bus_handle = bus.handle, }, + .polar_pan_speed = + PCA9685_PWM_MAX * atof(CONFIG_DRIVER_MOTORHAT_PAN_SPEED), + .encoder_cb = (motorhat_encoder_cb_t)clear_all_encoders, + .encoder_ctx = &encoder_array, + .limit_gpio = CONFIG_DRIVER_LIMIT_SWITCH_PIN, }; ESP_ERROR_CHECK(motorhat_init(&motorhat, &motorhat_config)); + // Initialize Wi-Fi and socket connection driver_wifi_config_t wifi_config = { .ssid = CONFIG_DRIVER_WIFI_SSID, .password = CONFIG_DRIVER_WIFI_PASSWORD, @@ -90,98 +142,30 @@ void app_main(void) { .port = CONFIG_DRIVER_SERVER_PORT, }; - ESP_ERROR_CHECK(driver_socket_init(&socket_handle, &socket_config)); - encoder_handle_t enc_axis1 ; - encoder_config_t enc_axis1_cfg = { - .P0_pin = 16, - .P1_pin = 17, - .glitch_filter_ns = ENCODER_GLITCH_FILTER, - .gear_ratio = 635.5, //120:24 gears, 127.1:1 motor - .limb_default = 0, - .resolution = ENCODER_STANDARD_RESOLUTION, - .invert_angle = 0 - }; - ESP_ERROR_CHECK(encoder_init(&enc_axis1, &enc_axis1_cfg)); - - encoder_handle_t enc_axis2 ; - encoder_config_t enc_axis2_cfg = { - .P0_pin = 34, - .P1_pin = 35, - .glitch_filter_ns = ENCODER_GLITCH_FILTER, - .gear_ratio = 457.56, //72:20 gears, 127.1:1 motor - .limb_default = 0, - .resolution = ENCODER_STANDARD_RESOLUTION, - .invert_angle = 0 - }; - ESP_ERROR_CHECK(encoder_init(&enc_axis2, &enc_axis2_cfg)); - - encoder_handle_t enc_axis3 ; - encoder_config_t enc_axis3_cfg = { - .P0_pin = 32, - .P1_pin = 33, - .glitch_filter_ns = ENCODER_GLITCH_FILTER, - .gear_ratio = 457.65, //72:20 gears, 127.1:1 motor - .limb_default = 0, - .resolution = ENCODER_STANDARD_RESOLUTION, - .invert_angle = 0 - }; - ESP_ERROR_CHECK(encoder_init(&enc_axis3, &enc_axis3_cfg)); - - encoder_handle_t enc_axis4 ; - encoder_config_t enc_axis4_cfg = { - .P0_pin = 25, - .P1_pin = 26, - .glitch_filter_ns = ENCODER_GLITCH_FILTER, - .gear_ratio = 125.541, //23:12 gears, 65.5:1 motor - .limb_default = 0, - .resolution = ENCODER_STANDARD_RESOLUTION, - .invert_angle = 0 - }; - ESP_ERROR_CHECK(encoder_init(&enc_axis4, &enc_axis4_cfg)); - - encoder_handle_t enc_axis5 ; - encoder_config_t enc_axis5_cfg = { - .P0_pin = 27, - .P1_pin = 14, - .glitch_filter_ns = ENCODER_GLITCH_FILTER, - .gear_ratio = 125.541, //23:12 gears, 65.5:1 motor - .limb_default = 0, - .resolution = ENCODER_STANDARD_RESOLUTION, - .invert_angle = 0 - }; - ESP_ERROR_CHECK(encoder_init(&enc_axis5, &enc_axis5_cfg)); - - // // Gripper is commented out because our specific application of this robot has no need of the gripper motor. - // // If in the future it is, make sure to double check the schematic reference for the pins and motor ratio. - // encoder_handle_t enc_gripper ; - // encoder_config_t enc_gripper_cfg = { - // .P0_pin = 13, - // .P1_pin = 4, - // .glitch_filter_ns = ENCODER_GLITCH_FILTER, - // .gear_ratio = 19.5, - // .limb_default = 0, - // .resolution = ENCODER_STANDARD_RESOLUTION, - // .invert_angle = 0 - // }; - // ESP_ERROR_CHECK(encoder_init(&enc_gripper, &enc_gripper_cfg)); - - ESP_ERROR_CHECK(encoder_start(&enc_axis1)); - ESP_ERROR_CHECK(encoder_start(&enc_axis2)); - // ESP_ERROR_CHECK(encoder_start(&enc_axis3)); - // ESP_ERROR_CHECK(encoder_start(&enc_axis4)); - // ESP_ERROR_CHECK(encoder_start(&enc_axis5)); - - - - motorhat_set_motor_direction(&motorhat, MOTORHAT_MOTOR1, - MOTORHAT_DIRECTION_FORWARD); - motorhat_set_motor_speed(&motorhat, MOTORHAT_MOTOR1, 200); - - vTaskDelay(1000 / portTICK_PERIOD_MS); - motorhat_set_motor_speed(&motorhat, MOTORHAT_MOTOR1, - MOTORHAT_DIRECTION_RELEASE); + ESP_ERROR_CHECK( + driver_socket_init(&socket_handle, &socket_config, &motor_interface)); + + + // Track encoder values + int axis1_count; + int axis2_count; while (1) { - vTaskDelay(pdMS_TO_TICKS(10000)); - } + esp_err_t err = encoder_get_raw_count(&encoders[0], &axis1_count); + if (err != ESP_OK) { + ESP_LOGE(TAG, "Failed to get encoder count: %s", esp_err_to_name(err)); + } + err = encoder_get_raw_count(&encoders[1], &axis2_count); + if (err != ESP_OK) { + ESP_LOGE(TAG, "Failed to get encoder count: %s", esp_err_to_name(err)); + } + ESP_LOGI(TAG, "Axis 1 count: %i Axis 2 count: %i", axis1_count, axis2_count); + + vTaskDelay(pdMS_TO_TICKS(500)); // Avoid busy loop + + // This loop can also be used to add periodic tasks like reading sensors, or + // checking limit switches. However, it must not terminate or structs + // initialized here will disappear from stack memory, breaking modules that + // use them. + } } diff --git a/test/main/esp_driver_test.c b/test/main/esp_driver_test.c index 17aa69a..fd5aa6c 100644 --- a/test/main/esp_driver_test.c +++ b/test/main/esp_driver_test.c @@ -1,32 +1,27 @@ +#include "driver/gpio.h" +#include "test_menu.h" #include "unity.h" #include "unity_fixture.h" -#include "test_menu.h" -#include "driver/gpio.h" - // NOTE: Define Groups here, and they will be automatically added to the menu #define TEST_GROUPS \ - X(I2C_Bus) \ - X(PCA9685) \ - X(MotorHAT) \ - X(ENCODER) \ - X(ADS1015) \ - X(Driver_WiFi) \ - + X(I2C_Bus) \ + X(PCA9685) \ + X(MotorHAT) \ + X(ENCODER) \ + X(ADS1015) \ + X(Driver_WiFi) -#define X(g) static void run_##g(void) { RUN_TEST_GROUP(g); } +#define X(g) \ + static void run_##g(void) { RUN_TEST_GROUP(g); } TEST_GROUPS #undef X - -#define X(g) { #g, run_##g }, -static const test_group_t groups[] = { - TEST_GROUPS -}; +#define X(g) {#g, run_##g}, +static const test_group_t groups[] = {TEST_GROUPS}; #undef X -void app_main(void) -{ - ESP_ERROR_CHECK(gpio_install_isr_service(0)); - test_menu_run(groups, sizeof(groups) / sizeof(groups[0])); +void app_main(void) { + ESP_ERROR_CHECK(gpio_install_isr_service(0)); + test_menu_run(groups, sizeof(groups) / sizeof(groups[0])); } diff --git a/test/main/include/test_menu.h b/test/main/include/test_menu.h index 5570b37..743215d 100644 --- a/test/main/include/test_menu.h +++ b/test/main/include/test_menu.h @@ -5,10 +5,10 @@ typedef void (*test_group_runner_fn)(void); typedef struct { - const char *name; - test_group_runner_fn runner; + const char* name; + test_group_runner_fn runner; } test_group_t; -void test_menu_run(const test_group_t *groups, size_t count); +void test_menu_run(const test_group_t* groups, size_t count); #endif /* _TEST_MENU_H_ */ diff --git a/test/main/test_menu.c b/test/main/test_menu.c index c58632f..e65e94f 100644 --- a/test/main/test_menu.c +++ b/test/main/test_menu.c @@ -1,175 +1,165 @@ /* ═══════════════════════════════════════════════════════════════ * test_menu.c — Simple serial test menu * ═══════════════════════════════════════════════════════════════ */ +#include "test_menu.h" + #include #include + #include "unity.h" #include "unity_fixture.h" -#include "test_menu.h" - /* ─────────────────────────────────────────────────────────────── * FRAMEWORK: Internal state * ─────────────────────────────────────────────────────────────── */ -static const test_group_t *s_groups = NULL; -static size_t s_group_count = 0; +static const test_group_t* s_groups = NULL; +static size_t s_group_count = 0; -static int s_last_total = 0; -static int s_last_failed = 0; +static int s_last_total = 0; +static int s_last_failed = 0; static int s_last_ignored = 0; - /* ─────────────────────────────────────────────────────────────── * FRAMEWORK: UART input — echoes chars so cursor feels alive * ─────────────────────────────────────────────────────────────── */ -static int read_line(char *buf, size_t max) -{ - size_t i = 0; +static int read_line(char* buf, size_t max) { + size_t i = 0; - /* Print prompt once before user starts typing */ - printf("> "); - fflush(stdout); + /* Print prompt once before user starts typing */ + printf("> "); + fflush(stdout); - while (i < max - 1) { - int c = getchar(); - if (c == EOF) continue; - - if (c == '\n' || c == '\r') { - break; - } - - /* Handle backspace — erase char from terminal too */ - if (c == 0x08 || c == 0x7F) { - if (i > 0) { - i--; - printf("\b \b"); - fflush(stdout); - } - continue; - } - - buf[i++] = (char)c; - putchar(c); + while (i < max - 1) { + int c = getchar(); + if (c == EOF) continue; + + if (c == '\n' || c == '\r') { + break; + } + + /* Handle backspace — erase char from terminal too */ + if (c == 0x08 || c == 0x7F) { + if (i > 0) { + i--; + printf("\b \b"); fflush(stdout); + } + continue; } - buf[i] = '\0'; - putchar('\n'); - return (int)i; -} + buf[i++] = (char)c; + putchar(c); + fflush(stdout); + } + buf[i] = '\0'; + putchar('\n'); + return (int)i; +} /* ─────────────────────────────────────────────────────────────── * FRAMEWORK: Run helpers * ─────────────────────────────────────────────────────────────── */ -static void capture_results(void) -{ - s_last_total = Unity.NumberOfTests; - s_last_failed = Unity.TestFailures; - s_last_ignored = Unity.TestIgnores; +static void capture_results(void) { + s_last_total = Unity.NumberOfTests; + s_last_failed = Unity.TestFailures; + s_last_ignored = Unity.TestIgnores; } -static void run_group(const test_group_t *g) -{ - printf("\n Running: %s\n", g->name); - printf(" --------\n"); - UNITY_BEGIN(); - g->runner(); - UNITY_END(); - capture_results(); +static void run_group(const test_group_t* g) { + printf("\n Running: %s\n", g->name); + printf(" --------\n"); + UNITY_BEGIN(); + g->runner(); + UNITY_END(); + capture_results(); } -static void run_all(void) -{ - printf("\n Running: all groups\n"); - printf(" --------\n"); - UNITY_BEGIN(); - for (size_t i = 0; i < s_group_count; i++) { - printf("\n [ %s ]\n", s_groups[i].name); - s_groups[i].runner(); - } - UNITY_END(); - capture_results(); +static void run_all(void) { + printf("\n Running: all groups\n"); + printf(" --------\n"); + UNITY_BEGIN(); + for (size_t i = 0; i < s_group_count; i++) { + printf("\n [ %s ]\n", s_groups[i].name); + s_groups[i].runner(); + } + UNITY_END(); + capture_results(); } - /* ─────────────────────────────────────────────────────────────── * FRAMEWORK: Menu rendering * ─────────────────────────────────────────────────────────────── */ -static void print_menu(void) -{ - printf("\n"); - printf(" --------------------------\n"); - printf(" Test Menu\n"); - printf(" --------------------------\n"); - printf(" a run all\n"); - printf(" r last results\n"); - printf(" q quit\n"); - printf(" --------------------------\n"); - - for (size_t i = 0; i < s_group_count; i++) { - printf(" %-2zu %s\n", i, s_groups[i].name); - } - - printf(" --------------------------\n"); +static void print_menu(void) { + printf("\n"); + printf(" --------------------------\n"); + printf(" Test Menu\n"); + printf(" --------------------------\n"); + printf(" a run all\n"); + printf(" r last results\n"); + printf(" q quit\n"); + printf(" --------------------------\n"); + + for (size_t i = 0; i < s_group_count; i++) { + printf(" %-2zu %s\n", i, s_groups[i].name); + } + + printf(" --------------------------\n"); } -static void print_last_results(void) -{ - printf("\n"); - if (s_last_total == 0) { - printf(" No tests run yet.\n\n"); - return; - } - - int passed = s_last_total - s_last_failed - s_last_ignored; - - printf(" --------------------------\n"); - printf(" Last results\n"); - printf(" --------------------------\n"); - printf(" Total %d\n", s_last_total); - printf(" Passed %d\n", passed); - printf(" Failed %d\n", s_last_failed); - printf(" Ignored %d\n", s_last_ignored); - printf(" --------------------------\n\n"); +static void print_last_results(void) { + printf("\n"); + if (s_last_total == 0) { + printf(" No tests run yet.\n\n"); + return; + } + + int passed = s_last_total - s_last_failed - s_last_ignored; + + printf(" --------------------------\n"); + printf(" Last results\n"); + printf(" --------------------------\n"); + printf(" Total %d\n", s_last_total); + printf(" Passed %d\n", passed); + printf(" Failed %d\n", s_last_failed); + printf(" Ignored %d\n", s_last_ignored); + printf(" --------------------------\n\n"); } - /* ─────────────────────────────────────────────────────────────── * USER: Call this from app_main — pass your groups array * ─────────────────────────────────────────────────────────────── */ -void test_menu_run(const test_group_t *groups, size_t count) -{ - s_groups = groups; - s_group_count = count; - - char buf[16]; - - print_menu(); - - while (1) { - read_line(buf, sizeof(buf)); - - if (strcmp(buf, "a") == 0) { - run_all(); - print_menu(); - - } else if (strcmp(buf, "r") == 0) { - print_last_results(); - - } else if (strcmp(buf, "q") == 0) { - printf("\n bye.\n\n"); - break; - - } else { - char *end; - long idx = strtol(buf, &end, 10); - if (end != buf && idx >= 0 && idx < (long)s_group_count) { - run_group(&s_groups[idx]); - print_menu(); - } else if (strlen(buf) > 0) { - printf(" unknown command.\n"); - } - } +void test_menu_run(const test_group_t* groups, size_t count) { + s_groups = groups; + s_group_count = count; + + char buf[16]; + + print_menu(); + + while (1) { + read_line(buf, sizeof(buf)); + + if (strcmp(buf, "a") == 0) { + run_all(); + print_menu(); + + } else if (strcmp(buf, "r") == 0) { + print_last_results(); + + } else if (strcmp(buf, "q") == 0) { + printf("\n bye.\n\n"); + break; + + } else { + char* end; + long idx = strtol(buf, &end, 10); + if (end != buf && idx >= 0 && idx < (long)s_group_count) { + run_group(&s_groups[idx]); + print_menu(); + } else if (strlen(buf) > 0) { + printf(" unknown command.\n"); + } } + } }