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 ret;
ret = RawWrite(CMD_READ_PARAMS, sizeof(CMD_READ_PARAMS));
ret = WaitWriteDone();
if (ret != ESP_OK) {
ESP_LOGE(TAG, "error writing read params cmd");
return ret;
}
uint8_t param_data[PARAMS_LEN];
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;
}
@ -241,7 +244,8 @@ esp_err_t E32_Driver::WriteParams(const Params& params) {
param_data[5] |= 3;
}
ret = RawWrite(param_data, PARAMS_LEN);
RawWrite(param_data, PARAMS_LEN);
ret = WaitWriteDone();
if (ret != ESP_OK) {
return ret;
}
@ -251,8 +255,8 @@ esp_err_t E32_Driver::WriteParams(const Params& params) {
return ESP_OK;
}
int E32_Driver::Write(Address address, Channel channel,
const uint8_t* data, size_t data_size) {
int E32_Driver::Write(Address address, Channel channel, const uint8_t* data,
size_t data_size) {
int written;
if (params_.tx_mode == TxFixed) {
uint8_t header[3];
@ -260,7 +264,8 @@ int E32_Driver::Write(Address address, Channel channel,
header[1] = address & 0xFF;
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) {
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);
}
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());
}
int E32_Driver::Write(const std::string& data) {
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) {
return uart_read_bytes(config_.uart_port, (uint8_t*)data, max_len,
ticks_to_wait);
@ -304,8 +313,13 @@ int E32_Driver::Read(std::string& data, TickType_t ticks_to_wait) {
return total_read;
}
esp_err_t E32_Driver::RawWrite(const uint8_t* data, size_t data_size) {
return uart_write_bytes(config_.uart_port, (const char*)data, data_size);
int E32_Driver::RawWrite(const uint8_t* data, size_t 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

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(const std::string& data);
esp_err_t WaitWriteDone(TickType_t ticks_to_wait = portMAX_DELAY);
int Read(uint8_t* data, int max_len,
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_;
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

17
main/ugv_display.cc
View File

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

6
main/ugv_display.hh
View File

@ -10,20 +10,16 @@ namespace ugv {
namespace comms {
class CommsClass;
}
namespace io {
class IOClass;
}
class DisplayClass {
public:
DisplayClass() = delete;
DisplayClass(comms::CommsClass *comms, io::IOClass *io);
DisplayClass(comms::CommsClass *comms);
void Init();
private:
comms::CommsClass *comms_;
io::IOClass *io_;
U8G2 *oled;
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) {
esp_err_t ret;
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);
ret = mcpwm_start(MCPWM_UNIT, MCPWM_TIMER_0);
ERROR_CHECK(ret);
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);
ret = mcpwm_start(MCPWM_UNIT, MCPWM_TIMER_1);
ERROR_CHECK(ret);

22
main/ugv_main.cc
View File

@ -4,13 +4,17 @@
#include "ugv_display.hh"
#include "ugv_io.hh"
#include <math.h>
namespace ugv {
using ugv::comms::CommsClass;
using ugv::io::IOClass;
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);
@ -26,7 +30,7 @@ struct State {
State() {
comms = new CommsClass();
io = new IOClass();
display = new DisplayClass(comms, io);
display = new DisplayClass(comms);
}
void Init() {
@ -37,21 +41,27 @@ struct State {
esp_timer_init();
esp_timer_create_args_t timer_args;
timer_args.callback = OnTimeout;
timer_args.arg = this;
timer_args.callback = OnTimeout;
timer_args.arg = this;
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_start_periodic(timer_handle, LOOP_PERIOD_US);
}
void 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) {
State *state = (State*)arg;
State *state = (State *)arg;
state->OnTick();
}