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";
//#define CALIBRATE
static constexpr float PI =
3.1415926535897932384626433832795028841971693993751058209749445923078164062;
constexpr uint64_t LOOP_PERIOD_US = 1e6 / 100;
constexpr float LOOP_HZ = 1000000.f / static_cast<float>(LOOP_PERIOD_US);
constexpr float LOOP_PERIOD_S = 1 / LOOP_HZ;
@ -108,11 +113,14 @@ void UGV::UpdateAHRS() {
void UGV::DoDebugPrint() {
auto &mpu = inputs_.mpu;
#ifdef CALIBRATE
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);
#else
ESP_LOGD(TAG, "ahrs: yaw=%f, pitch=%f, roll=%f", yaw_, pitch_, roll_);
ESP_LOGV(TAG, "target angle: %f", angle_controller_.Setpoint());
#endif
}
void UGV::ReadComms() {
@ -155,7 +163,7 @@ void UGV::ReadComms() {
void UGV::OnTick() {
ESP_LOGV(TAG, "OnTick");
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_);
UpdateAHRS();
@ -267,9 +275,10 @@ void UGV::OnTick() {
break;
}
case UGV_State::STATE_TEST:
#define BASIC_TEST
#ifdef BASIC_TEST
outputs.left_motor = sinf(time_s * PI);
outputs.right_motor = cosf(time_s * PI);
outputs_.left_motor = sinf(time_s * PI);
outputs_.right_motor = cosf(time_s * PI);
#else
angle_controller_.Enable();
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 &&
inputs_.mpu.gyro_rate.z == 0) {
outputs_.left_motor = 0;
outputs_.right_motor = 0;
// outputs_.left_motor = 0;
// outputs_.right_motor = 0;
} else {
float dleft = outputs_.left_motor - last_left_;
float dright = outputs_.right_motor - last_right_;
@ -319,7 +328,13 @@ void UGV::OnTick() {
comms->status.set_state(next_state_);
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();
last_print_ = time_us;
}