add stillness checking

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 {

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<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; }
 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;

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();

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 {

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_start=834,
-  serialized_end=982,
+  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_start=1003,
-  serialized_end=1157,
+  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_start=364,
-  serialized_end=441,
+  serialized_end=459,
 )
 
 
@@ -364,8 +371,8 @@ _DRIVEHEADINGDATA = _descriptor.Descriptor(
   extension_ranges=[],
   oneofs=[
   ],
-  serialized_start=443,
+  serialized_start=461,
-  serialized_end=493,
+  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_start=514,
-  serialized_end=730,
+  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_start=750,
-  serialized_end=813,
+  serialized_end=831,
 )
 
 _UGV_STATUS.fields_by_name['state'].enum_type = _UGV_STATE