inital code for adc

This commit is contained in:
Laila van Reenen 2024-08-14 15:29:57 +02:00
parent 80ffc67790
commit 4f47712d27
Signed by: LailaTheElf
GPG Key ID: 1F4E6EE3E6DDF769
5 changed files with 200 additions and 10 deletions

View File

@ -12,6 +12,7 @@ idf_component_register(
./servos.c
./logger.c
./ws.c
./adc.c
INCLUDE_DIRS "./"
PRIV_REQUIRES
# project components
@ -19,6 +20,6 @@ idf_component_register(
# idf extra components
led_strip
# idf base components
esp_websocket_client
esp_websocket_client esp_adc
spi_flash driver nvs_flash esp_wifi
)

78
rx_esp32/src/adc.c Normal file
View File

@ -0,0 +1,78 @@
#include <stdint.h>
#include <stdbool.h>
#include <esp_adc/adc_continuous.h>
#include "adc.h"
#include "hal/adc_types.h"
adc_digi_output_data_t ADC_Buffer[6][10];
uint8_t ADC_conf_available = 0;
adc_continuous_handle_t adc_handle;
static bool IRAM_ATTR s_conv_done_cb(adc_continuous_handle_t handle, const adc_continuous_evt_data_t *edata, void *user_data)
{
// BaseType_t mustYield = pdFALSE;
// //Notify that ADC continuous driver has done enough number of conversions
// vTaskNotifyGiveFromISR(s_task_handle, &mustYield);
// return (mustYield == pdTRUE);
ADC_conf_available++;
return true;
}
void adc_init()
{
adc_continuous_handle_cfg_t adc_config = {
.max_store_buf_size = sizeof(ADC_Buffer),
.conv_frame_size = sizeof(adc_digi_output_data_t) * 6,
};
adc_continuous_new_handle(&adc_config, &adc_handle);
adc_continuous_config_t dig_cfg = {
.sample_freq_hz = 100 * 6,
.conv_mode = ADC_CONV_SINGLE_UNIT_1,
.format = ADC_DIGI_OUTPUT_FORMAT_TYPE2,
};
adc_digi_pattern_config_t adc_pattern[SOC_ADC_PATT_LEN_MAX] = {0};
dig_cfg.pattern_num = 6;
for (int i = 0; i < 6; i++) {
adc_pattern[i].atten = ADC_ATTEN_DB_12;
adc_pattern[i].unit = ADC_UNIT_1;
adc_pattern[i].bit_width = ADC_BITWIDTH_12;
}
adc_pattern[0].channel = ADC_CHANNEL_0 & 0x7;
adc_pattern[1].channel = ADC_CHANNEL_1 & 0x7;
adc_pattern[2].channel = ADC_CHANNEL_2 & 0x7;
adc_pattern[3].channel = ADC_CHANNEL_3 & 0x7;
adc_pattern[4].channel = ADC_CHANNEL_4 & 0x7;
adc_pattern[5].channel = ADC_CHANNEL_5 & 0x7;
dig_cfg.adc_pattern = adc_pattern;
adc_continuous_config(adc_handle, &dig_cfg);
adc_continuous_evt_cbs_t cbs = {
.on_conv_done = s_conv_done_cb,
};
adc_continuous_register_event_callbacks(adc_handle, &cbs, NULL);
adc_continuous_start(adc_handle);
}
bool adc_read(ADC_Data_t* out_data)
{
adc_digi_output_data_t data[6];
uint32_t ret_num = 0;
esp_err_t ret = adc_continuous_read(
adc_handle,
(uint8_t*)&data,
sizeof(adc_digi_output_data_t) * 6,
&ret_num,
0
);
if (ret == ESP_OK)
{
for (int i=0; i<6; i++) {
out_data->value[i] = data[i].type2.data;
}
}
return (ret == ESP_OK);
}

14
rx_esp32/src/adc.h Normal file
View File

@ -0,0 +1,14 @@
#ifndef ADC_H
#define ADC_H
#include <stdint.h>
#include <stdbool.h>
typedef struct {
uint16_t value[6];
} ADC_Data_t;
void adc_init();
bool adc_read(ADC_Data_t* data);
#endif // ADC_H

View File

@ -3,7 +3,9 @@
#include <stdint.h>
#define TARGET_DEV "{{ target }}"
#define TARGET_TX 0
#define TARGET_RX 1
#define TARGET {% if target == "tx" %}TARGET_TX{% else %}TARGET_RX{% endif %}
#define WIFI_SSID "MBCBootjes"
#define WIFI_AUTH WIFI_AUTH_WPA_WPA2_PSK

View File

@ -18,6 +18,9 @@
#include "servos.h"
#include "logger.h"
#include "ws.h"
#if TARGET == TARGET_TX
#include "adc.h"
#endif
bool volatile running = true;
CMDList_t* cmdList;
@ -27,14 +30,33 @@ bool rxBuffer_overflow = false;
typedef enum {
STATE_WIFI_CONNECTING,
STATE_WIFI_WAIT_CONNECTION,
#if TARGET == TARGET_RX
STATE_WS_CONNECTING,
STATE_WS_WAIT_CONNECTION,
STATE_IDEL,
STATE_DRIVING
#elif TARGET == TARGET_TX
STATE_WIFI_CONNECTED,
#endif
} MainState_t;
MainState_t MainState = STATE_WIFI_CONNECTING;
#if TARGET == TARGET_TX
typedef enum {
STATE_WS_CONNECTING,
STATE_WS_WAIT_CONNECTION,
STATE_IDEL,
STATE_DRIVING
} WSState_t;
WSState_t WSStates[3] = {STATE_WS_CONNECTING, STATE_WS_CONNECTING, STATE_WS_CONNECTING};
uint8_t WS_conn_proc = 0;
#endif
#if TARGET == TARGET_RX
ws_client_t ws_client;
#elif TARGET == TARGET_TX
ws_client_t ws_client[3];
#endif
int charOut_uart(const char* c)
{
@ -42,23 +64,40 @@ int charOut_uart(const char* c)
return 0;
}
#if TARGET == TARGET_RX
int charOut_ws(const char* c)
{
ws_putchar(ws_client, *c);
return 0;
}
#elif TARGET == TARGET_TX
int charOut_ws0(const char* c)
{
ws_putchar(ws_client[0], *c);
return 0;
}
int charOut_ws1(const char* c)
{
ws_putchar(ws_client[1], *c);
return 0;
}
int charOut_ws2(const char* c)
{
ws_putchar(ws_client[2], *c);
return 0;
}
CLI_charOutFn charOut_ws[3] = {
&charOut_ws0,
&charOut_ws1,
&charOut_ws2
}
#endif
void app_main() {
// disable watchdog
ESP_ERROR_CHECK(esp_task_wdt_deinit());
// wait so I have time to open the serial monitor
// for (unsigned long i=0; i < 10000; i++)
// {
// printf(".");
// }
// printf("\n");
/* Initialize NVS — it is used to store PHY calibration data */
esp_err_t ret = nvs_flash_init();
if (ret == ESP_ERR_NVS_NO_FREE_PAGES || ret == ESP_ERR_NVS_NEW_VERSION_FOUND)
@ -70,11 +109,16 @@ void app_main() {
printChipInfo();
#if TARGET == TARGET_TX
led_init();
led_setRGB(0, 0, 20);
#endif
servo_init();
wifiInit();
#if TARGET == TARGET_TX
adc_init();
#endif
running = true;
@ -85,7 +129,12 @@ void app_main() {
uint8_t charIn = 0;
EventBits_t bits;
MainState_t lastMainState = MainState;
#if TARGET == TARGET_RX
CLI_t cli_ws_client;
#elif TARGET == TARGET_TX
CLI_t cli_ws_client[3];
#endif
LOG_D("main: main loop starting in state %d", MainState);
while (running)
{
@ -111,27 +160,71 @@ void app_main() {
bits = xEventGroupGetBits(s_wifi_event_group);
if ((bits & WIFI_CONNECTED_BIT) != 0)
{
#if TARGET == TARGET_TX
led_setRGB(0, 20, 0);
#endif
MainState = STATE_WS_CONNECTING;
LOG_I("main: connected to ap SSID '%s'", WIFI_SSID);
}
else if ((bits & WIFI_FAIL_BIT) != 0)
{
#if TARGET == TARGET_TX
led_setRGB(2, 0, 0);
#endif
MainState = STATE_WIFI_CONNECTING;
// wifi_reconnect();
LOG_E("main: Failed to connect to SSID '%s'", WIFI_SSID);
}
else if (bits != 0)
{
#if TARGET == TARGET_TX
led_setRGB(2, 0, 0);
#endif
// MainState = STATE_WIFI_CONNECTING;
LOG_C("main: UNEXPECTED EVENT (bits: 0x%04x)", bits);
}
break;
#if TARGET == TARGET_TX
case STATE_WIFI_CONNECTED:
switch (WSStates[WS_conn_proc])
{
case STATE_WS_CONNECTING:
ws_client[WS_conn_proc] = ws_connect(WS_URL);
WSStates[WS_conn_proc] = STATE_WS_WAIT_CONNECTION;
break;
case STATE_WS_WAIT_CONNECTION:
if (ws_client[WS_conn_proc]->connected)
{
cli_ws_client[WS_conn_proc] = CLI_init((CLI_charOutFn)&charOut_ws[WS_conn_proc], cmdList);
cli_ws_client[WS_conn_proc].echo = false;
MainState = STATE_IDEL;
}
break;
case STATE_IDEL:
case STATE_DRIVING:
if (!ws_client[WS_conn_proc]->connected)
{
CLI_deinit(&cli_ws_client[WS_conn_proc]);
MainState = STATE_WS_WAIT_CONNECTION;
}
while ((charIn = ws_getchar(ws_client[WS_conn_proc])) != 255)
{
CLI_charIn(&cli_ws_client[WS_conn_proc], charIn);
}
ws_sendData(ws_client[WS_conn_proc]);
break;
}
ADC_Data_t data;
if (adc_read(&data))
{
LOG_D("ADC_DATA: %d, %d, %d, %d, %d, %d", data.value[0], data.value[1], data.value[2], data.value[3], data.value[4], data.value[5]);
}
break;
#elif TARGET == TARGET_RX
case STATE_WS_CONNECTING:
ws_client = ws_connect(WS_URL);
MainState = STATE_WS_WAIT_CONNECTION;
@ -145,7 +238,6 @@ void app_main() {
}
break;
case STATE_IDEL:
case STATE_DRIVING:
if (!ws_client->connected)
@ -160,10 +252,13 @@ void app_main() {
}
ws_sendData(ws_client);
break;
#endif
}
}
#if TARGET == TARGET_TX
led_setRGB(0, 0, 0);
#endif
servo_deinit();
CLI_deinit(&cli_uart);