amikhalev/uas-ugv
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

config changes

Browse Source
This commit is contained in:
Alex Mikhalev 2019-05-23 12:05:30 -07:00
parent fefcd6e44f
commit 28a6d087d7
6 changed files with 42 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
main/config.proto
View File

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

26
main/ugv.cc
View File

@ -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:

3
main/ugv.hh
View File

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

4
tools/config.yml
View File

@ -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

13
tools/config_pb2.py
View File

@ -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

9
tools/ugv_cmd.py
View File

@ -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,
}