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

Few cleanups in ugv.cc

Browse Source
This commit is contained in:
Alex Mikhalev 2019-06-01 17:19:13 -07:00
parent 1e3adb691e
commit 429d195603
1 changed files with 21 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
main/ugv.cc
View File

@ -8,6 +8,11 @@ namespace ugv {
static const char *TAG = "ugv_main"; static const char *TAG = "ugv_main";
//#define CALIBRATE
static constexpr float PI =
3.1415926535897932384626433832795028841971693993751058209749445923078164062;
constexpr uint64_t LOOP_PERIOD_US = 1e6 / 100; constexpr uint64_t LOOP_PERIOD_US = 1e6 / 100;
constexpr float LOOP_HZ = 1000000.f / static_cast<float>(LOOP_PERIOD_US); constexpr float LOOP_HZ = 1000000.f / static_cast<float>(LOOP_PERIOD_US);
constexpr float LOOP_PERIOD_S = 1 / LOOP_HZ; constexpr float LOOP_PERIOD_S = 1 / LOOP_HZ;
@ -108,11 +113,14 @@ void UGV::UpdateAHRS() {
void UGV::DoDebugPrint() { void UGV::DoDebugPrint() {
auto &mpu = inputs_.mpu; auto &mpu = inputs_.mpu;
#ifdef CALIBRATE
ESP_LOGD(TAG, "inputs: acc=(%f, %f, %f) gyro=(%f, %f, %f) mag=(%f, %f, %f)", 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.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); mpu.gyro_rate.y, mpu.gyro_rate.z, mpu.mag.x, mpu.mag.y, mpu.mag.z);
#else
ESP_LOGD(TAG, "ahrs: yaw=%f, pitch=%f, roll=%f", yaw_, pitch_, roll_); ESP_LOGD(TAG, "ahrs: yaw=%f, pitch=%f, roll=%f", yaw_, pitch_, roll_);
ESP_LOGV(TAG, "target angle: %f", angle_controller_.Setpoint()); ESP_LOGV(TAG, "target angle: %f", angle_controller_.Setpoint());
#endif
} }
void UGV::ReadComms() { void UGV::ReadComms() {
@ -155,7 +163,7 @@ void UGV::ReadComms() {
void UGV::OnTick() { void UGV::OnTick() {
ESP_LOGV(TAG, "OnTick"); ESP_LOGV(TAG, "OnTick");
int64_t time_us = esp_timer_get_time(); int64_t time_us = esp_timer_get_time();
// float time_s = ((float)time_us) / 1e6; float time_s = ((float)time_us) / 1e6;
io->ReadInputs(inputs_); io->ReadInputs(inputs_);
UpdateAHRS(); UpdateAHRS();
@ -267,9 +275,10 @@ void UGV::OnTick() {
break; break;
} }
case UGV_State::STATE_TEST: case UGV_State::STATE_TEST:
#define BASIC_TEST
#ifdef BASIC_TEST #ifdef BASIC_TEST
outputs.left_motor = sinf(time_s * PI); outputs_.left_motor = sinf(time_s * PI);
outputs.right_motor = cosf(time_s * PI); outputs_.right_motor = cosf(time_s * PI);
#else #else
angle_controller_.Enable(); angle_controller_.Enable();
angle_controller_.Setpoint(90.0); angle_controller_.Setpoint(90.0);
@ -292,8 +301,8 @@ void UGV::OnTick() {
if (inputs_.mpu.gyro_rate.x == 0 && inputs_.mpu.gyro_rate.y == 0 && if (inputs_.mpu.gyro_rate.x == 0 && inputs_.mpu.gyro_rate.y == 0 &&
inputs_.mpu.gyro_rate.z == 0) { inputs_.mpu.gyro_rate.z == 0) {
outputs_.left_motor = 0; // outputs_.left_motor = 0;
outputs_.right_motor = 0; // outputs_.right_motor = 0;
} else { } else {
float dleft = outputs_.left_motor - last_left_; float dleft = outputs_.left_motor - last_left_;
float dright = outputs_.right_motor - last_right_; float dright = outputs_.right_motor - last_right_;
@ -319,7 +328,13 @@ void UGV::OnTick() {
comms->status.set_state(next_state_); comms->status.set_state(next_state_);
comms->Unlock(); comms->Unlock();
if (time_us >= last_print_ + 500 * 1000) { #ifdef CALIBRATE
const long debug_print_period = 50 * 1000;
#else
const long debug_print_period = 500 * 1000;
#endif
if (time_us >= last_print_ + debug_print_period) {
DoDebugPrint(); DoDebugPrint();
last_print_ = time_us; last_print_ = time_us;
} }