config changes

main/config.proto

@@ -17,4 +17,5 @@ message Config {
   float drive_power = 2;
   PidParams angle_pid = 3;
   float min_flip_pitch = 4;
+  float mag_declination = 5;
 }

main/ugv.cc

@@ -46,6 +46,8 @@ config::Config UGV::DefaultConfig() {
 
   c.set_min_target_dist(10.0);
   c.set_min_flip_pitch(90.0);
+  c.set_drive_power(0.3);
+  c.set_mag_declination(11.5);
   return c;
 }
 
@@ -82,6 +84,15 @@ void UGV::UpdateAHRS() {
   io::Vec3f &g = inputs_.mpu.gyro_rate, &a = inputs_.mpu.accel,
             &m = inputs_.mpu.mag;
   ahrs_.update(g.x, g.y, g.z, a.x, a.y, a.z, m.x, m.y, m.z);
+  yaw_ = ahrs_.getYaw() + conf_.mag_declination();
+  while (yaw_ > 360.) {
+    yaw_ -= 360.;
+  }
+  while (yaw_ < 0.) {
+    yaw_ += 360.;
+  }
+  pitch_ = ahrs_.getPitch();
+  roll_ = ahrs_.getRoll();
 }
 
 void UGV::DoDebugPrint() {
@@ -89,16 +100,16 @@ void UGV::DoDebugPrint() {
   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);
-  //ESP_LOGD(TAG, "ahrs: yaw=%f, pitch=%f, roll=%f", ahrs_.getYaw(),
+  ESP_LOGD(TAG, "ahrs: yaw=%f, pitch=%f, roll=%f", yaw_, pitch_, roll_);
-  //         ahrs_.getPitch(), ahrs_.getRoll());
+  ESP_LOGD(TAG, "target angle: %f", angle_controller_.Setpoint());
 }
 
 void UGV::ReadComms() {
   comms->Lock();
   UpdateLocationFromGPS(*(comms->status.mutable_location()), inputs_.gps);
-  comms->status.set_yaw_angle(ahrs_.getYaw());
+  comms->status.set_yaw_angle(yaw_);
-  comms->status.set_pitch_angle(ahrs_.getPitch());
+  comms->status.set_pitch_angle(pitch_);
-  comms->status.set_roll_angle(ahrs_.getRoll());
+  comms->status.set_roll_angle(roll_);
   UGV_State comms_ugv_state = comms->status.state();
   if (comms_ugv_state != current_state_) {
     ESP_LOGI(TAG, "Comms updated UGV state to %d", comms_ugv_state);
@@ -135,12 +146,12 @@ void UGV::OnTick() {
   ReadComms();
   next_state_ = current_state_;
 
-  angle_controller_.Input(ahrs_.getYaw());
+  angle_controller_.Input(yaw_);
   float drive_power    = 0.;
   outputs_.left_motor  = 0.0;
   outputs_.right_motor = 0.0;
 
-  float pitch          = ahrs_.getRoll(); // TODO: fix quaternion -> YPR
+  float pitch          = roll_; // TODO: fix quaternion -> YPR
   auto  min_flip_pitch = conf_.min_flip_pitch();
   bool  is_upside_down = (pitch > min_flip_pitch) || (pitch < -min_flip_pitch);
 
@@ -217,6 +228,7 @@ void UGV::OnTick() {
       float tgt_bearing = current_pos.bearing_toward(target_);
       angle_controller_.Enable();
       angle_controller_.Setpoint(tgt_bearing);
+      drive_power = conf_.drive_power();
       break;
     }
     case UGV_State::STATE_TEST:

main/ugv.hh

@@ -43,6 +43,9 @@ class UGV {
   int64_t     last_print_;
   UGV_State   current_state_;
   UGV_State   next_state_;
+  float       yaw_;
+  float       pitch_;
+  float       roll_;
 
   void UpdateAHRS();
   void DoDebugPrint();

tools/config.yml

@@ -1,10 +1,12 @@
 REVISION: 1
 angle_pid:
   kp: 0.10
-  ki: 0.0
+  ki: 0.0001
   kd: 0.4
   max_output: 0.5
   max_i_error: 15.0
 
 min_target_dist: 10.0
 min_flip_pitch: 90.0
+drive_power: 0.3
+mag_declination: 11.5

tools/config_pb2.py

@@ -20,7 +20,7 @@ DESCRIPTOR = _descriptor.FileDescriptor(
   package='ugv.config',
   syntax='proto3',
   serialized_options=_b('H\003'),
-  serialized_pb=_b('\n\x0c\x63onfig.proto\x12\nugv.config\"X\n\tPidParams\x12\n\n\x02kp\x18\x01 \x01(\x02\x12\n\n\x02ki\x18\x02 \x01(\x02\x12\n\n\x02kd\x18\x03 \x01(\x02\x12\x12\n\nmax_output\x18\x04 \x01(\x02\x12\x13\n\x0bmax_i_error\x18\x05 \x01(\x02\"x\n\x06\x43onfig\x12\x17\n\x0fmin_target_dist\x18\x01 \x01(\x02\x12\x13\n\x0b\x64rive_power\x18\x02 \x01(\x02\x12(\n\tangle_pid\x18\x03 \x01(\x0b\x32\x15.ugv.config.PidParams\x12\x16\n\x0emin_flip_pitch\x18\x04 \x01(\x02\x42\x02H\x03\x62\x06proto3')
+  serialized_pb=_b('\n\x0c\x63onfig.proto\x12\nugv.config\"X\n\tPidParams\x12\n\n\x02kp\x18\x01 \x01(\x02\x12\n\n\x02ki\x18\x02 \x01(\x02\x12\n\n\x02kd\x18\x03 \x01(\x02\x12\x12\n\nmax_output\x18\x04 \x01(\x02\x12\x13\n\x0bmax_i_error\x18\x05 \x01(\x02\"\x91\x01\n\x06\x43onfig\x12\x17\n\x0fmin_target_dist\x18\x01 \x01(\x02\x12\x13\n\x0b\x64rive_power\x18\x02 \x01(\x02\x12(\n\tangle_pid\x18\x03 \x01(\x0b\x32\x15.ugv.config.PidParams\x12\x16\n\x0emin_flip_pitch\x18\x04 \x01(\x02\x12\x17\n\x0fmag_declination\x18\x05 \x01(\x02\x42\x02H\x03\x62\x06proto3')
 )
 
 
@@ -120,6 +120,13 @@ _CONFIG = _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='mag_declination', full_name='ugv.config.Config.mag_declination', index=4,
+      number=5, type=2, cpp_type=6, label=1,
+      has_default_value=False, default_value=float(0),
+      message_type=None, enum_type=None, containing_type=None,
+      is_extension=False, extension_scope=None,
+      serialized_options=None, file=DESCRIPTOR),
   ],
   extensions=[
   ],
@@ -132,8 +139,8 @@ _CONFIG = _descriptor.Descriptor(
   extension_ranges=[],
   oneofs=[
   ],
-  serialized_start=118,
+  serialized_start=119,
-  serialized_end=238,
+  serialized_end=264,
 )
 
 _CONFIG.fields_by_name['angle_pid'].message_type = _PIDPARAMS

tools/ugv_cmd.py

@@ -34,11 +34,14 @@ class UGV_CLI:
             'q': self.exit,
             'disable': self.disable,
             'd': self.disable,
-            'target': self.set_target,
+            'set_target': self.set_target,
-            'config': self.set_config,
+            'st': self.set_target,
-            'c': self.set_config,
+            'set_config': self.set_config,
+            'sc': self.set_config,
             'drive_heading': self.drive_heading,
+            'dh': self.drive_heading,
             'drive_to_target': self.drive_to_target,
+            'dt': self.drive_to_target,
             'get_status': self.get_status,
             's': self.get_status,
         }