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

send config over rf link

master
Alex Mikhalev 6 years ago
parent
9fe62e38af
commit
31f41c4b3c
6 changed files with 202 additions and 94 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 2
      main/CMakeLists.txt
  2. 20
      main/config.proto
  3. 13
      main/messages.proto
  4. 66
      main/ugv_comms.cc
  5. 10
      main/ugv_comms.hh
  6. 169
      main/ugv_main.cc

2
main/CMakeLists.txt
Unescape View File

@ -18,6 +18,8 @@ set(COMPONENT_PRIV_INCLUDEDIRS "." ${PB_OUT}) @@ -18,6 +18,8 @@ set(COMPONENT_PRIV_INCLUDEDIRS "." ${PB_OUT})
set(COMPONENT_REQUIRES "u8g2" "sx127x_driver" "protobuf" "MPU-driver" "minmea" "mbedtls")
proto_generate_cpp(messages.proto ${PB_OUT} PROTO_HDRS PROTO_SRCS)
proto_generate_cpp(config.proto ${PB_OUT} PROTO_HDRS PROTO_SRCS)
message(STATUS "PROTO_SRCS: ${PROTO_SRCS}")
list(APPEND COMPONENT_SRCS ${PROTO_SRCS} ${PROTO_HDRS})
register_component()

20
main/config.proto
Unescape View File

@ -0,0 +1,20 @@ @@ -0,0 +1,20 @@
syntax = "proto3";
package ugv.config;
option optimize_for = LITE_RUNTIME;
message PidParams {
float kp = 1;
float ki = 2;
float kd = 3;
float max_output = 4;
float max_i_error = 5;
}
message Config {
float min_target_dist = 1;
float drive_power = 2;
PidParams angle_pid = 3;
float min_flip_pitch = 4;
}

13
main/messages.proto
Unescape View File

@ -1,6 +1,8 @@ @@ -1,6 +1,8 @@
syntax = "proto3";
package uas.ugv.messages;
package ugv.messages;
import public "config.proto";
option optimize_for = LITE_RUNTIME;
@ -15,6 +17,11 @@ enum UGV_State { @@ -15,6 +17,11 @@ enum UGV_State {
STATE_DRIVE_HEADING = 7;
}
message TargetLocation {
float latitude = 1;
float longitude = 2;
}
message Location {
uint32 fix_quality = 1;
float latitude = 2;
@ -40,6 +47,8 @@ enum GroundCommandType { @@ -40,6 +47,8 @@ enum GroundCommandType {
CMD_DRIVE_TO_TARGET = 1;
CMD_TEST = 2;
CMD_DRIVE_HEADING = 3;
CMD_SET_TARGET = 4;
CMD_SET_CONFIG = 5;
}
message DriveHeadingData {
@ -52,6 +61,8 @@ message GroundCommand { @@ -52,6 +61,8 @@ message GroundCommand {
GroundCommandType type = 2;
oneof data {
DriveHeadingData drive_heading = 3;
TargetLocation target_location = 4;
config.Config config = 5;
}
}

66
main/ugv_comms.cc
Unescape View File

@ -14,24 +14,29 @@ namespace ugv { @@ -14,24 +14,29 @@ namespace ugv {
namespace comms {
static const char *TAG = "ugv_comms";
static const TickType_t SEND_PERIOD = pdMS_TO_TICKS(5000);
CommsClass::CommsClass()
: location(),
ugv_state(messages::STATE_IDLE),
last_packet_tick(0),
: last_packet_tick(0),
last_packet_rssi(INT32_MIN),
last_packet_snr(INT8_MIN) {
last_packet_snr(INT8_MIN),
status(),
drive_heading(),
new_target(nullptr),
new_config(nullptr) {
status.set_state(messages::STATE_IDLE);
mutex = xSemaphoreCreateMutex();
}
void CommsClass::Init() {
esp_err_t ret;
location.set_fix_quality(0);
location.set_latitude(43.65);
location.set_longitude(-116.20);
location.set_altitude(2730);
ugv_state = messages::UGV_State::STATE_IDLE;
auto *loc = status.mutable_location();
loc->set_fix_quality(0);
loc->set_latitude(43.65);
loc->set_longitude(-116.20);
loc->set_altitude(2730);
status.set_state(messages::UGV_State::STATE_IDLE);
#ifdef COMMS_SX127X
sx127x_config_t lora_config = sx127x_config_default();
@ -115,9 +120,8 @@ void CommsClass::RunTask() { @@ -115,9 +120,8 @@ void CommsClass::RunTask() {
using messages::UGV_State;
using messages::UGV_Status;
TickType_t send_period = pdMS_TO_TICKS(2000);
TickType_t current_tick = xTaskGetTickCount();
TickType_t next_send = current_tick + send_period;
TickType_t next_send = current_tick + SEND_PERIOD;
esp_err_t ret;
@ -130,7 +134,6 @@ void CommsClass::RunTask() { @@ -130,7 +134,6 @@ void CommsClass::RunTask() {
UGV_Message ugv_message;
std::string ugv_message_data;
UGV_Status *status = ugv_message.mutable_status();
while (true) {
TickType_t delay_ticks = next_send - current_tick;
@ -146,8 +149,8 @@ void CommsClass::RunTask() { @@ -146,8 +149,8 @@ void CommsClass::RunTask() {
// receiving packet data now
rx_len = lora.ReadLn(rx_buf, delay_ticks);
if (rx_len <= 0) {
ESP_LOGI(TAG, "timeout for packet rx");
lora.Flush();
ESP_LOGV(TAG, "timeout for packet rx");
// lora.Flush();
} else {
ESP_LOGV(TAG, "rx data: %.*s", rx_buf.size(), rx_buf.c_str());
HandlePacket((uint8_t *)rx_buf.c_str(), rx_buf.size());
@ -162,9 +165,7 @@ void CommsClass::RunTask() { @@ -162,9 +165,7 @@ void CommsClass::RunTask() {
}
Lock();
status->mutable_location()->CopyFrom(this->location);
status->set_yaw_angle(this->yaw_angle);
status->set_state(this->ugv_state);
ugv_message.mutable_status()->CopyFrom(this->status);
Unlock();
ugv_message.SerializeToString(&ugv_message_data);
ret = SendPacket(ugv_message_data);
@ -175,7 +176,7 @@ void CommsClass::RunTask() { @@ -175,7 +176,7 @@ void CommsClass::RunTask() {
}
current_tick = xTaskGetTickCount();
next_send = current_tick + send_period;
next_send = current_tick + SEND_PERIOD;
}
}
@ -237,24 +238,43 @@ void CommsClass::HandlePacket(const uint8_t *data, size_t data_size) { @@ -237,24 +238,43 @@ void CommsClass::HandlePacket(const uint8_t *data, size_t data_size) {
void CommsClass::HandleCommand(const messages::GroundCommand &command) {
ESP_LOGI(TAG, "rx command id %d type %d", command.id(), command.type());
using messages::UGV_State;
xSemaphoreTake(mutex, pdMS_TO_TICKS(10));
switch (command.type()) {
case messages::CMD_DISABLE: ugv_state = messages::STATE_IDLE; break;
case messages::CMD_DISABLE: status.set_state(UGV_State::STATE_IDLE); break;
case messages::CMD_DRIVE_TO_TARGET:
ugv_state = messages::STATE_AQUIRING;
status.set_state(UGV_State::STATE_AQUIRING);
break;
case messages::CMD_TEST: ugv_state = messages::STATE_TEST; break;
case messages::CMD_TEST: status.set_state(UGV_State::STATE_TEST); break;
case messages::CMD_DRIVE_HEADING: {
if (command.has_drive_heading()) {
this->ugv_state = messages::STATE_DRIVE_HEADING;
status.set_state(UGV_State::STATE_DRIVE_HEADING);
this->drive_heading = command.drive_heading();
} else {
this->ugv_state = messages::STATE_IDLE;
status.set_state(UGV_State::STATE_IDLE);
ESP_LOGE(TAG, "invalid CMD_DRIVE_HEADING");
}
break;
}
case messages::CMD_SET_TARGET: {
if (command.has_target_location()) {
if (!new_target) {
new_target = new messages::TargetLocation();
}
new_target->CopyFrom(command.target_location());
}
break;
}
case messages::CMD_SET_CONFIG: {
if (command.has_config()) {
if (!new_config) {
new_config = new config::Config();
}
new_config->CopyFrom(command.config());
}
break;
}
default:
ESP_LOGW(TAG, "unhandled command type: %d", command.type());
xSemaphoreGive(mutex);

10
main/ugv_comms.hh
Unescape View File

@ -15,7 +15,8 @@ @@ -15,7 +15,8 @@
namespace ugv {
namespace comms {
namespace messages = uas::ugv::messages;
namespace messages = ugv::messages;
namespace config = ugv::config;
class CommsClass {
public:
@ -34,13 +35,14 @@ class CommsClass { @@ -34,13 +35,14 @@ class CommsClass {
public:
SemaphoreHandle_t mutex;
messages::Location location;
float yaw_angle;
messages::UGV_State ugv_state;
TickType_t last_packet_tick;
int32_t last_packet_rssi;
int8_t last_packet_snr;
messages::UGV_Status status;
messages::DriveHeadingData drive_heading;
messages::TargetLocation* new_target;
config::Config* new_config;
private:
#ifdef COMMS_SX127X

169
main/ugv_main.cc
Unescape View File

@ -1,13 +1,14 @@ @@ -1,13 +1,14 @@
#include <esp_log.h>
#include <esp_timer.h>
#include <math.h>
#include "MadgwickAHRS.h"
#include "config.pb.h"
#include "i2c_mutex.h"
#include "ugv_comms.hh"
#include "ugv_display.hh"
#include "ugv_io.hh"
#include <math.h>
namespace ugv {
using ugv::comms::CommsClass;
@ -29,14 +30,6 @@ static const float RAD_PER_DEG = PI / 180.f; @@ -29,14 +30,6 @@ static const float RAD_PER_DEG = PI / 180.f;
// Radius of earth in meters
static const float EARTH_RAD = 6372795.f;
static const float DRIVE_POWER = 0.5;
static const float ANGLE_P = 0.02;
static const float ANGLE_I = 0.000005;
static const float ANGLE_D = -0.01;
static const float MAX_ANGLE_POWER = 0.3;
static const float MAX_ANGLE_I_ERROR = 15.0;
static const float MIN_DIST = 10.0;
extern "C" void OnTimeout(void *arg);
void UpdateLocationFromGPS(comms::messages::Location &location,
@ -52,9 +45,14 @@ struct LatLong { @@ -52,9 +45,14 @@ struct LatLong {
float latitude;
float longitude;
inline LatLong() : LatLong(0., 0.) {}
inline LatLong(double latitude_, double longitude_)
: latitude(latitude_), longitude(longitude_) {}
inline LatLong(const comms::messages::TargetLocation &loc)
: latitude(loc.latitude()), longitude(loc.longitude()) {}
/**
* Return distance from this LatLong to target, in meters
*/
@ -93,7 +91,23 @@ struct LatLong { @@ -93,7 +91,23 @@ struct LatLong {
class PIDController {
public:
explicit PIDController(float dt, float kp, float ki = 0., float kd = 0.);
explicit PIDController(float dt, float kp = 0., float ki = 0., float kd = 0.);
void DeltaT(float dt) {
dt_ = dt;
Reset();
}
float DeltaT() { return dt_; }
float GetP() { return kp_; }
float GetI() { return ki_; }
float GetD() { return kd_; }
void SetPID(float kp, float ki, float kd) {
kp_ = kp;
ki_ = ki;
kd_ = kd;
}
void MaxOutput(float max_output) { max_output_ = max_output; }
float MaxOutput() const { return max_output_; }
@ -183,17 +197,18 @@ float PIDController::Update() { @@ -183,17 +197,18 @@ float PIDController::Update() {
}
float error = Error();
output_ += kp_ * error;
if (fabsf(error) > max_i_error_) {
integral_ = 0.;
} else {
integral_ += error * dt_;
}
output_ += kp_ * error;
output_ += ki_ * integral_;
}
if (!isnan(last_error_)) {
output_ += kd_ * (error - last_error_);
}
output_ = clamp_mag(output_, max_output_);
last_error_ = error;
@ -217,24 +232,49 @@ struct State { @@ -217,24 +232,49 @@ struct State {
DisplayClass * display;
esp_timer_handle_t timer_handle;
io::Inputs inputs;
io::Outputs outputs;
int64_t last_print;
io::Inputs inputs_;
io::Outputs outputs_;
int64_t last_print_;
Madgwick ahrs_;
LatLong target;
LatLong target_;
PIDController angle_controller_;
config::Config conf_;
State()
: target{34.069022, -118.443067},
angle_controller_(LOOP_PERIOD_S, ANGLE_P, ANGLE_I, ANGLE_D) {
angle_controller_.MaxOutput(MAX_ANGLE_POWER);
angle_controller_.MaxIError(MAX_ANGLE_I_ERROR);
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();
@ -252,47 +292,60 @@ struct State { @@ -252,47 +292,60 @@ struct State {
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;
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);
// 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;
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
ESP_LOGD(TAG,
"inputs: acc=(%f, %f, %f) gyro=(%f, %f, %f) mag=(%f, %f, %f)",
inputs.mpu.accel.x, inputs.mpu.accel.y, inputs.mpu.accel.z,
inputs.mpu.gyro_rate.x, inputs.mpu.gyro_rate.y,
inputs.mpu.gyro_rate.z, inputs.mpu.mag.x, inputs.mpu.mag.y,
inputs.mpu.mag.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;
last_print_ = time_us;
}
comms->Lock();
UpdateLocationFromGPS(comms->location, inputs.gps);
comms->yaw_angle = ahrs_.getYaw();
UGV_State ugv_state = comms->ugv_state;
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;
outputs_.left_motor = 0.0;
outputs_.right_motor = 0.0;
float pitch = ahrs_.getPitch();
bool is_upside_down = (pitch > 90.) || (pitch < -90.);
auto min_flip_pitch = conf_.min_flip_pitch();
bool is_upside_down = (pitch > min_flip_pitch) || (pitch < -min_flip_pitch);
switch (ugv_state) {
default:
@ -307,9 +360,9 @@ struct State { @@ -307,9 +360,9 @@ struct State {
}
angle_controller_.Disable();
TickType_t current_tick = xTaskGetTickCount();
TickType_t ticks_since_gps = current_tick - inputs.gps.last_update;
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;
bool not_invalid = inputs_.gps.fix_quality != io::GPS_FIX_INVALID;
if (not_old && not_invalid) {
next_state = UGV_State::STATE_TURNING;
}
@ -317,8 +370,8 @@ struct State { @@ -317,8 +370,8 @@ struct State {
}
case UGV_State::STATE_FLIPPING: {
angle_controller_.Disable();
outputs.left_motor = -1.0;
outputs.right_motor = -1.0;
outputs_.left_motor = -1.0;
outputs_.right_motor = -1.0;
if (!is_upside_down) {
next_state = UGV_State::STATE_AQUIRING;
break;
@ -330,13 +383,13 @@ struct State { @@ -330,13 +383,13 @@ struct State {
next_state = UGV_State::STATE_FLIPPING;
break;
}
if (inputs.gps.fix_quality == io::GPS_FIX_INVALID) {
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);
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);
@ -350,21 +403,21 @@ struct State { @@ -350,21 +403,21 @@ struct State {
next_state = UGV_State::STATE_FLIPPING;
break;
}
if (inputs.gps.fix_quality == io::GPS_FIX_INVALID) {
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);
LatLong current_pos = {inputs_.gps.latitude, inputs_.gps.longitude};
float tgt_dist = current_pos.distance_to(target_);
if (tgt_dist <= MIN_DIST) {
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);
float tgt_bearing = current_pos.bearing_toward(target_);
angle_controller_.Enable();
angle_controller_.Setpoint(tgt_bearing);
break;
@ -387,14 +440,14 @@ struct State { @@ -387,14 +440,14 @@ struct State {
if (angle_controller_.Enabled()) {
float angle_pwr = angle_controller_.Update();
outputs.left_motor = drive_power - angle_pwr;
outputs.right_motor = drive_power + angle_pwr;
outputs_.left_motor = drive_power - angle_pwr;
outputs_.right_motor = drive_power + angle_pwr;
}
io->WriteOutputs(outputs);
io->WriteOutputs(outputs_);
comms->Lock();
comms->ugv_state = next_state;
comms->status.set_state(next_state);
comms->Unlock();
}
};

Write Preview
Loading…
Cancel
Save