diff --git a/main/config.proto b/main/config.proto index 9e266df..698905d 100644 --- a/main/config.proto +++ b/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; } diff --git a/main/ugv.cc b/main/ugv.cc index cd38722..887b885 100644 --- a/main/ugv.cc +++ b/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(), - // ahrs_.getPitch(), ahrs_.getRoll()); + ESP_LOGD(TAG, "ahrs: yaw=%f, pitch=%f, roll=%f", yaw_, pitch_, roll_); + 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_pitch_angle(ahrs_.getPitch()); - comms->status.set_roll_angle(ahrs_.getRoll()); + comms->status.set_yaw_angle(yaw_); + comms->status.set_pitch_angle(pitch_); + 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: diff --git a/main/ugv.hh b/main/ugv.hh index 643a7a9..7ae83ad 100644 --- a/main/ugv.hh +++ b/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(); diff --git a/tools/config.yml b/tools/config.yml index 10a6559..4ee890f 100644 --- a/tools/config.yml +++ b/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 diff --git a/tools/config_pb2.py b/tools/config_pb2.py index c02cff0..49140e0 100644 --- a/tools/config_pb2.py +++ b/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_end=238, + serialized_start=119, + serialized_end=264, ) _CONFIG.fields_by_name['angle_pid'].message_type = _PIDPARAMS diff --git a/tools/ugv_cmd.py b/tools/ugv_cmd.py index 53babfa..62a47a1 100755 --- a/tools/ugv_cmd.py +++ b/tools/ugv_cmd.py @@ -34,11 +34,14 @@ class UGV_CLI: 'q': self.exit, 'disable': self.disable, 'd': self.disable, - 'target': self.set_target, - 'config': self.set_config, - 'c': self.set_config, + 'set_target': self.set_target, + 'st': self.set_target, + '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, }