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

move io stuff out of display

try-fix-comms-errors
Alex Mikhalev 6 years ago
parent
5bfb073e92
commit
9b000427be
6 changed files with 48 additions and 35 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 30
      main/e32_driver.cc
  2. 4
      main/e32_driver.hh
  3. 17
      main/ugv_display.cc
  4. 6
      main/ugv_display.hh
  5. 4
      main/ugv_io.cc
  6. 22
      main/ugv_main.cc

30
main/e32_driver.cc
Unescape 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
Unescape 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
Unescape 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
Unescape 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
Unescape 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
Unescape 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();
} }

Write Preview
Loading…
Cancel
Save