Finish display impl, write UI widget, refactor
This commit is contained in:
maddiebaka
+206
@@ -0,0 +1,206 @@
|
||||
#include "motor.h"
|
||||
|
||||
#include "driver/spi_common.h"
|
||||
#include "driver/rmt_tx.h"
|
||||
#include "driver/uart.h"
|
||||
#include "esp_log.h"
|
||||
|
||||
#include "dshot_esc_encoder.h"
|
||||
|
||||
#define GPIO_ESC_CTRL CONFIG_ESC_CTRL_PIN
|
||||
#define GPIO_ESC_RX CONFIG_TELEMETRY_RX_PIN
|
||||
#define ESP_INTR_FLAG_DEFAULT 0
|
||||
|
||||
static const char *TAG = "spincoat-plater-firmware/motor";
|
||||
|
||||
static QueueHandle_t uart_queue = NULL;
|
||||
const int uart_buffer_size = (1024 * 2);
|
||||
|
||||
rmt_encoder_handle_t dshot_encoder = NULL;
|
||||
rmt_channel_handle_t esc_chan = NULL;
|
||||
|
||||
rmt_transmit_config_t tx_config = {
|
||||
.loop_count = 0,
|
||||
};
|
||||
|
||||
dshot_esc_throttle_t throttle = {
|
||||
.throttle = 0,
|
||||
.telemetry_req = false,
|
||||
};
|
||||
|
||||
/**
|
||||
* Sends a telemetry packet at a set, constant interval
|
||||
*/
|
||||
void v_telemetry_packet_func(void *pvParameters) {
|
||||
TickType_t frequency = 10 / portTICK_PERIOD_MS;
|
||||
TickType_t last_wake_time = xTaskGetTickCount();
|
||||
while(1) {
|
||||
throttle.telemetry_req = true;
|
||||
vTaskDelayUntil(&last_wake_time, frequency);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Sends zero throttle to arm ESC for control. Stop/delete this task once the ESC has armed.
|
||||
*/
|
||||
void v_initialize_esc_throttle_func(void *pvParameters) {
|
||||
while(1) {
|
||||
ESP_ERROR_CHECK(rmt_transmit(esc_chan, dshot_encoder, &throttle, sizeof(throttle), &tx_config));
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Starts task *v_initialize_esc_throttle_func()* for a few seconds and then destroys it.
|
||||
* This function takes care of the arming stage of ESC control.
|
||||
*/
|
||||
void initialize_esc_throttle(void) {
|
||||
TaskHandle_t v_init_throttle_handle = NULL;
|
||||
xTaskCreate(&v_initialize_esc_throttle_func, "v_init_throttle_func", 2048, NULL, 5, &v_init_throttle_handle);
|
||||
vTaskDelay(pdMS_TO_TICKS(5000));
|
||||
vTaskDelete(v_init_throttle_handle);
|
||||
}
|
||||
|
||||
/**
|
||||
* Initialize the RMT system in preparation for sending DSHOT packets to the connected ESC.
|
||||
*/
|
||||
void init_rmt_esc_tx(void) {
|
||||
ESP_LOGI(TAG, "Create RMT TX channel");
|
||||
rmt_tx_channel_config_t tx_chan_config = {
|
||||
.clk_src = RMT_CLK_SRC_DEFAULT, // select a clock that can provide needed resolution
|
||||
.gpio_num = GPIO_ESC_CTRL,
|
||||
.mem_block_symbols = 64,
|
||||
.resolution_hz = DSHOT_ESC_RESOLUTION_HZ,
|
||||
.trans_queue_depth = 10, // set the number of transactions that can be pending in the background
|
||||
};
|
||||
ESP_ERROR_CHECK(rmt_new_tx_channel(&tx_chan_config, &esc_chan));
|
||||
|
||||
ESP_LOGI(TAG, "Install Dshot ESC encoder");
|
||||
dshot_esc_encoder_config_t encoder_config = {
|
||||
.resolution = DSHOT_ESC_RESOLUTION_HZ,
|
||||
.baud_rate = 300000, // DSHOT300 protocol
|
||||
.post_delay_us = 50, // extra delay between each frame
|
||||
};
|
||||
ESP_ERROR_CHECK(rmt_new_dshot_esc_encoder(&encoder_config, &dshot_encoder));
|
||||
|
||||
ESP_LOGI(TAG, "Enable RMT TX channel");
|
||||
ESP_ERROR_CHECK(rmt_enable(esc_chan));
|
||||
|
||||
ESP_LOGI(TAG, "Start ESC by sending zero throttle for a while...");
|
||||
initialize_esc_throttle();
|
||||
}
|
||||
|
||||
/**
|
||||
* Initialize the UART receive pin so that we can receive telemetry data from the connected ESC.
|
||||
*/
|
||||
void init_telemetry_uart_rx(void) {
|
||||
uart_config_t uart_config = {
|
||||
.baud_rate = 115200,
|
||||
.data_bits = UART_DATA_8_BITS,
|
||||
.parity = UART_PARITY_DISABLE,
|
||||
.stop_bits = UART_STOP_BITS_1,
|
||||
.flow_ctrl = UART_HW_FLOWCTRL_CTS_RTS,
|
||||
.rx_flow_ctrl_thresh = 122,
|
||||
};
|
||||
|
||||
ESP_ERROR_CHECK(uart_driver_install(ESC_UART_NUM, uart_buffer_size, uart_buffer_size, 10, &uart_queue, 0));
|
||||
ESP_ERROR_CHECK(uart_param_config(ESC_UART_NUM, &uart_config));
|
||||
ESP_ERROR_CHECK(uart_set_pin(ESC_UART_NUM, UART_PIN_NO_CHANGE, GPIO_ESC_RX, UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE));
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* Sends a DSHOT packet via the RMT. Make sure the RMT channel has been initialized
|
||||
* by calling *init_rmt_esc_tx()*
|
||||
*/
|
||||
void send_dshot_packet(void) {
|
||||
ESP_ERROR_CHECK(rmt_transmit(esc_chan, dshot_encoder, &throttle, sizeof(throttle), &tx_config));
|
||||
|
||||
if(throttle.telemetry_req == true) {
|
||||
throttle.telemetry_req = false;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Calculate one step of the crc8 and return it
|
||||
*/
|
||||
uint8_t update_crc8(uint8_t crc, uint8_t crc_seed){
|
||||
uint8_t crc_u, i;
|
||||
crc_u = crc;
|
||||
crc_u ^= crc_seed;
|
||||
for ( i=0; i<8; i++) crc_u = ( crc_u & 0x80 ) ? 0x7 ^ ( crc_u << 1 ) : ( crc_u << 1 );
|
||||
return (crc_u);
|
||||
}
|
||||
|
||||
/**
|
||||
* Calculate the entire crc8 for a KISS frame and return it for validation against the
|
||||
* transmitted crc8
|
||||
*/
|
||||
uint8_t get_crc8(uint8_t *Buf, uint8_t BufLen){
|
||||
uint8_t crc = 0, i;
|
||||
for( i=0; i<BufLen; i++) crc = update_crc8(Buf[i], crc);
|
||||
return (crc);
|
||||
}
|
||||
|
||||
/**
|
||||
* Parse the KISS telemetry frame and check the crc8
|
||||
* TODO: Do more with the data than print it
|
||||
*/
|
||||
void parse_telemetry(void) {
|
||||
uint8_t frame_size = 10;
|
||||
uint8_t data[128];
|
||||
// get data
|
||||
uint8_t length = uart_read_bytes(ESC_UART_NUM, data, frame_size, 100);
|
||||
|
||||
uart_flush(ESC_UART_NUM);
|
||||
|
||||
if(length < 10) return;
|
||||
|
||||
// chop out just the payload
|
||||
uint8_t payload[128];
|
||||
uint8_t payload_length = (frame_size - 1);
|
||||
for(uint8_t i = 0; i < payload_length; i++) {
|
||||
payload[i] = data[i];
|
||||
}
|
||||
|
||||
// calculate the crc8
|
||||
uint8_t expected_crc8 = get_crc8(payload, payload_length);
|
||||
uint8_t received_crc8 = (uint8_t) data[frame_size - 1];
|
||||
|
||||
if(expected_crc8 != received_crc8) return;
|
||||
|
||||
for(uint8_t i = 0; i < length; i++) {
|
||||
printf("%d - %d\n", i, data[i]);
|
||||
}
|
||||
printf("--------------------\n");
|
||||
printf("expected: %d\n", expected_crc8);
|
||||
printf("received: %d\n", received_crc8);
|
||||
printf("======================\n");
|
||||
}
|
||||
|
||||
/**
|
||||
* Helper function that sets up the RMT, creates the telemetry task, and
|
||||
* initializes the telemetry UART.
|
||||
*/
|
||||
void init_motor(void) {
|
||||
init_rmt_esc_tx();
|
||||
throttle.throttle = 300;
|
||||
xTaskCreate(&v_telemetry_packet_func, "v_telemetry_packet_func", 2048, NULL, 1, NULL);
|
||||
init_telemetry_uart_rx();
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the current throttle value
|
||||
*/
|
||||
uint16_t get_throttle() {
|
||||
return throttle.throttle;
|
||||
}
|
||||
|
||||
/**
|
||||
* Updates the throttle value.
|
||||
*
|
||||
* \arg \c throttle The new throttle value between 0-2048
|
||||
*/
|
||||
|
||||
void update_throttle(uint16_t thr) {
|
||||
throttle.throttle = thr;
|
||||
}
|
||||
Reference in New Issue
Block a user