|
|
|
|
@ -9,9 +9,16 @@ namespace ugv {
@@ -9,9 +9,16 @@ 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); |
|
|
|
|
constexpr float LOOP_HZ = 1000000.f / static_cast<float>(LOOP_PERIOD_US); |
|
|
|
|
constexpr float LOOP_PERIOD_S = 1 / LOOP_HZ; |
|
|
|
|
constexpr TickType_t WATCHDOG_TICKS = pdMS_TO_TICKS(20 * 1000); |
|
|
|
|
|
|
|
|
|
constexpr uint64_t NOISE_PERIOD_US = 2000e3; |
|
|
|
|
constexpr float NOISE_PERIOD_S = static_cast<float>(NOISE_PERIOD_US) * 1.e-6f; |
|
|
|
|
|
|
|
|
|
constexpr float ACCEL_NOISE_THRESHOLD = 0.001; |
|
|
|
|
constexpr float GYRO_NOISE_THRESHOLD = 0.3; |
|
|
|
|
|
|
|
|
|
void UpdateLocationFromGPS(comms::messages::Location &location, |
|
|
|
|
const io::GpsData & gps_data) { |
|
|
|
|
location.set_fix_quality(gps_data.fix_quality); |
|
|
|
|
@ -65,7 +72,7 @@ void UGV::SetTarget(LatLong target) { target_ = target; }
@@ -65,7 +72,7 @@ void UGV::SetTarget(LatLong target) { target_ = target; }
|
|
|
|
|
void UGV::Init() { |
|
|
|
|
esp_timer_init(); |
|
|
|
|
|
|
|
|
|
ahrs_.begin(LOOP_PERIOD_S); // rough sample frequency
|
|
|
|
|
ahrs_.begin(LOOP_HZ); // rough sample frequency
|
|
|
|
|
|
|
|
|
|
io->Init(); |
|
|
|
|
comms->Init(); |
|
|
|
|
@ -79,6 +86,7 @@ void UGV::Init() {
@@ -79,6 +86,7 @@ void UGV::Init() {
|
|
|
|
|
esp_timer_create(&timer_args, &this->timer_handle); |
|
|
|
|
esp_timer_start_periodic(timer_handle, LOOP_PERIOD_US); |
|
|
|
|
last_print_ = 0; |
|
|
|
|
last_noise_ = 0; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void UGV::UpdateAHRS() { |
|
|
|
|
@ -98,11 +106,11 @@ void UGV::UpdateAHRS() {
@@ -98,11 +106,11 @@ void UGV::UpdateAHRS() {
|
|
|
|
|
|
|
|
|
|
void UGV::DoDebugPrint() { |
|
|
|
|
auto &mpu = inputs_.mpu; |
|
|
|
|
ESP_LOGD(TAG, "inputs: acc=(%f, %f, %f) gyro=(%f, %f, %f) mag=(%f, %f, %f)", |
|
|
|
|
ESP_LOGV(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", yaw_, pitch_, roll_); |
|
|
|
|
ESP_LOGD(TAG, "target angle: %f", angle_controller_.Setpoint()); |
|
|
|
|
ESP_LOGV(TAG, "target angle: %f", angle_controller_.Setpoint()); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void UGV::ReadComms() { |
|
|
|
|
@ -111,6 +119,7 @@ void UGV::ReadComms() {
@@ -111,6 +119,7 @@ void UGV::ReadComms() {
|
|
|
|
|
comms->status.set_yaw_angle(yaw_); |
|
|
|
|
comms->status.set_pitch_angle(pitch_); |
|
|
|
|
comms->status.set_roll_angle(roll_); |
|
|
|
|
comms->status.set_is_still(is_still_); |
|
|
|
|
UGV_State comms_ugv_state = comms->status.state(); |
|
|
|
|
TickType_t ticks_since_last_packet = |
|
|
|
|
(xTaskGetTickCount() - comms->last_packet_tick); |
|
|
|
|
@ -148,14 +157,6 @@ void UGV::OnTick() {
@@ -148,14 +157,6 @@ void UGV::OnTick() {
|
|
|
|
|
io->ReadInputs(inputs_); |
|
|
|
|
UpdateAHRS(); |
|
|
|
|
|
|
|
|
|
if (time_us >= last_print_ + 500 * 1000) { |
|
|
|
|
DoDebugPrint(); |
|
|
|
|
last_print_ = time_us; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
ReadComms(); |
|
|
|
|
next_state_ = current_state_; |
|
|
|
|
|
|
|
|
|
angle_controller_.Input(yaw_); |
|
|
|
|
float drive_power = 0.; |
|
|
|
|
outputs_.left_motor = 0.0; |
|
|
|
|
@ -165,6 +166,28 @@ void UGV::OnTick() {
@@ -165,6 +166,28 @@ void UGV::OnTick() {
|
|
|
|
|
auto min_flip_pitch = conf_.min_flip_pitch(); |
|
|
|
|
bool is_upside_down = (pitch > min_flip_pitch) || (pitch < -min_flip_pitch); |
|
|
|
|
|
|
|
|
|
io::Vec3f d_accel = inputs_.mpu.accel - last_mpu_.accel; |
|
|
|
|
io::Vec3f d_gyro = inputs_.mpu.gyro_rate - last_mpu_.gyro_rate; |
|
|
|
|
last_mpu_ = inputs_.mpu; |
|
|
|
|
|
|
|
|
|
accel_noise_accum_ += d_accel.norm2() * LOOP_PERIOD_S; |
|
|
|
|
gyro_noise_accum_ += d_gyro.norm2() * LOOP_PERIOD_S; |
|
|
|
|
|
|
|
|
|
if (time_us >= last_noise_ + NOISE_PERIOD_US) { |
|
|
|
|
accel_noise_ = accel_noise_accum_ / NOISE_PERIOD_S; |
|
|
|
|
gyro_noise_ = gyro_noise_accum_ / NOISE_PERIOD_S; |
|
|
|
|
is_still_ = (accel_noise_ < ACCEL_NOISE_THRESHOLD) && |
|
|
|
|
(gyro_noise_ < GYRO_NOISE_THRESHOLD); |
|
|
|
|
ESP_LOGD(TAG, "is still: %s, accel noise: %f, gyro noise: %f", |
|
|
|
|
is_still_ ? "still" : "moving", accel_noise_, gyro_noise_); |
|
|
|
|
accel_noise_accum_ = 0; |
|
|
|
|
gyro_noise_accum_ = 0; |
|
|
|
|
last_noise_ = time_us; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
ReadComms(); |
|
|
|
|
next_state_ = current_state_; |
|
|
|
|
|
|
|
|
|
switch (current_state_) { |
|
|
|
|
default: |
|
|
|
|
ESP_LOGW(TAG, "unhandled state: %d", current_state_); |
|
|
|
|
@ -271,6 +294,11 @@ void UGV::OnTick() {
@@ -271,6 +294,11 @@ void UGV::OnTick() {
|
|
|
|
|
comms->Lock(); |
|
|
|
|
comms->status.set_state(next_state_); |
|
|
|
|
comms->Unlock(); |
|
|
|
|
|
|
|
|
|
if (time_us >= last_print_ + 500 * 1000) { |
|
|
|
|
DoDebugPrint(); |
|
|
|
|
last_print_ = time_us; |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
UGV *the_ugv; |
|
|
|
|
|
