amikhalev/uas-ugv
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

move io stuff out of display

Browse Source
This commit is contained in:
Alex Mikhalev 2019-01-24 19:40:17 -08:00
parent 5bfb073e92
commit 9b000427be
6 changed files with 48 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
main/e32_driver.cc
View File

@ -104,12 +104,15 @@ esp_err_t E32_Driver::Free() {
esp_err_t E32_Driver::ReadParams(Params& params) { esp_err_t E32_Driver::ReadParams(Params& params) {
esp_err_t ret; esp_err_t ret;
ret = RawWrite(CMD_READ_PARAMS, sizeof(CMD_READ_PARAMS)); ret = RawWrite(CMD_READ_PARAMS, sizeof(CMD_READ_PARAMS));
ret = WaitWriteDone();
if (ret != ESP_OK) { if (ret != ESP_OK) {
ESP_LOGE(TAG, "error writing read params cmd");
return ret; return ret;
} }
uint8_t param_data[PARAMS_LEN]; uint8_t param_data[PARAMS_LEN];
ret = Read(param_data, PARAMS_LEN, pdMS_TO_TICKS(1000)); ret = Read(param_data, PARAMS_LEN, pdMS_TO_TICKS(1000));
if (ret != ESP_OK) { if (ret != PARAMS_LEN) {
ESP_LOGE(TAG, "error reading params");
return ret; return ret;
} }
@ -241,7 +244,8 @@ esp_err_t E32_Driver::WriteParams(const Params& params) {
param_data[5] |= 3; param_data[5] |= 3;
} }
ret = RawWrite(param_data, PARAMS_LEN); RawWrite(param_data, PARAMS_LEN);
ret = WaitWriteDone();
if (ret != ESP_OK) { if (ret != ESP_OK) {
return ret; return ret;
} }
@ -251,8 +255,8 @@ esp_err_t E32_Driver::WriteParams(const Params& params) {
return ESP_OK; return ESP_OK;
} }
int E32_Driver::Write(Address address, Channel channel, int E32_Driver::Write(Address address, Channel channel, const uint8_t* data,
const uint8_t* data, size_t data_size) { size_t data_size) {
int written; int written;
if (params_.tx_mode == TxFixed) { if (params_.tx_mode == TxFixed) {
uint8_t header[3]; uint8_t header[3];
@ -260,7 +264,8 @@ int E32_Driver::Write(Address address, Channel channel,
header[1] = address & 0xFF; header[1] = address & 0xFF;
header[2] = channel; header[2] = channel;
written = uart_write_bytes(config_.uart_port, (char*)header, sizeof(header)); written =
uart_write_bytes(config_.uart_port, (char*)header, sizeof(header));
if (written < 0) { if (written < 0) {
return written; return written;
} }
@ -272,13 +277,17 @@ int E32_Driver::Write(const uint8_t* data, size_t data_size) {
return Write(params_.address, params_.comm_channel, data, data_size); return Write(params_.address, params_.comm_channel, data, data_size);
} }
int E32_Driver::Write(Address address, Channel channel, int E32_Driver::Write(Address address, Channel channel,
const std::string& data) { const std::string& data) {
return Write(address, channel, (uint8_t*)data.c_str(), data.size()); return Write(address, channel, (uint8_t*)data.c_str(), data.size());
} }
int E32_Driver::Write(const std::string& data) { int E32_Driver::Write(const std::string& data) {
return Write((uint8_t*)data.c_str(), data.size()); return Write((uint8_t*)data.c_str(), data.size());
} }
esp_err_t E32_Driver::WaitWriteDone(TickType_t ticks_to_wait) {
return uart_wait_tx_done(config_.uart_port, ticks_to_wait);
}
int E32_Driver::Read(uint8_t* data, int max_len, TickType_t ticks_to_wait) { int E32_Driver::Read(uint8_t* data, int max_len, TickType_t ticks_to_wait) {
return uart_read_bytes(config_.uart_port, (uint8_t*)data, max_len, return uart_read_bytes(config_.uart_port, (uint8_t*)data, max_len,
ticks_to_wait); ticks_to_wait);
@ -304,8 +313,13 @@ int E32_Driver::Read(std::string& data, TickType_t ticks_to_wait) {
return total_read; return total_read;
} }
esp_err_t E32_Driver::RawWrite(const uint8_t* data, size_t data_size) { int E32_Driver::RawWrite(const uint8_t* data, size_t data_size) {
return uart_write_bytes(config_.uart_port, (const char*)data, data_size); int written =
uart_write_bytes(config_.uart_port, (const char*)data, data_size);
if (written < 0) {
ESP_LOGE(TAG, "RawWrite error: %d", written);
}
return written;
} }
} // namespace e32 } // namespace e32

4
main/e32_driver.hh
View File

@ -67,6 +67,8 @@ class E32_Driver {
int Write(Address address, Channel channel, const std::string& data); int Write(Address address, Channel channel, const std::string& data);
int Write(const std::string& data); int Write(const std::string& data);
esp_err_t WaitWriteDone(TickType_t ticks_to_wait = portMAX_DELAY);
int Read(uint8_t* data, int max_len, int Read(uint8_t* data, int max_len,
TickType_t ticks_to_wait = portMAX_DELAY); TickType_t ticks_to_wait = portMAX_DELAY);
int Read(std::string& data, TickType_t ticks_to_wait = portMAX_DELAY); int Read(std::string& data, TickType_t ticks_to_wait = portMAX_DELAY);
@ -76,7 +78,7 @@ class E32_Driver {
Config config_; Config config_;
Params params_; Params params_;
esp_err_t RawWrite(const uint8_t* data, size_t data_size); int RawWrite(const uint8_t* data, size_t data_size);
}; };
} // namespace e32 } // namespace e32

17
main/ugv_display.cc
View File

@ -4,17 +4,15 @@
#include "U8g2lib.hh" #include "U8g2lib.hh"
#include "ugv_comms.hh" #include "ugv_comms.hh"
#include "ugv_config.h" #include "ugv_config.h"
#include "ugv_io.hh"
namespace ugv { namespace ugv {
static const char* TAG = "ugv_display"; static const char* TAG = "ugv_display";
using comms::CommsClass; using comms::CommsClass;
using io::IOClass;
DisplayClass::DisplayClass(CommsClass* comms, IOClass* io) DisplayClass::DisplayClass(CommsClass* comms)
: comms_(comms), io_(io) {} : comms_(comms) {}
void DisplayClass::Init() { void DisplayClass::Init() {
// For Heltec ESP32 LoRa // For Heltec ESP32 LoRa
@ -39,14 +37,7 @@ void DisplayClass::Run() {
int32_t last_packet_rssi; int32_t last_packet_rssi;
int8_t last_packet_snr; int8_t last_packet_snr;
io::Inputs inputs;
io::Outputs outputs;
while (true) { while (true) {
io_->ReadInputs(inputs);
outputs.left_motor = outputs.left_motor > 0 ? -0.5 : 1.0;
outputs.right_motor = outputs.right_motor > 0 ? -0.5 : 1.0;
io_->WriteOutputs(outputs);
// ESP_LOGI(TAG, "inputs %s", inputs.ToString()); // ESP_LOGI(TAG, "inputs %s", inputs.ToString());
oled->firstPage(); oled->firstPage();
@ -86,8 +77,8 @@ void DisplayClass::Run() {
oled->setCursor(4, 4 * 8); oled->setCursor(4, 4 * 8);
oled->print("no pkt rx"); oled->print("no pkt rx");
} }
oled->setCursor(4, 6 * 8); // oled->setCursor(4, 6 * 8);
oled->printf("motors: (%f, %f)", outputs.left_motor, outputs.right_motor); // oled->printf("motors: (%f, %f)", outputs.left_motor, outputs.right_motor);
} while (oled->nextPage()); } while (oled->nextPage());
vTaskDelay(pdMS_TO_TICKS(1000)); vTaskDelay(pdMS_TO_TICKS(1000));
} }

6
main/ugv_display.hh
View File

@ -10,20 +10,16 @@ namespace ugv {
namespace comms { namespace comms {
class CommsClass; class CommsClass;
} }
namespace io {
class IOClass;
}
class DisplayClass { class DisplayClass {
public: public:
DisplayClass() = delete; DisplayClass() = delete;
DisplayClass(comms::CommsClass *comms, io::IOClass *io); DisplayClass(comms::CommsClass *comms);
void Init(); void Init();
private: private:
comms::CommsClass *comms_; comms::CommsClass *comms_;
io::IOClass *io_;
U8G2 *oled; U8G2 *oled;
TaskHandle_t task_handle_; TaskHandle_t task_handle_;

4
main/ugv_io.cc
View File

@ -71,12 +71,12 @@ void IOClass::ReadInputs(Inputs &inputs) {
void IOClass::WriteOutputs(const Outputs &outputs) { void IOClass::WriteOutputs(const Outputs &outputs) {
esp_err_t ret; esp_err_t ret;
ret = mcpwm_set_duty(MCPWM_UNIT, MCPWM_TIMER_0, MCPWM_OPR_A, ret = mcpwm_set_duty(MCPWM_UNIT, MCPWM_TIMER_0, MCPWM_OPR_A,
fabs(outputs.left_motor * 100.0)); fabsf(outputs.left_motor * 100.0));
ERROR_CHECK(ret); ERROR_CHECK(ret);
ret = mcpwm_start(MCPWM_UNIT, MCPWM_TIMER_0); ret = mcpwm_start(MCPWM_UNIT, MCPWM_TIMER_0);
ERROR_CHECK(ret); ERROR_CHECK(ret);
ret = mcpwm_set_duty(MCPWM_UNIT, MCPWM_TIMER_1, MCPWM_OPR_A, ret = mcpwm_set_duty(MCPWM_UNIT, MCPWM_TIMER_1, MCPWM_OPR_A,
fabs(outputs.right_motor * 100.0)); fabsf(outputs.right_motor * 100.0));
ERROR_CHECK(ret); ERROR_CHECK(ret);
ret = mcpwm_start(MCPWM_UNIT, MCPWM_TIMER_1); ret = mcpwm_start(MCPWM_UNIT, MCPWM_TIMER_1);
ERROR_CHECK(ret); ERROR_CHECK(ret);

22
main/ugv_main.cc
View File

@ -4,13 +4,17 @@
#include "ugv_display.hh" #include "ugv_display.hh"
#include "ugv_io.hh" #include "ugv_io.hh"
#include <math.h>
namespace ugv { namespace ugv {
using ugv::comms::CommsClass; using ugv::comms::CommsClass;
using ugv::io::IOClass; using ugv::io::IOClass;
static const char *TAG = "ugv_main"; static const char *TAG = "ugv_main";
constexpr uint64_t LOOP_PERIOD_US = 1e6 / 1;
constexpr uint64_t LOOP_PERIOD_US = 1e6 / 100;
static const float PI = atanf(1.0) * 4.0;
extern "C" void OnTimeout(void *arg); extern "C" void OnTimeout(void *arg);
@ -26,7 +30,7 @@ struct State {
State() { State() {
comms = new CommsClass(); comms = new CommsClass();
io = new IOClass(); io = new IOClass();
display = new DisplayClass(comms, io); display = new DisplayClass(comms);
} }
void Init() { void Init() {
@ -37,21 +41,27 @@ struct State {
esp_timer_init(); esp_timer_init();
esp_timer_create_args_t timer_args; esp_timer_create_args_t timer_args;
timer_args.callback = OnTimeout; timer_args.callback = OnTimeout;
timer_args.arg = this; timer_args.arg = this;
timer_args.dispatch_method = ESP_TIMER_TASK; timer_args.dispatch_method = ESP_TIMER_TASK;
timer_args.name = "ugv_main_loop"; timer_args.name = "ugv_main_loop";
esp_timer_create(&timer_args, &this->timer_handle); esp_timer_create(&timer_args, &this->timer_handle);
esp_timer_start_periodic(timer_handle, LOOP_PERIOD_US); esp_timer_start_periodic(timer_handle, LOOP_PERIOD_US);
} }
void OnTick() { void OnTick() {
ESP_LOGD(TAG, "OnTick"); ESP_LOGD(TAG, "OnTick");
int64_t time_us = esp_timer_get_time();
float time_s = ((float)time_us) / 1e6;
io->ReadInputs(inputs);
outputs.left_motor = sinf(time_s * PI);
outputs.right_motor = cosf(time_s * PI);
io->WriteOutputs(outputs);
} }
}; };
extern "C" void OnTimeout(void *arg) { extern "C" void OnTimeout(void *arg) {
State *state = (State*)arg; State *state = (State *)arg;
state->OnTick(); state->OnTick();
} }