diff --git a/main/messages.proto b/main/messages.proto index f55210c..cb71c81 100644 --- a/main/messages.proto +++ b/main/messages.proto @@ -35,6 +35,7 @@ message UGV_Status { float yaw_angle = 3; float pitch_angle = 4; float roll_angle = 5; + bool is_still = 6; } message UGV_Message { diff --git a/main/ugv.cc b/main/ugv.cc index 4d513bd..83ce731 100644 --- a/main/ugv.cc +++ b/main/ugv.cc @@ -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(LOOP_PERIOD_US); +constexpr float LOOP_HZ = 1000000.f / static_cast(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(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; } 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() { 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() { 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() { 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() { 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() { 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() { 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; diff --git a/main/ugv.hh b/main/ugv.hh index 7ae83ad..579df50 100644 --- a/main/ugv.hh +++ b/main/ugv.hh @@ -46,6 +46,13 @@ class UGV { float yaw_; float pitch_; float roll_; + io::MpuData last_mpu_; + int64_t last_noise_; + float accel_noise_accum_; + float gyro_noise_accum_; + float accel_noise_; + float gyro_noise_; + bool is_still_; void UpdateAHRS(); void DoDebugPrint(); diff --git a/main/ugv_io_mpu.hh b/main/ugv_io_mpu.hh index ee1acf7..05fd94c 100644 --- a/main/ugv_io_mpu.hh +++ b/main/ugv_io_mpu.hh @@ -33,9 +33,22 @@ struct Vec3f { return *this; } + Vec3f operator-(const Vec3f& a) const { return {x - a.x, y - a.y, z - a.z}; } + + Vec3f& operator-=(const Vec3f& a) { + x -= a.x; + y -= a.y; + z -= a.z; + return *this; + } + float dot(const Vec3f& v) const { return x * v.x + y * v.y + z * v.z; } + + float norm2() const { + return x * x + y * y + z * z; + } }; struct Mat3f { diff --git a/tools/messages_pb2.py b/tools/messages_pb2.py index 2468ab0..c85df9a 100644 --- a/tools/messages_pb2.py +++ b/tools/messages_pb2.py @@ -23,7 +23,7 @@ DESCRIPTOR = _descriptor.FileDescriptor( package='ugv.messages', syntax='proto3', serialized_options=_b('H\003'), - serialized_pb=_b('\n\x0emessages.proto\x12\x0cugv.messages\x1a\x0c\x63onfig.proto\"5\n\x0eTargetLocation\x12\x10\n\x08latitude\x18\x01 \x01(\x02\x12\x11\n\tlongitude\x18\x02 \x01(\x02\"V\n\x08Location\x12\x13\n\x0b\x66ix_quality\x18\x01 \x01(\r\x12\x10\n\x08latitude\x18\x02 \x01(\x02\x12\x11\n\tlongitude\x18\x03 \x01(\x02\x12\x10\n\x08\x61ltitude\x18\x04 \x01(\x02\"\x9a\x01\n\nUGV_Status\x12&\n\x05state\x18\x01 \x01(\x0e\x32\x17.ugv.messages.UGV_State\x12(\n\x08location\x18\x02 \x01(\x0b\x32\x16.ugv.messages.Location\x12\x11\n\tyaw_angle\x18\x03 \x01(\x02\x12\x13\n\x0bpitch_angle\x18\x04 \x01(\x02\x12\x12\n\nroll_angle\x18\x05 \x01(\x02\"_\n\x0bUGV_Message\x12*\n\x06status\x18\x01 \x01(\x0b\x32\x18.ugv.messages.UGV_StatusH\x00\x12\x15\n\x0b\x63ommand_ack\x18\x02 \x01(\rH\x00\x42\r\n\x0bugv_message\"2\n\x10\x44riveHeadingData\x12\x0f\n\x07heading\x18\x01 \x01(\x02\x12\r\n\x05power\x18\x02 \x01(\x02\"\xea\x01\n\rGroundCommand\x12\n\n\x02id\x18\x01 \x01(\r\x12-\n\x04type\x18\x02 \x01(\x0e\x32\x1f.ugv.messages.GroundCommandType\x12\x37\n\rdrive_heading\x18\x03 \x01(\x0b\x32\x1e.ugv.messages.DriveHeadingDataH\x00\x12\x37\n\x0ftarget_location\x18\x04 \x01(\x0b\x32\x1c.ugv.messages.TargetLocationH\x00\x12$\n\x06\x63onfig\x18\x05 \x01(\x0b\x32\x12.ugv.config.ConfigH\x00\x42\x06\n\x04\x64\x61ta\"Q\n\rGroundMessage\x12.\n\x07\x63ommand\x18\x01 \x01(\x0b\x32\x1b.ugv.messages.GroundCommandH\x00\x42\x10\n\x0eground_message*\xa6\x01\n\tUGV_State\x12\x0e\n\nSTATE_IDLE\x10\x00\x12\x12\n\x0eSTATE_AQUIRING\x10\x01\x12\x11\n\rSTATE_DRIVING\x10\x02\x12\x12\n\x0eSTATE_FINISHED\x10\x03\x12\x0e\n\nSTATE_TEST\x10\x04\x12\x12\n\x0eSTATE_FLIPPING\x10\x05\x12\x11\n\rSTATE_TURNING\x10\x06\x12\x17\n\x13STATE_DRIVE_HEADING\x10\x07*\xac\x01\n\x11GroundCommandType\x12\x0f\n\x0b\x43MD_DISABLE\x10\x00\x12\x17\n\x13\x43MD_DRIVE_TO_TARGET\x10\x01\x12\x0c\n\x08\x43MD_TEST\x10\x02\x12\x15\n\x11\x43MD_DRIVE_HEADING\x10\x03\x12\x12\n\x0e\x43MD_SET_TARGET\x10\x04\x12\x12\n\x0e\x43MD_SET_CONFIG\x10\x05\x12\x12\n\x0e\x43MD_GET_STATUS\x10\x06\x12\x0c\n\x08\x43MD_PING\x10\x07\x42\x02H\x03P\x00\x62\x06proto3') + serialized_pb=_b('\n\x0emessages.proto\x12\x0cugv.messages\x1a\x0c\x63onfig.proto\"5\n\x0eTargetLocation\x12\x10\n\x08latitude\x18\x01 \x01(\x02\x12\x11\n\tlongitude\x18\x02 \x01(\x02\"V\n\x08Location\x12\x13\n\x0b\x66ix_quality\x18\x01 \x01(\r\x12\x10\n\x08latitude\x18\x02 \x01(\x02\x12\x11\n\tlongitude\x18\x03 \x01(\x02\x12\x10\n\x08\x61ltitude\x18\x04 \x01(\x02\"\xac\x01\n\nUGV_Status\x12&\n\x05state\x18\x01 \x01(\x0e\x32\x17.ugv.messages.UGV_State\x12(\n\x08location\x18\x02 \x01(\x0b\x32\x16.ugv.messages.Location\x12\x11\n\tyaw_angle\x18\x03 \x01(\x02\x12\x13\n\x0bpitch_angle\x18\x04 \x01(\x02\x12\x12\n\nroll_angle\x18\x05 \x01(\x02\x12\x10\n\x08is_still\x18\x06 \x01(\x08\"_\n\x0bUGV_Message\x12*\n\x06status\x18\x01 \x01(\x0b\x32\x18.ugv.messages.UGV_StatusH\x00\x12\x15\n\x0b\x63ommand_ack\x18\x02 \x01(\rH\x00\x42\r\n\x0bugv_message\"2\n\x10\x44riveHeadingData\x12\x0f\n\x07heading\x18\x01 \x01(\x02\x12\r\n\x05power\x18\x02 \x01(\x02\"\xea\x01\n\rGroundCommand\x12\n\n\x02id\x18\x01 \x01(\r\x12-\n\x04type\x18\x02 \x01(\x0e\x32\x1f.ugv.messages.GroundCommandType\x12\x37\n\rdrive_heading\x18\x03 \x01(\x0b\x32\x1e.ugv.messages.DriveHeadingDataH\x00\x12\x37\n\x0ftarget_location\x18\x04 \x01(\x0b\x32\x1c.ugv.messages.TargetLocationH\x00\x12$\n\x06\x63onfig\x18\x05 \x01(\x0b\x32\x12.ugv.config.ConfigH\x00\x42\x06\n\x04\x64\x61ta\"Q\n\rGroundMessage\x12.\n\x07\x63ommand\x18\x01 \x01(\x0b\x32\x1b.ugv.messages.GroundCommandH\x00\x42\x10\n\x0eground_message*\xa6\x01\n\tUGV_State\x12\x0e\n\nSTATE_IDLE\x10\x00\x12\x12\n\x0eSTATE_AQUIRING\x10\x01\x12\x11\n\rSTATE_DRIVING\x10\x02\x12\x12\n\x0eSTATE_FINISHED\x10\x03\x12\x0e\n\nSTATE_TEST\x10\x04\x12\x12\n\x0eSTATE_FLIPPING\x10\x05\x12\x11\n\rSTATE_TURNING\x10\x06\x12\x17\n\x13STATE_DRIVE_HEADING\x10\x07*\xac\x01\n\x11GroundCommandType\x12\x0f\n\x0b\x43MD_DISABLE\x10\x00\x12\x17\n\x13\x43MD_DRIVE_TO_TARGET\x10\x01\x12\x0c\n\x08\x43MD_TEST\x10\x02\x12\x15\n\x11\x43MD_DRIVE_HEADING\x10\x03\x12\x12\n\x0e\x43MD_SET_TARGET\x10\x04\x12\x12\n\x0e\x43MD_SET_CONFIG\x10\x05\x12\x12\n\x0e\x43MD_GET_STATUS\x10\x06\x12\x0c\n\x08\x43MD_PING\x10\x07\x42\x02H\x03P\x00\x62\x06proto3') , dependencies=[config__pb2.DESCRIPTOR,], public_dependencies=[config__pb2.DESCRIPTOR,]) @@ -69,8 +69,8 @@ _UGV_STATE = _descriptor.EnumDescriptor( ], containing_type=None, serialized_options=None, - serialized_start=816, - serialized_end=982, + serialized_start=834, + serialized_end=1000, ) _sym_db.RegisterEnumDescriptor(_UGV_STATE) @@ -116,8 +116,8 @@ _GROUNDCOMMANDTYPE = _descriptor.EnumDescriptor( ], containing_type=None, serialized_options=None, - serialized_start=985, - serialized_end=1157, + serialized_start=1003, + serialized_end=1175, ) _sym_db.RegisterEnumDescriptor(_GROUNDCOMMANDTYPE) @@ -273,6 +273,13 @@ _UGV_STATUS = _descriptor.Descriptor( message_type=None, enum_type=None, containing_type=None, is_extension=False, extension_scope=None, serialized_options=None, file=DESCRIPTOR), + _descriptor.FieldDescriptor( + name='is_still', full_name='ugv.messages.UGV_Status.is_still', index=5, + number=6, type=8, cpp_type=7, label=1, + has_default_value=False, default_value=False, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR), ], extensions=[ ], @@ -286,7 +293,7 @@ _UGV_STATUS = _descriptor.Descriptor( oneofs=[ ], serialized_start=190, - serialized_end=344, + serialized_end=362, ) @@ -326,8 +333,8 @@ _UGV_MESSAGE = _descriptor.Descriptor( name='ugv_message', full_name='ugv.messages.UGV_Message.ugv_message', index=0, containing_type=None, fields=[]), ], - serialized_start=346, - serialized_end=441, + serialized_start=364, + serialized_end=459, ) @@ -364,8 +371,8 @@ _DRIVEHEADINGDATA = _descriptor.Descriptor( extension_ranges=[], oneofs=[ ], - serialized_start=443, - serialized_end=493, + serialized_start=461, + serialized_end=511, ) @@ -426,8 +433,8 @@ _GROUNDCOMMAND = _descriptor.Descriptor( name='data', full_name='ugv.messages.GroundCommand.data', index=0, containing_type=None, fields=[]), ], - serialized_start=496, - serialized_end=730, + serialized_start=514, + serialized_end=748, ) @@ -460,8 +467,8 @@ _GROUNDMESSAGE = _descriptor.Descriptor( name='ground_message', full_name='ugv.messages.GroundMessage.ground_message', index=0, containing_type=None, fields=[]), ], - serialized_start=732, - serialized_end=813, + serialized_start=750, + serialized_end=831, ) _UGV_STATUS.fields_by_name['state'].enum_type = _UGV_STATE