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

refactor everything

Browse Source
This commit is contained in:
Alex Mikhalev 2019-05-15 21:50:38 -07:00
parent fe5160abfc
commit d738c2b4a7
4 changed files with 328 additions and 275 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
main/CMakeLists.txt
View File

@ -15,6 +15,7 @@ set(COMPONENT_SRCS
"e32_driver.cc" "e32_driver.cc"
"pid_controller.cc" "pid_controller.cc"
"lat_long.cc" "lat_long.cc"
"ugv.cc"
) )
set(COMPONENT_PRIV_INCLUDEDIRS "." ${PB_OUT}) set(COMPONENT_PRIV_INCLUDEDIRS "." ${PB_OUT})
set(COMPONENT_REQUIRES "u8g2" "sx127x_driver" "protobuf" "MPU-driver" "minmea" "mbedtls") set(COMPONENT_REQUIRES "u8g2" "sx127x_driver" "protobuf" "MPU-driver" "minmea" "mbedtls")

255
main/ugv.cc Normal file
View File

@ -0,0 +1,255 @@
#include "ugv.hh"
#include "ugv_comms.hh"
#include "ugv_display.hh"
#include "ugv_io.hh"
namespace ugv {
static const char *TAG = "ugv_main";
constexpr uint64_t LOOP_PERIOD_US = 1e6 / 100;
constexpr float LOOP_PERIOD_S = 1000000.f / static_cast<float>(LOOP_PERIOD_US);
void UpdateLocationFromGPS(comms::messages::Location &location,
const io::GpsData & gps_data) {
location.set_fix_quality(gps_data.fix_quality);
location.set_latitude(gps_data.latitude);
location.set_longitude(gps_data.longitude);
location.set_altitude(gps_data.altitude);
}
extern "C" void UGV_TickTimeout(void *arg) {
UGV *ugv = (UGV *)arg;
ugv->OnTick();
}
UGV::UGV() : angle_controller_(LOOP_PERIOD_S) {
SetTarget({34.069022, -118.443067});
comms = new CommsClass();
io = new IOClass();
display = new DisplayClass(comms);
SetConfig(DefaultConfig());
}
config::Config UGV::DefaultConfig() {
config::Config c;
auto *apid = c.mutable_angle_pid();
apid->set_kp(0.10);
apid->set_ki(0.0);
apid->set_kd(0.4);
apid->set_max_output(0.5);
apid->set_max_i_error(15.0);
c.set_min_target_dist(10.0);
c.set_min_flip_pitch(90.0);
return c;
}
void UGV::SetConfig(const config::Config &conf) {
auto &apid = conf.angle_pid();
angle_controller_.SetPID(apid.kp(), apid.ki(), apid.kd());
angle_controller_.MaxOutput(apid.max_output());
angle_controller_.MaxIError(apid.max_i_error());
conf_ = conf;
}
void UGV::SetTarget(LatLong target) { target_ = target; }
void UGV::Init() {
esp_timer_init();
ahrs_.begin(LOOP_PERIOD_S); // rough sample frequency
io->Init();
comms->Init();
display->Init();
esp_timer_create_args_t timer_args;
timer_args.callback = UGV_TickTimeout;
timer_args.arg = this;
timer_args.dispatch_method = ESP_TIMER_TASK;
timer_args.name = "ugv_main_loop";
esp_timer_create(&timer_args, &this->timer_handle);
esp_timer_start_periodic(timer_handle, LOOP_PERIOD_US);
last_print_ = 0;
}
void UGV::UpdateAHRS() {
io::Vec3f &g = inputs_.mpu.gyro_rate, &a = inputs_.mpu.accel,
&m = inputs_.mpu.mag;
ahrs_.update(g.x, g.y, g.z, a.x, a.y, a.z, m.x, m.y, m.z);
}
void UGV::DoDebugPrint() {
auto &mpu = inputs_.mpu;
ESP_LOGD(TAG, "inputs: acc=(%f, %f, %f) gyro=(%f, %f, %f) mag=(%f, %f, %f)",
mpu.accel.x, mpu.accel.y, mpu.accel.z, mpu.gyro_rate.x,
mpu.gyro_rate.y, mpu.gyro_rate.z, mpu.mag.x, mpu.mag.y, mpu.mag.z);
ESP_LOGD(TAG, "ahrs: yaw=%f, pitch=%f, roll=%f", ahrs_.getYaw(),
ahrs_.getPitch(), ahrs_.getRoll());
ESP_LOGD(TAG, "PID: error: %f", angle_controller_.Error());
}
void UGV::ReadComms() {
comms->Lock();
UpdateLocationFromGPS(*(comms->status.mutable_location()), inputs_.gps);
comms->status.set_yaw_angle(ahrs_.getYaw());
current_state_ = comms->status.state();
if (comms->new_target) {
SetTarget(*comms->new_target);
ESP_LOGI(TAG, "Updating target to (%f, %f)", target_.latitude,
target_.longitude);
delete comms->new_target;
comms->new_target = nullptr;
}
if (comms->new_config) {
ESP_LOGI(TAG, "Updating config");
SetConfig(*comms->new_config);
delete comms->new_config;
comms->new_config = nullptr;
}
comms->Unlock();
}
void UGV::OnTick() {
ESP_LOGV(TAG, "OnTick");
int64_t time_us = esp_timer_get_time();
// float time_s = ((float)time_us) / 1e6;
io->ReadInputs(inputs_);
UpdateAHRS();
if (time_us >= last_print_ + 500 * 1000) {
DoDebugPrint();
last_print_ = time_us;
}
ReadComms();
next_state_ = current_state_;
angle_controller_.Input(ahrs_.getYaw());
float drive_power = 0.;
outputs_.left_motor = 0.0;
outputs_.right_motor = 0.0;
float pitch = ahrs_.getPitch();
auto min_flip_pitch = conf_.min_flip_pitch();
bool is_upside_down = (pitch > min_flip_pitch) || (pitch < -min_flip_pitch);
switch (current_state_) {
default:
ESP_LOGW(TAG, "unhandled state: %d", current_state_);
// fall through
case UGV_State::STATE_IDLE:
case UGV_State::STATE_FINISHED: angle_controller_.Disable(); break;
case UGV_State::STATE_AQUIRING: {
if (is_upside_down) {
next_state_ = UGV_State::STATE_FLIPPING;
break;
}
angle_controller_.Disable();
TickType_t current_tick = xTaskGetTickCount();
TickType_t ticks_since_gps = current_tick - inputs_.gps.last_update;
bool not_old = ticks_since_gps <= pdMS_TO_TICKS(2000);
bool not_invalid = inputs_.gps.fix_quality != io::GPS_FIX_INVALID;
if (not_old && not_invalid) {
next_state_ = UGV_State::STATE_TURNING;
}
break;
}
case UGV_State::STATE_FLIPPING: {
angle_controller_.Disable();
outputs_.left_motor = -1.0;
outputs_.right_motor = -1.0;
if (!is_upside_down) {
next_state_ = UGV_State::STATE_AQUIRING;
break;
}
break;
}
case UGV_State::STATE_TURNING: {
if (is_upside_down) {
next_state_ = UGV_State::STATE_FLIPPING;
break;
}
if (inputs_.gps.fix_quality == io::GPS_FIX_INVALID) {
next_state_ = UGV_State::STATE_AQUIRING;
break;
}
LatLong current_pos = {inputs_.gps.latitude, inputs_.gps.longitude};
float tgt_bearing = current_pos.bearing_toward(target_);
angle_controller_.Enable();
angle_controller_.Setpoint(tgt_bearing);
if (fabs(angle_controller_.Error()) <= 5.0) {
next_state_ = UGV_State::STATE_DRIVING;
}
break;
}
case UGV_State::STATE_DRIVING: {
if (is_upside_down) {
next_state_ = UGV_State::STATE_FLIPPING;
break;
}
if (inputs_.gps.fix_quality == io::GPS_FIX_INVALID) {
next_state_ = UGV_State::STATE_AQUIRING;
break;
}
LatLong current_pos = {inputs_.gps.latitude, inputs_.gps.longitude};
float tgt_dist = current_pos.distance_to(target_);
if (tgt_dist <= conf_.min_target_dist()) {
ESP_LOGI(TAG, "Finished driving to target");
next_state_ = UGV_State::STATE_FINISHED;
break;
}
float tgt_bearing = current_pos.bearing_toward(target_);
angle_controller_.Enable();
angle_controller_.Setpoint(tgt_bearing);
break;
}
case UGV_State::STATE_TEST:
#ifdef BASIC_TEST
outputs.left_motor = sinf(time_s * PI);
outputs.right_motor = cosf(time_s * PI);
#else
angle_controller_.Enable();
angle_controller_.Setpoint(90.0);
#endif
break;
case UGV_State::STATE_DRIVE_HEADING:
angle_controller_.Enable();
angle_controller_.Setpoint(comms->drive_heading.heading());
drive_power = comms->drive_heading.power();
break;
}
if (angle_controller_.Enabled()) {
float angle_pwr = angle_controller_.Update();
outputs_.left_motor = drive_power - angle_pwr;
outputs_.right_motor = drive_power + angle_pwr;
}
io->WriteOutputs(outputs_);
comms->Lock();
comms->status.set_state(next_state_);
comms->Unlock();
}
UGV *the_ugv;
void Start(void) {
ESP_LOGI(TAG, "Starting UAS UGV");
the_ugv = new UGV();
the_ugv->Init();
ESP_LOGI(TAG, "Setup finished");
}
} // namespace ugv

68
main/ugv.hh Normal file
View File

@ -0,0 +1,68 @@
#pragma once
#include <esp_log.h>
#include <esp_timer.h>
#include "MadgwickAHRS.h"
#include "config.pb.h"
#include "lat_long.hh"
#include "pid_controller.hh"
#include "ugv_io.hh"
namespace ugv {
namespace comms {
class CommsClass;
}
class DisplayClass;
using ugv::comms::CommsClass;
using ugv::comms::messages::UGV_State;
using ugv::io::IOClass;
extern "C" void UGV_TickTimeout(void *arg);
void UpdateLocationFromGPS(comms::messages::Location &location,
const io::GpsData & gps_data);
class UGV {
private:
CommsClass * comms;
IOClass * io;
DisplayClass * display;
esp_timer_handle_t timer_handle;
LatLong target_;
config::Config conf_;
PIDController angle_controller_;
Madgwick ahrs_;
io::Inputs inputs_;
io::Outputs outputs_;
int64_t last_print_;
UGV_State current_state_;
UGV_State next_state_;
void UpdateAHRS();
void DoDebugPrint();
void ReadComms();
public:
explicit UGV();
static config::Config DefaultConfig();
void SetConfig(const config::Config &conf);
void SetTarget(LatLong target);
void Init();
void OnTick();
};
extern UGV *the_ugv;
void Start(void);
} // namespace ugv

277
main/ugv_main.cc
View File

@ -1,282 +1,11 @@
#include <esp_log.h>
#include <esp_timer.h>
#include <math.h>
#include "MadgwickAHRS.h"
#include "config.pb.h"
#include "i2c_mutex.h" #include "i2c_mutex.h"
#include "ugv_comms.hh" #include "ugv.hh"
#include "ugv_display.hh"
#include "ugv_io.hh"
#include "pid_controller.hh"
#include "lat_long.hh"
namespace ugv {
using ugv::comms::CommsClass;
using ugv::comms::messages::UGV_State;
using ugv::io::IOClass;
static const char *TAG = "ugv_main";
extern "C" { extern "C" {
SemaphoreHandle_t i2c_mutex; SemaphoreHandle_t i2c_mutex;
} }
constexpr uint64_t LOOP_PERIOD_US = 1e6 / 100; extern "C" void app_main() {
constexpr float LOOP_PERIOD_S = 1000000.f / static_cast<float>(LOOP_PERIOD_US);
extern "C" void OnTimeout(void *arg);
void UpdateLocationFromGPS(comms::messages::Location &location,
const io::GpsData & gps_data) {
location.set_fix_quality(gps_data.fix_quality);
location.set_latitude(gps_data.latitude);
location.set_longitude(gps_data.longitude);
location.set_altitude(gps_data.altitude);
}
struct State {
public:
CommsClass * comms;
IOClass * io;
DisplayClass * display;
esp_timer_handle_t timer_handle;
io::Inputs inputs_;
io::Outputs outputs_;
int64_t last_print_;
Madgwick ahrs_;
LatLong target_;
PIDController angle_controller_;
config::Config conf_;
State() : angle_controller_(LOOP_PERIOD_S) {
SetTarget({34.069022, -118.443067});
comms = new CommsClass();
io = new IOClass();
display = new DisplayClass(comms);
SetConfig(DefaultConfig());
}
static config::Config DefaultConfig() {
config::Config c;
auto *apid = c.mutable_angle_pid();
apid->set_kp(0.10);
apid->set_ki(0.0);
apid->set_kd(0.4);
apid->set_max_output(0.5);
apid->set_max_i_error(15.0);
c.set_min_target_dist(10.0);
c.set_min_flip_pitch(90.0);
return c;
}
void SetConfig(const config::Config &conf) {
auto &apid = conf.angle_pid();
angle_controller_.SetPID(apid.kp(), apid.ki(), apid.kd());
angle_controller_.MaxOutput(apid.max_output());
angle_controller_.MaxIError(apid.max_i_error());
conf_ = conf;
}
void SetTarget(LatLong target) { target_ = target; }
void Init() {
esp_timer_init();
i2c_mutex = xSemaphoreCreateMutex(); i2c_mutex = xSemaphoreCreateMutex();
ugv::Start();
ahrs_.begin(LOOP_PERIOD_S); // rough sample frequency
io->Init();
comms->Init();
display->Init();
esp_timer_create_args_t timer_args;
timer_args.callback = OnTimeout;
timer_args.arg = this;
timer_args.dispatch_method = ESP_TIMER_TASK;
timer_args.name = "ugv_main_loop";
esp_timer_create(&timer_args, &this->timer_handle);
esp_timer_start_periodic(timer_handle, LOOP_PERIOD_US);
last_print_ = 0;
} }
void OnTick() {
ESP_LOGV(TAG, "OnTick");
int64_t time_us = esp_timer_get_time();
// float time_s = ((float)time_us) / 1e6;
io->ReadInputs(inputs_);
{
io::Vec3f &g = inputs_.mpu.gyro_rate, &a = inputs_.mpu.accel,
&m = inputs_.mpu.mag;
ahrs_.update(g.x, g.y, g.z, a.x, a.y, a.z, m.x, m.y, m.z);
}
if (time_us >= last_print_ + 500 * 1000) { // 1s
auto &mpu = inputs_.mpu;
ESP_LOGD(
TAG, "inputs: acc=(%f, %f, %f) gyro=(%f, %f, %f) mag=(%f, %f, %f)",
mpu.accel.x, mpu.accel.y, mpu.accel.z, mpu.gyro_rate.x,
mpu.gyro_rate.y, mpu.gyro_rate.z, mpu.mag.x, mpu.mag.y, mpu.mag.z);
ESP_LOGD(TAG, "ahrs: yaw=%f, pitch=%f, roll=%f", ahrs_.getYaw(),
ahrs_.getPitch(), ahrs_.getRoll());
ESP_LOGD(TAG, "PID: error: %f", angle_controller_.Error());
last_print_ = time_us;
}
comms->Lock();
UpdateLocationFromGPS(*(comms->status.mutable_location()), inputs_.gps);
comms->status.set_yaw_angle(ahrs_.getYaw());
UGV_State ugv_state = comms->status.state();
if (comms->new_target) {
SetTarget(*comms->new_target);
ESP_LOGI(TAG, "Updating target to (%f, %f)", target_.latitude,
target_.longitude);
delete comms->new_target;
comms->new_target = nullptr;
}
if (comms->new_config) {
ESP_LOGI(TAG, "Updating config");
SetConfig(*comms->new_config);
delete comms->new_config;
comms->new_config = nullptr;
}
comms->Unlock();
UGV_State next_state = ugv_state;
angle_controller_.Input(ahrs_.getYaw());
float drive_power = 0.;
outputs_.left_motor = 0.0;
outputs_.right_motor = 0.0;
float pitch = ahrs_.getPitch();
auto min_flip_pitch = conf_.min_flip_pitch();
bool is_upside_down = (pitch > min_flip_pitch) || (pitch < -min_flip_pitch);
switch (ugv_state) {
default:
ESP_LOGW(TAG, "unhandled state: %d", ugv_state);
// fall through
case UGV_State::STATE_IDLE:
case UGV_State::STATE_FINISHED: angle_controller_.Disable(); break;
case UGV_State::STATE_AQUIRING: {
if (is_upside_down) {
next_state = UGV_State::STATE_FLIPPING;
break;
}
angle_controller_.Disable();
TickType_t current_tick = xTaskGetTickCount();
TickType_t ticks_since_gps = current_tick - inputs_.gps.last_update;
bool not_old = ticks_since_gps <= pdMS_TO_TICKS(2000);
bool not_invalid = inputs_.gps.fix_quality != io::GPS_FIX_INVALID;
if (not_old && not_invalid) {
next_state = UGV_State::STATE_TURNING;
}
break;
}
case UGV_State::STATE_FLIPPING: {
angle_controller_.Disable();
outputs_.left_motor = -1.0;
outputs_.right_motor = -1.0;
if (!is_upside_down) {
next_state = UGV_State::STATE_AQUIRING;
break;
}
break;
}
case UGV_State::STATE_TURNING: {
if (is_upside_down) {
next_state = UGV_State::STATE_FLIPPING;
break;
}
if (inputs_.gps.fix_quality == io::GPS_FIX_INVALID) {
next_state = UGV_State::STATE_AQUIRING;
break;
}
LatLong current_pos = {inputs_.gps.latitude, inputs_.gps.longitude};
float tgt_bearing = current_pos.bearing_toward(target_);
angle_controller_.Enable();
angle_controller_.Setpoint(tgt_bearing);
if (fabs(angle_controller_.Error()) <= 5.0) {
next_state = UGV_State::STATE_DRIVING;
}
break;
}
case UGV_State::STATE_DRIVING: {
if (is_upside_down) {
next_state = UGV_State::STATE_FLIPPING;
break;
}
if (inputs_.gps.fix_quality == io::GPS_FIX_INVALID) {
next_state = UGV_State::STATE_AQUIRING;
break;
}
LatLong current_pos = {inputs_.gps.latitude, inputs_.gps.longitude};
float tgt_dist = current_pos.distance_to(target_);
if (tgt_dist <= conf_.min_target_dist()) {
ESP_LOGI(TAG, "Finished driving to target");
next_state = UGV_State::STATE_FINISHED;
break;
}
float tgt_bearing = current_pos.bearing_toward(target_);
angle_controller_.Enable();
angle_controller_.Setpoint(tgt_bearing);
break;
}
case UGV_State::STATE_TEST:
#ifdef BASIC_TEST
outputs.left_motor = sinf(time_s * PI);
outputs.right_motor = cosf(time_s * PI);
#else
angle_controller_.Enable();
angle_controller_.Setpoint(90.0);
#endif
break;
case UGV_State::STATE_DRIVE_HEADING:
angle_controller_.Enable();
angle_controller_.Setpoint(comms->drive_heading.heading());
drive_power = comms->drive_heading.power();
break;
}
if (angle_controller_.Enabled()) {
float angle_pwr = angle_controller_.Update();
outputs_.left_motor = drive_power - angle_pwr;
outputs_.right_motor = drive_power + angle_pwr;
}
io->WriteOutputs(outputs_);
comms->Lock();
comms->status.set_state(next_state);
comms->Unlock();
}
};
extern "C" void OnTimeout(void *arg) {
State *state = (State *)arg;
state->OnTick();
}
State *state;
void Setup(void) {
ESP_LOGI(TAG, "Starting UAS UGV");
state = new State();
state->Init();
ESP_LOGI(TAG, "Setup finished");
}
} // namespace ugv
extern "C" void app_main() { ugv::Setup(); }