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

add stillness checking

Browse Source
This commit is contained in:
Alex Mikhalev 2019-05-23 20:03:40 -07:00
parent 068cd2d33b
commit 15f4080482
5 changed files with 82 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
main/messages.proto
View File

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

52
main/ugv.cc
View File

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

7
main/ugv.hh
View File

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

13
main/ugv_io_mpu.hh
View File

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

35
tools/messages_pb2.py
View File

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