|
|
|
|
@ -21,35 +21,29 @@ SemaphoreHandle_t i2c_mutex;
@@ -21,35 +21,29 @@ SemaphoreHandle_t i2c_mutex;
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
constexpr uint64_t LOOP_PERIOD_US = 1e6 / 100; |
|
|
|
|
constexpr float LOOP_PERIOD_S = 1000000.f / static_cast<float>(LOOP_PERIOD_US); |
|
|
|
|
static const float PI = |
|
|
|
|
3.1415926535897932384626433832795028841971693993751058209749445923078164062; |
|
|
|
|
|
|
|
|
|
extern "C" void OnTimeout(void *arg); |
|
|
|
|
|
|
|
|
|
void UpdateLocationFromGPS(comms::messages::Location &location, |
|
|
|
|
const io::GpsData & gps_data) { |
|
|
|
|
location.set_fix_quality(gps_data.fix_quality); |
|
|
|
|
location.set_latitude(gps_data.latitude); |
|
|
|
|
location.set_longitude(gps_data.longitude); |
|
|
|
|
location.set_altitude(gps_data.altitude); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
static const float RAD_PER_DEG = PI / 180.f; |
|
|
|
|
// Radius of earth in meters
|
|
|
|
|
static const float EARTH_RAD = 6372795.f; |
|
|
|
|
|
|
|
|
|
static const float DRIVE_POWER = 0.5; |
|
|
|
|
static const float ANGLE_P = 0.05; |
|
|
|
|
static const float MAX_ANGLE_POWER = 0.3; |
|
|
|
|
static const float ANGLE_I = 0.00; |
|
|
|
|
static const float ANGLE_D = -0.05; |
|
|
|
|
static const float MAX_ANGLE_POWER = 0.5; |
|
|
|
|
static const float MIN_DIST = 10.0; |
|
|
|
|
|
|
|
|
|
float clamp_mag(float x, float max_mag) { |
|
|
|
|
if (x > max_mag) |
|
|
|
|
return max_mag; |
|
|
|
|
else if (x < -max_mag) |
|
|
|
|
return -max_mag; |
|
|
|
|
else |
|
|
|
|
return x; |
|
|
|
|
extern "C" void OnTimeout(void *arg); |
|
|
|
|
|
|
|
|
|
void UpdateLocationFromGPS(comms::messages::Location &location, |
|
|
|
|
const io::GpsData & gps_data) { |
|
|
|
|
location.set_fix_quality(gps_data.fix_quality); |
|
|
|
|
location.set_latitude(gps_data.latitude); |
|
|
|
|
location.set_longitude(gps_data.longitude); |
|
|
|
|
location.set_altitude(gps_data.altitude); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
struct LatLong { |
|
|
|
|
@ -96,19 +90,134 @@ struct LatLong {
@@ -96,19 +90,134 @@ struct LatLong {
|
|
|
|
|
} |
|
|
|
|
}; |
|
|
|
|
|
|
|
|
|
class PIDController { |
|
|
|
|
public: |
|
|
|
|
explicit PIDController(float dt, float kp, float ki = 0., float kd = 0.); |
|
|
|
|
|
|
|
|
|
void MaxOutput(float max_output) { max_output_ = max_output; } |
|
|
|
|
float MaxOutput() const { return max_output_; } |
|
|
|
|
|
|
|
|
|
void Setpoint(float setpoint) { setpoint_ = setpoint; } |
|
|
|
|
float Setpoint() const { return setpoint_; } |
|
|
|
|
|
|
|
|
|
void Input(float input) { input_ = input; } |
|
|
|
|
float Input() const { return input_; }; |
|
|
|
|
|
|
|
|
|
float Error() const { |
|
|
|
|
float error = setpoint_ - input_; |
|
|
|
|
// TODO: have this be configurable
|
|
|
|
|
while (error < 180.f) error += 360.f; |
|
|
|
|
while (error > 180.f) error -= 360.f; |
|
|
|
|
return error; |
|
|
|
|
} |
|
|
|
|
float Output() const { return output_; }; |
|
|
|
|
|
|
|
|
|
float Update(); |
|
|
|
|
float Update(float input); |
|
|
|
|
float Update(float input, float setpoint); |
|
|
|
|
|
|
|
|
|
void Reset(); |
|
|
|
|
void Enable(bool enable = true) { enabled_ = enable; } |
|
|
|
|
void Disable() { Enable(false); } |
|
|
|
|
bool Enabled() const { return enabled_; } |
|
|
|
|
|
|
|
|
|
private: |
|
|
|
|
static float clamp_mag(float x, float mag); |
|
|
|
|
|
|
|
|
|
float dt_; |
|
|
|
|
float kp_; |
|
|
|
|
float ki_; |
|
|
|
|
float kd_; |
|
|
|
|
float max_output_; |
|
|
|
|
|
|
|
|
|
bool enabled_; |
|
|
|
|
float setpoint_; |
|
|
|
|
float input_; |
|
|
|
|
float output_; |
|
|
|
|
|
|
|
|
|
float integral_; |
|
|
|
|
float last_error_; |
|
|
|
|
}; |
|
|
|
|
|
|
|
|
|
float PIDController::clamp_mag(float x, float max_mag) { |
|
|
|
|
if (x > max_mag) |
|
|
|
|
return max_mag; |
|
|
|
|
else if (x < -max_mag) |
|
|
|
|
return -max_mag; |
|
|
|
|
else |
|
|
|
|
return x; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
PIDController::PIDController(float dt, float kp, float ki, float kd) |
|
|
|
|
: dt_(dt), |
|
|
|
|
kp_(kp), |
|
|
|
|
ki_(ki), |
|
|
|
|
kd_(kd), |
|
|
|
|
max_output_(INFINITY), |
|
|
|
|
enabled_(false), |
|
|
|
|
setpoint_(0), |
|
|
|
|
input_(0), |
|
|
|
|
output_(0), |
|
|
|
|
integral_(0), |
|
|
|
|
last_error_(0) {} |
|
|
|
|
|
|
|
|
|
void PIDController::Reset() { |
|
|
|
|
enabled_ = false; |
|
|
|
|
setpoint_ = 0.; |
|
|
|
|
input_ = 0.; |
|
|
|
|
output_ = 0.; |
|
|
|
|
integral_ = 0.; |
|
|
|
|
last_error_ = NAN; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
float PIDController::Update() { |
|
|
|
|
output_ = 0.; |
|
|
|
|
if (!enabled_) { |
|
|
|
|
return output_; |
|
|
|
|
} |
|
|
|
|
float error = Error(); |
|
|
|
|
|
|
|
|
|
integral_ += error * dt_; |
|
|
|
|
|
|
|
|
|
output_ += kp_ * error; |
|
|
|
|
output_ += ki_ * integral_; |
|
|
|
|
if (!isnan(last_error_)) { |
|
|
|
|
output_ += kd_ * (error - last_error_); |
|
|
|
|
} |
|
|
|
|
output_ = clamp_mag(output_, max_output_); |
|
|
|
|
|
|
|
|
|
last_error_ = error; |
|
|
|
|
return output_; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
float PIDController::Update(float input) { |
|
|
|
|
Input(input); |
|
|
|
|
return Update(); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
float PIDController::Update(float input, float setpoint) { |
|
|
|
|
Setpoint(setpoint); |
|
|
|
|
return Update(input); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
struct State { |
|
|
|
|
public: |
|
|
|
|
CommsClass * comms; |
|
|
|
|
IOClass * io; |
|
|
|
|
DisplayClass * display; |
|
|
|
|
esp_timer_handle_t timer_handle; |
|
|
|
|
|
|
|
|
|
io::Inputs inputs; |
|
|
|
|
io::Outputs outputs; |
|
|
|
|
int64_t last_print; |
|
|
|
|
Madgwick ahrs_; |
|
|
|
|
LatLong target; |
|
|
|
|
PIDController angle_controller_; |
|
|
|
|
|
|
|
|
|
State() : target{34.069022, -118.443067} { |
|
|
|
|
State() |
|
|
|
|
: target{34.069022, -118.443067}, |
|
|
|
|
angle_controller_(LOOP_PERIOD_S, ANGLE_P) { |
|
|
|
|
angle_controller_.MaxOutput(MAX_ANGLE_POWER); |
|
|
|
|
comms = new CommsClass(); |
|
|
|
|
io = new IOClass(); |
|
|
|
|
display = new DisplayClass(comms); |
|
|
|
|
@ -118,8 +227,7 @@ struct State {
@@ -118,8 +227,7 @@ struct State {
|
|
|
|
|
esp_timer_init(); |
|
|
|
|
i2c_mutex = xSemaphoreCreateMutex(); |
|
|
|
|
|
|
|
|
|
ahrs_.begin(1000000.f / |
|
|
|
|
static_cast<float>(LOOP_PERIOD_US)); // rough sample frequency
|
|
|
|
|
ahrs_.begin(LOOP_PERIOD_S); // rough sample frequency
|
|
|
|
|
|
|
|
|
|
io->Init(); |
|
|
|
|
comms->Init(); |
|
|
|
|
@ -164,23 +272,44 @@ struct State {
@@ -164,23 +272,44 @@ struct State {
|
|
|
|
|
comms->Unlock(); |
|
|
|
|
UGV_State next_state = ugv_state; |
|
|
|
|
|
|
|
|
|
angle_controller_.Input(ahrs_.getYaw()); |
|
|
|
|
float drive_power = 0.; |
|
|
|
|
outputs.left_motor = 0.0; |
|
|
|
|
outputs.right_motor = 0.0; |
|
|
|
|
|
|
|
|
|
switch (ugv_state) { |
|
|
|
|
default: |
|
|
|
|
ESP_LOGW(TAG, "unhandled state: %d", ugv_state); |
|
|
|
|
// fall through
|
|
|
|
|
case UGV_State::STATE_IDLE: |
|
|
|
|
case UGV_State::STATE_FINISHED: |
|
|
|
|
outputs.left_motor = 0.0; |
|
|
|
|
outputs.right_motor = 0.0; |
|
|
|
|
break; |
|
|
|
|
case UGV_State::STATE_FINISHED: angle_controller_.Disable(); break; |
|
|
|
|
case UGV_State::STATE_AQUIRING: { |
|
|
|
|
angle_controller_.Disable(); |
|
|
|
|
TickType_t current_tick = xTaskGetTickCount(); |
|
|
|
|
TickType_t ticks_since_gps = current_tick - inputs.gps.last_update; |
|
|
|
|
bool not_old = ticks_since_gps <= pdMS_TO_TICKS(2000); |
|
|
|
|
bool not_invalid = inputs.gps.fix_quality != io::GPS_FIX_INVALID; |
|
|
|
|
outputs.left_motor = 0.0; |
|
|
|
|
outputs.right_motor = 0.0; |
|
|
|
|
if (not_old && not_invalid) { |
|
|
|
|
next_state = UGV_State::STATE_TURNING; |
|
|
|
|
} |
|
|
|
|
break; |
|
|
|
|
} |
|
|
|
|
case UGV_State::STATE_FLIPPING: { |
|
|
|
|
angle_controller_.Disable(); |
|
|
|
|
break; |
|
|
|
|
} |
|
|
|
|
case UGV_State::STATE_TURNING: { |
|
|
|
|
if (inputs.gps.fix_quality == io::GPS_FIX_INVALID) { |
|
|
|
|
next_state = UGV_State::STATE_AQUIRING; |
|
|
|
|
break; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
LatLong current_pos = {inputs.gps.latitude, inputs.gps.longitude}; |
|
|
|
|
float tgt_bearing = current_pos.bearing_toward(target); |
|
|
|
|
angle_controller_.Enable(); |
|
|
|
|
angle_controller_.Setpoint(tgt_bearing); |
|
|
|
|
|
|
|
|
|
if (fabs(angle_controller_.Error()) <= 5.0) { |
|
|
|
|
next_state = UGV_State::STATE_DRIVING; |
|
|
|
|
} |
|
|
|
|
break; |
|
|
|
|
@ -201,15 +330,8 @@ struct State {
@@ -201,15 +330,8 @@ struct State {
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
float tgt_bearing = current_pos.bearing_toward(target); |
|
|
|
|
float cur_bearing = ahrs_.getYaw(); |
|
|
|
|
float angle_delta = tgt_bearing - cur_bearing; |
|
|
|
|
if (angle_delta < 180.f) angle_delta += 360.f; |
|
|
|
|
if (angle_delta > 180.f) angle_delta -= 360.f; |
|
|
|
|
float angle_pwr = clamp_mag(angle_delta * ANGLE_P, MAX_ANGLE_POWER); |
|
|
|
|
float power = DRIVE_POWER; |
|
|
|
|
|
|
|
|
|
outputs.left_motor = power - angle_pwr; |
|
|
|
|
outputs.right_motor = power + angle_pwr; |
|
|
|
|
angle_controller_.Enable(); |
|
|
|
|
angle_controller_.Setpoint(tgt_bearing); |
|
|
|
|
break; |
|
|
|
|
} |
|
|
|
|
case UGV_State::STATE_TEST: |
|
|
|
|
@ -217,19 +339,23 @@ struct State {
@@ -217,19 +339,23 @@ struct State {
|
|
|
|
|
outputs.left_motor = sinf(time_s * PI); |
|
|
|
|
outputs.right_motor = cosf(time_s * PI); |
|
|
|
|
#else |
|
|
|
|
float tgt_bearing = 90.0; |
|
|
|
|
float cur_bearing = ahrs_.getYaw(); |
|
|
|
|
float angle_delta = tgt_bearing - cur_bearing; |
|
|
|
|
if (angle_delta < 180.f) angle_delta += 360.f; |
|
|
|
|
if (angle_delta > 180.f) angle_delta -= 360.f; |
|
|
|
|
float angle_pwr = clamp_mag(angle_delta * ANGLE_P, MAX_ANGLE_POWER); |
|
|
|
|
float power = 0.0; |
|
|
|
|
|
|
|
|
|
outputs.left_motor = power - angle_pwr; |
|
|
|
|
outputs.right_motor = power + angle_pwr; |
|
|
|
|
angle_controller_.Enable(); |
|
|
|
|
angle_controller_.Setpoint(90.0); |
|
|
|
|
#endif |
|
|
|
|
break; |
|
|
|
|
case UGV_State::STATE_DRIVE_HEADING: |
|
|
|
|
angle_controller_.Enable(); |
|
|
|
|
angle_controller_.Setpoint(comms->drive_heading.heading()); |
|
|
|
|
drive_power = comms->drive_heading.power(); |
|
|
|
|
break; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
if (angle_controller_.Enabled()) { |
|
|
|
|
float angle_pwr = angle_controller_.Update(); |
|
|
|
|
outputs.left_motor = drive_power + angle_pwr; |
|
|
|
|
outputs.right_motor = drive_power - angle_pwr; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
io->WriteOutputs(outputs); |
|
|
|
|
|
|
|
|
|
comms->Lock(); |
|
|
|
|
|
