From 2bb1d17c7e312e6f60bcd6e8f1ac6698fe623060 Mon Sep 17 00:00:00 2001 From: Doug Weibel Date: Sun, 7 Oct 2012 14:46:26 -0600 Subject: [PATCH 1/4] Changes to the PID controller. Adds "limit" to the parameter set. Implements an output limit where the output magnitude is limited by the parameter value "limit". Also changes the integrator saturation such that the integrator is not updated (added to) if either updating it will cause the integrator values magnitude to exceed "intmax" or if the output magnitude would exceed "limit" with an updated integrator value. Arbitrary large limit values were hard coded into multirotor_attitude_control.c. These should be changed to parametric values or something sensible. This commit will temporarily break fixedwing_control.c. A following commit will repair it along with significant changes to the inner loop control. This commit has been tested to compile with fixedwing_control.c temporarily removed. No other testing has been completed. --- .../multirotor_attitude_control.c | 16 +-- apps/systemlib/pid/pid.c | 100 +++++++----------- apps/systemlib/pid/pid.h | 6 +- 3 files changed, 53 insertions(+), 69 deletions(-) diff --git a/apps/multirotor_att_control/multirotor_attitude_control.c b/apps/multirotor_att_control/multirotor_attitude_control.c index 2129915d12..cb34209b1c 100644 --- a/apps/multirotor_att_control/multirotor_attitude_control.c +++ b/apps/multirotor_att_control/multirotor_attitude_control.c @@ -212,13 +212,13 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s parameters_update(&h, &p); // pid_init(&yaw_pos_controller, p.yaw_p, p.yaw_i, p.yaw_d, p.yaw_awu, - // PID_MODE_DERIVATIV_SET, 154); + // 90.0f, PID_MODE_DERIVATIV_SET, 154); // Arbitrarily large limit added pid_init(&yaw_speed_controller, p.yawrate_p, p.yawrate_i, p.yawrate_d, p.yawrate_awu, - PID_MODE_DERIVATIV_SET); + 90.0f, PID_MODE_DERIVATIV_SET); // Arbitrarily large limit added pid_init(&pitch_controller, p.att_p, p.att_i, p.att_d, p.att_awu, - PID_MODE_DERIVATIV_SET); + 90.0f, PID_MODE_DERIVATIV_SET); // Arbitrarily large limit added pid_init(&roll_controller, p.att_p, p.att_i, p.att_d, p.att_awu, - PID_MODE_DERIVATIV_SET); + 90.0f, PID_MODE_DERIVATIV_SET); // Arbitrarily large limit added initialized = true; } @@ -228,10 +228,10 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s /* update parameters from storage */ parameters_update(&h, &p); /* apply parameters */ - // pid_set_parameters(&yaw_pos_controller, p.yaw_p, p.yaw_i, p.yaw_d, p.yaw_awu); - pid_set_parameters(&yaw_speed_controller, p.yawrate_p, p.yawrate_i, p.yawrate_d, p.yawrate_awu); - pid_set_parameters(&pitch_controller, p.att_p, p.att_i, p.att_d, p.att_awu); - pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, p.att_awu); + // pid_set_parameters(&yaw_pos_controller, p.yaw_p, p.yaw_i, p.yaw_d, p.yaw_awu, 90.0f); + pid_set_parameters(&yaw_speed_controller, p.yawrate_p, p.yawrate_i, p.yawrate_d, p.yawrate_awu, 90.0f); + pid_set_parameters(&pitch_controller, p.att_p, p.att_i, p.att_d, p.att_awu, 90.0f); + pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, p.att_awu, 90.0f); } /* calculate current control outputs */ diff --git a/apps/systemlib/pid/pid.c b/apps/systemlib/pid/pid.c index cff5e6bbe2..f9b50d030b 100644 --- a/apps/systemlib/pid/pid.c +++ b/apps/systemlib/pid/pid.c @@ -43,12 +43,13 @@ #include __EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, - uint8_t mode) + float limit, uint8_t mode) { pid->kp = kp; pid->ki = ki; pid->kd = kd; pid->intmax = intmax; + pid->limit = limit; pid->mode = mode; pid->count = 0; pid->saturated = 0; @@ -58,7 +59,7 @@ __EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, pid->error_previous = 0; pid->integral = 0; } -__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax) +__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax, float limit) { int ret = 0; @@ -85,8 +86,13 @@ __EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float } else { ret = 1; } + + if (isfinite(limit)) { + pid->limit = limit; + } else { + ret = 1; + } - // pid->limit = limit; return ret; } @@ -122,72 +128,48 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo float i, d; pid->sp = sp; + + // Calculated current error value float error = pid->sp - val; - - if (pid->saturated && (pid->integral * error > 0)) { - //Output is saturated and the integral would get bigger (positive or negative) - i = pid->integral; - - //Reset saturation. If we are still saturated this will be set again at output limit check. - pid->saturated = 0; - - } else { - i = pid->integral + (error * dt); - } - - // Anti-Windup. Needed if we don't use the saturation above. - if (pid->intmax != 0.0f) { - if (i > pid->intmax) { - pid->integral = pid->intmax; - - } else if (i < -pid->intmax) { - - pid->integral = -pid->intmax; - - } else { - pid->integral = i; - } - } - - if (pid->mode == PID_MODE_DERIVATIV_CALC) { - d = (error - pid->error_previous) / dt; - - } else if (pid->mode == PID_MODE_DERIVATIV_SET) { - d = -val_dot; - - } else { - d = 0.0f; - } - - if (pid->kd == 0.0f) { - d = 0.0f; - } - - if (pid->ki == 0.0f) { - i = 0; - } - - float p; - - if (pid->kp == 0.0f) { - p = 0.0f; - } else { - p = error; - } - - if (isfinite(error)) { + + if (isfinite(error)) { // Why is this necessary? DEW pid->error_previous = error; } + // Calculate or measured current error derivative + + if (pid->mode == PID_MODE_DERIVATIV_CALC) { + d = (error - pid->error_previous) / dt; + } else if (pid->mode == PID_MODE_DERIVATIV_SET) { + d = -val_dot; + } else { + d = 0.0f; + } + + // Calculate the error integral and check for saturation + i = pid->integral + (error * dt); + if( fabs((error * pid->kp) + (i * pid->ki) + (d * pid->kd)) > pid->limit || + fabs(i) > pid->intmax ) + { + i = pid->integral; // If saturated then do not update integral value + pid->saturated = 1; + } else { + if (!isfinite(i)) { + i = 0; + } + pid->integral = i; + pid->saturated = 0; + } + + // Calculate the output. Limit output magnitude to pid->limit float output = (pid->error_previous * pid->kp) + (i * pid->ki) + (d * pid->kd); + if (output > pid->limit) output = pid->limit; + if (output < -pid->limit) output = -pid->limit; if (isfinite(output)) { pid->last_output = output; } - if (!isfinite(pid->integral)) { - pid->integral = 0; - } return pid->last_output; } diff --git a/apps/systemlib/pid/pid.h b/apps/systemlib/pid/pid.h index 098b2bef81..b51afef9ef 100644 --- a/apps/systemlib/pid/pid.h +++ b/apps/systemlib/pid/pid.h @@ -49,6 +49,8 @@ #define PID_MODE_DERIVATIV_CALC 0 /* Use PID_MODE_DERIVATIV_SET if you have the derivative already (Gyros, Kalman) */ #define PID_MODE_DERIVATIV_SET 1 +// Use PID_MODE_DERIVATIV_NONE for a PI controller (vs PID) +#define PID_MODE_DERIVATIV_NONE 9 typedef struct { float kp; @@ -65,8 +67,8 @@ typedef struct { uint8_t saturated; } PID_t; -__EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, uint8_t mode); -__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax); +__EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, float limit, uint8_t mode); +__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax, float limit); //void pid_set(PID_t *pid, float sp); __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt); From 4fea0a3fc15346efc366aadf1d80697664b6f3f0 Mon Sep 17 00:00:00 2001 From: Doug Weibel Date: Sun, 7 Oct 2012 14:50:07 -0600 Subject: [PATCH 2/4] This commit changes the inner loop control structures for fixed wing attitude control. Nested rate and angle loops are used with limits on both the rate setpoint A simple outer navigation loop is retained for navigation control. This will be replaced later. The pitch set point is hard coded to zero. Pitch stabilization should work. This commit compiles, but needs further testing. --- apps/fixedwing_control/fixedwing_control.c | 160 ++++++++++++++++----- 1 file changed, 121 insertions(+), 39 deletions(-) diff --git a/apps/fixedwing_control/fixedwing_control.c b/apps/fixedwing_control/fixedwing_control.c index ad08247e10..a693971c97 100644 --- a/apps/fixedwing_control/fixedwing_control.c +++ b/apps/fixedwing_control/fixedwing_control.c @@ -2,6 +2,7 @@ * * Copyright (C) 2012 PX4 Development Team. All rights reserved. * Author: @author Ivan Ovinnikov + * Modifications: Doug Weibel * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -31,7 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. * ****************************************************************************/ -// Workflow test comment - DEW + /** * @file fixedwing_control.c * Implementation of a fixed wing attitude and position controller. @@ -86,27 +87,61 @@ static void usage(const char *reason); * Controller parameters, accessible via MAVLink * */ -PARAM_DEFINE_FLOAT(FW_ROLL_POS_P, 0.3f); -PARAM_DEFINE_FLOAT(FW_ROLL_POS_I, 0.002f); -PARAM_DEFINE_FLOAT(FW_ROLL_POS_AWU, 0.2f); -PARAM_DEFINE_FLOAT(FW_ROLL_POS_LIM, 0.6f); +// Roll control parameters +PARAM_DEFINE_FLOAT(FW_ROLL_RATE_P, 0.3f); +// Need to add functionality to suppress integrator windup while on the ground +// Suggested value of FW_ROLL_RATE_I is 0.0 till this is in place +PARAM_DEFINE_FLOAT(FW_ROLL_RATE_I, 0.0f); +PARAM_DEFINE_FLOAT(FW_ROLL_RATE_AWU, 0.0f); +PARAM_DEFINE_FLOAT(FW_ROLL_RATE_LIMIT, 0.7f); // Roll rate limit in radians/sec +PARAM_DEFINE_FLOAT(FW_ROLL_ANGLE_P, 0.3f); +PARAM_DEFINE_FLOAT(FW_ROLL_ANGLE_LIMIT, 0.7f); // Roll angle limit in radians + +//Pitch control parameters +PARAM_DEFINE_FLOAT(FW_PITCH_RATE_P, 0.3f); +// Need to add functionality to suppress integrator windup while on the ground +// Suggested value of FW_PITCH_RATE_I is 0.0 till this is in place +PARAM_DEFINE_FLOAT(FW_PITCH_RATE_I, 0.0f); +PARAM_DEFINE_FLOAT(FW_PITCH_RATE_AWU, 0.0f); +PARAM_DEFINE_FLOAT(FW_PITCH_RATE_LIMIT, 0.35f); // Pitch rate limit in radians/sec +PARAM_DEFINE_FLOAT(FW_PITCH_ANGLE_P, 0.3f); +PARAM_DEFINE_FLOAT(FW_PITCH_ANGLE_LIMIT, 0.35f); // Pitch angle limit in radians struct fw_att_control_params { - float roll_pos_p; - float roll_pos_i; - float roll_pos_awu; - float roll_pos_lim; + float roll_rate_p; + float roll_rate_i; + float roll_rate_awu; + float roll_rate_limit; + float roll_angle_p; + float roll_angle_limit; + float pitch_rate_p; + float pitch_rate_i; + float pitch_rate_awu; + float pitch_rate_limit; + float pitch_angle_p; + float pitch_angle_limit; }; struct fw_att_control_param_handles { - param_t roll_pos_p; - param_t roll_pos_i; - param_t roll_pos_awu; - param_t roll_pos_lim; + param_t roll_rate_p; + param_t roll_rate_i; + param_t roll_rate_awu; + param_t roll_rate_limit; + param_t roll_angle_p; + param_t roll_angle_limit; + param_t pitch_rate_p; + param_t pitch_rate_i; + param_t pitch_rate_awu; + param_t pitch_rate_limit; + param_t pitch_angle_p; + param_t pitch_angle_limit; }; -// XXX outsource position control to a separate app some time +// TO_DO - Navigation control will be moved to a separate app +// Attitude control will just handle the inner angle and rate loops +// to control pitch and roll, and turn coordination via rudder and +// possibly throttle compensation for battery voltage sag. PARAM_DEFINE_FLOAT(FW_HEADING_P, 0.1f); PARAM_DEFINE_FLOAT(FW_HEADING_LIM, 0.15f); @@ -147,9 +182,9 @@ static int pos_parameters_update(const struct fw_pos_control_param_handles *h, s /** - * The fixed wing control main loop. + * The fixed wing control main thread. * - * This loop executes continously and calculates the control + * The main loop executes continously and calculates the control * response. * * @param argc number of arguments @@ -223,19 +258,31 @@ int fixedwing_control_thread_main(int argc, char *argv[]) pos_parameters_init(&hpos); pos_parameters_update(&hpos, &ppos); - PID_t roll_pos_controller; - pid_init(&roll_pos_controller, p.roll_pos_p, p.roll_pos_i, 0.0f, p.roll_pos_awu, - PID_MODE_DERIVATIV_SET); +// TO_DO Fix output limit functionallity of PID controller or add that function elsewhere + PID_t roll_rate_controller; + pid_init(&roll_rate_controller, p.roll_rate_p, p.roll_rate_i, 0.0f, p.roll_rate_awu, + p.roll_rate_limit,PID_MODE_DERIVATIV_NONE); + PID_t roll_angle_controller; + pid_init(&roll_angle_controller, p.roll_angle_p, 0.0f, 0.0f, 0.0f, + p.roll_angle_limit,PID_MODE_DERIVATIV_NONE); + + PID_t pitch_rate_controller; + pid_init(&pitch_rate_controller, p.pitch_rate_p, p.pitch_rate_i, 0.0f, p.pitch_rate_awu, + p.pitch_rate_limit,PID_MODE_DERIVATIV_NONE); + PID_t pitch_angle_controller; + pid_init(&pitch_angle_controller, p.pitch_angle_p, 0.0f, 0.0f, 0.0f, + p.pitch_angle_limit,PID_MODE_DERIVATIV_NONE); PID_t heading_controller; pid_init(&heading_controller, ppos.heading_p, 0.0f, 0.0f, 0.0f, - PID_MODE_DERIVATIV_SET); + 100.0f,PID_MODE_DERIVATIV_SET); // Temporary arbitrarily large limit // XXX remove in production /* advertise debug value */ struct debug_key_value_s dbg = { .key = "", .value = 0.0f }; orb_advert_t pub_dbg = orb_advertise(ORB_ID(debug_key_value), &dbg); + // This is the top of the main loop while(!thread_should_exit) { struct pollfd fds[1] = { @@ -254,8 +301,15 @@ int fixedwing_control_thread_main(int argc, char *argv[]) if (loopcounter % 20 == 0) { att_parameters_update(&h, &p); pos_parameters_update(&hpos, &ppos); - pid_set_parameters(&roll_pos_controller, p.roll_pos_p, p.roll_pos_i, 0.0f, p.roll_pos_awu); - pid_set_parameters(&heading_controller, ppos.heading_p, 0.0f, 0.0f, 0.0f); + pid_set_parameters(&roll_rate_controller, p.roll_rate_p, p.roll_rate_i, 0.0f, + p.roll_rate_awu, p.roll_rate_limit); + pid_set_parameters(&roll_angle_controller, p.roll_angle_p, 0.0f, 0.0f, + 0.0f, p.roll_angle_limit); + pid_set_parameters(&pitch_rate_controller, p.pitch_rate_p, p.pitch_rate_i, 0.0f, + p.pitch_rate_awu, p.pitch_rate_limit); + pid_set_parameters(&pitch_angle_controller, p.pitch_angle_p, 0.0f, 0.0f, + 0.0f, p.pitch_angle_limit); + pid_set_parameters(&heading_controller, ppos.heading_p, 0.0f, 0.0f, 0.0f, 90.0f); } /* if position updated, run position control loop */ @@ -359,14 +413,25 @@ int fixedwing_control_thread_main(int argc, char *argv[]) printf("[fixedwing control] roll sp: %8.4f, \n", att_sp.roll_body); } - // actuators.control[0] = pid_calculate(&roll_pos_controller, att_sp.roll_body, - // att.roll, att.rollspeed, deltaTpos); - actuators.control[0] = pid_calculate(&roll_pos_controller, att_sp.roll_body, - att.roll, att.rollspeed, deltaTpos); - actuators.control[1] = 0; - actuators.control[2] = 0; + // actuator control[0] is aileron (or elevon roll control) + // Commanded roll rate from P controller on roll angle + float roll_rate_command = pid_calculate(&roll_angle_controller, att_sp.roll_body, + att.roll, 0.0f, deltaTpos); + // actuator control from PI controller on roll rate + actuators.control[0] = pid_calculate(&roll_rate_controller, roll_rate_command, + att.rollspeed, 0.0f, deltaTpos); + + // actuator control[1] is elevator (or elevon pitch control) + // Commanded pitch rate from P controller on pitch angle + float pitch_rate_command = pid_calculate(&pitch_angle_controller, att_sp.pitch_body, + att.pitch, 0.0f, deltaTpos); + // actuator control from PI controller on pitch rate + actuators.control[1] = pid_calculate(&pitch_rate_controller, pitch_rate_command, + att.pitchspeed, 0.0f, deltaTpos); + + // actuator control[3] is throttle actuators.control[3] = att_sp.thrust; - + /* publish attitude setpoint (for MAVLink) */ orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); @@ -446,21 +511,38 @@ int fixedwing_control_main(int argc, char *argv[]) static int att_parameters_init(struct fw_att_control_param_handles *h) { /* PID parameters */ - h->roll_pos_p = param_find("FW_ROLL_POS_P"); - h->roll_pos_i = param_find("FW_ROLL_POS_I"); - h->roll_pos_lim = param_find("FW_ROLL_POS_LIM"); - h->roll_pos_awu = param_find("FW_ROLL_POS_AWU"); - + + h->roll_rate_p = param_find("FW_ROLL_RATE_P"); + h->roll_rate_i = param_find(";FW_ROLL_RATE_I"); + h->roll_rate_awu = param_find("FW_ROLL_RATE_AWU"); + h->roll_rate_limit = param_find("FW_ROLL_RATE_LIMIT"); + h->roll_angle_p = param_find("FW_ROLL_ANGLE_P"); + h->roll_angle_limit = param_find("FW_ROLL_ANGLE_LIMIT"); + h->pitch_rate_p = param_find("FW_PITCH_RATE_P"); + h->pitch_rate_i = param_find("FW_PITCH_RATE_I"); + h->pitch_rate_awu = param_find("FW_PITCH_RATE_AWU"); + h->pitch_rate_limit = param_find("FW_PITCH_RATE_LIMIT"); + h->pitch_angle_p = param_find("FW_PITCH_ANGLE_P"); + h->pitch_angle_p = param_find("FW_PITCH_ANGLE_LIMIT"); + return OK; } static int att_parameters_update(const struct fw_att_control_param_handles *h, struct fw_att_control_params *p) { - param_get(h->roll_pos_p, &(p->roll_pos_p)); - param_get(h->roll_pos_i, &(p->roll_pos_i)); - param_get(h->roll_pos_lim, &(p->roll_pos_lim)); - param_get(h->roll_pos_awu, &(p->roll_pos_awu)); - + param_get(h->roll_rate_p, &(p->roll_rate_p)); + param_get(h->roll_rate_i, &(p->roll_rate_i)); + param_get(h->roll_rate_awu, &(p->roll_rate_awu)); + param_get(h->roll_rate_limit, &(p->roll_rate_limit)); + param_get(h->roll_angle_p, &(p->roll_angle_p)); + param_get(h->roll_angle_limit, &(p->roll_angle_limit)); + param_get(h->pitch_rate_p, &(p->pitch_rate_p)); + param_get(h->pitch_rate_i, &(p->pitch_rate_i)); + param_get(h->pitch_rate_awu, &(p->pitch_rate_awu)); + param_get(h->pitch_rate_limit, &(p->pitch_rate_limit)); + param_get(h->pitch_angle_p, &(p->pitch_angle_p)); + param_get(h->pitch_angle_limit, &(p->pitch_angle_limit)); + return OK; } From 77e6375920df67344e895e669dd2a641a7b87b6e Mon Sep 17 00:00:00 2001 From: Doug Weibel Date: Mon, 8 Oct 2012 14:14:43 -0600 Subject: [PATCH 3/4] Change parameter names in FW control to fit within MAVLink parameter name size limit --- apps/fixedwing_control/fixedwing_control.c | 160 +++++++++++---------- 1 file changed, 81 insertions(+), 79 deletions(-) diff --git a/apps/fixedwing_control/fixedwing_control.c b/apps/fixedwing_control/fixedwing_control.c index a693971c97..8629c2f110 100644 --- a/apps/fixedwing_control/fixedwing_control.c +++ b/apps/fixedwing_control/fixedwing_control.c @@ -88,53 +88,53 @@ static void usage(const char *reason); * */ // Roll control parameters -PARAM_DEFINE_FLOAT(FW_ROLL_RATE_P, 0.3f); +PARAM_DEFINE_FLOAT(FW_ROLLRATE_P, 0.3f); // Need to add functionality to suppress integrator windup while on the ground -// Suggested value of FW_ROLL_RATE_I is 0.0 till this is in place -PARAM_DEFINE_FLOAT(FW_ROLL_RATE_I, 0.0f); -PARAM_DEFINE_FLOAT(FW_ROLL_RATE_AWU, 0.0f); -PARAM_DEFINE_FLOAT(FW_ROLL_RATE_LIMIT, 0.7f); // Roll rate limit in radians/sec -PARAM_DEFINE_FLOAT(FW_ROLL_ANGLE_P, 0.3f); -PARAM_DEFINE_FLOAT(FW_ROLL_ANGLE_LIMIT, 0.7f); // Roll angle limit in radians +// Suggested value of FW_ROLLRATE_I is 0.0 till this is in place +PARAM_DEFINE_FLOAT(FW_ROLLRATE_I, 0.0f); +PARAM_DEFINE_FLOAT(FW_ROLLRATE_AWU, 0.0f); +PARAM_DEFINE_FLOAT(FW_ROLLRATE_LIM, 0.7f); // Roll rate limit in radians/sec +PARAM_DEFINE_FLOAT(FW_ROLL_P, 0.3f); +PARAM_DEFINE_FLOAT(FW_ROLL_LIM, 0.7f); // Roll angle limit in radians //Pitch control parameters -PARAM_DEFINE_FLOAT(FW_PITCH_RATE_P, 0.3f); +PARAM_DEFINE_FLOAT(FW_PITCHRATE_P, 0.3f); // Need to add functionality to suppress integrator windup while on the ground -// Suggested value of FW_PITCH_RATE_I is 0.0 till this is in place -PARAM_DEFINE_FLOAT(FW_PITCH_RATE_I, 0.0f); -PARAM_DEFINE_FLOAT(FW_PITCH_RATE_AWU, 0.0f); -PARAM_DEFINE_FLOAT(FW_PITCH_RATE_LIMIT, 0.35f); // Pitch rate limit in radians/sec -PARAM_DEFINE_FLOAT(FW_PITCH_ANGLE_P, 0.3f); -PARAM_DEFINE_FLOAT(FW_PITCH_ANGLE_LIMIT, 0.35f); // Pitch angle limit in radians +// Suggested value of FW_PITCHRATE_I is 0.0 till this is in place +PARAM_DEFINE_FLOAT(FW_PITCHRATE_I, 0.0f); +PARAM_DEFINE_FLOAT(FW_PITCHRATE_AWU, 0.0f); +PARAM_DEFINE_FLOAT(FW_PITCHRATE_LIM, 0.35f); // Pitch rate limit in radians/sec +PARAM_DEFINE_FLOAT(FW_PITCH_P, 0.3f); +PARAM_DEFINE_FLOAT(FW_PITCH_LIM, 0.35f); // Pitch angle limit in radians struct fw_att_control_params { - float roll_rate_p; - float roll_rate_i; - float roll_rate_awu; - float roll_rate_limit; - float roll_angle_p; - float roll_angle_limit; - float pitch_rate_p; - float pitch_rate_i; - float pitch_rate_awu; - float pitch_rate_limit; - float pitch_angle_p; - float pitch_angle_limit; + float rollrate_p; + float rollrate_i; + float rollrate_awu; + float rollrate_lim; + float roll_p; + float roll_lim; + float pitchrate_p; + float pitchrate_i; + float pitchrate_awu; + float pitchrate_lim; + float pitch_p; + float pitch_lim; }; struct fw_att_control_param_handles { - param_t roll_rate_p; - param_t roll_rate_i; - param_t roll_rate_awu; - param_t roll_rate_limit; - param_t roll_angle_p; - param_t roll_angle_limit; - param_t pitch_rate_p; - param_t pitch_rate_i; - param_t pitch_rate_awu; - param_t pitch_rate_limit; - param_t pitch_angle_p; - param_t pitch_angle_limit; + param_t rollrate_p; + param_t rollrate_i; + param_t rollrate_awu; + param_t rollrate_lim; + param_t roll_p; + param_t roll_lim; + param_t pitchrate_p; + param_t pitchrate_i; + param_t pitchrate_awu; + param_t pitchrate_lim; + param_t pitch_p; + param_t pitch_lim; }; @@ -260,18 +260,18 @@ int fixedwing_control_thread_main(int argc, char *argv[]) // TO_DO Fix output limit functionallity of PID controller or add that function elsewhere PID_t roll_rate_controller; - pid_init(&roll_rate_controller, p.roll_rate_p, p.roll_rate_i, 0.0f, p.roll_rate_awu, - p.roll_rate_limit,PID_MODE_DERIVATIV_NONE); + pid_init(&roll_rate_controller, p.rollrate_p, p.rollrate_i, 0.0f, p.rollrate_awu, + p.rollrate_lim,PID_MODE_DERIVATIV_NONE); PID_t roll_angle_controller; - pid_init(&roll_angle_controller, p.roll_angle_p, 0.0f, 0.0f, 0.0f, - p.roll_angle_limit,PID_MODE_DERIVATIV_NONE); + pid_init(&roll_angle_controller, p.roll_p, 0.0f, 0.0f, 0.0f, + p.roll_lim,PID_MODE_DERIVATIV_NONE); PID_t pitch_rate_controller; - pid_init(&pitch_rate_controller, p.pitch_rate_p, p.pitch_rate_i, 0.0f, p.pitch_rate_awu, - p.pitch_rate_limit,PID_MODE_DERIVATIV_NONE); + pid_init(&pitch_rate_controller, p.pitchrate_p, p.pitchrate_i, 0.0f, p.pitchrate_awu, + p.pitchrate_lim,PID_MODE_DERIVATIV_NONE); PID_t pitch_angle_controller; - pid_init(&pitch_angle_controller, p.pitch_angle_p, 0.0f, 0.0f, 0.0f, - p.pitch_angle_limit,PID_MODE_DERIVATIV_NONE); + pid_init(&pitch_angle_controller, p.pitch_p, 0.0f, 0.0f, 0.0f, + p.pitch_lim,PID_MODE_DERIVATIV_NONE); PID_t heading_controller; pid_init(&heading_controller, ppos.heading_p, 0.0f, 0.0f, 0.0f, @@ -298,18 +298,20 @@ int fixedwing_control_thread_main(int argc, char *argv[]) } else { // FIXME SUBSCRIBE - if (loopcounter % 20 == 0) { + if (loopcounter % 100 == 0) { att_parameters_update(&h, &p); pos_parameters_update(&hpos, &ppos); - pid_set_parameters(&roll_rate_controller, p.roll_rate_p, p.roll_rate_i, 0.0f, - p.roll_rate_awu, p.roll_rate_limit); - pid_set_parameters(&roll_angle_controller, p.roll_angle_p, 0.0f, 0.0f, - 0.0f, p.roll_angle_limit); - pid_set_parameters(&pitch_rate_controller, p.pitch_rate_p, p.pitch_rate_i, 0.0f, - p.pitch_rate_awu, p.pitch_rate_limit); - pid_set_parameters(&pitch_angle_controller, p.pitch_angle_p, 0.0f, 0.0f, - 0.0f, p.pitch_angle_limit); + pid_set_parameters(&roll_rate_controller, p.rollrate_p, p.rollrate_i, 0.0f, + p.rollrate_awu, p.rollrate_lim); + pid_set_parameters(&roll_angle_controller, p.roll_p, 0.0f, 0.0f, + 0.0f, p.roll_lim); + pid_set_parameters(&pitch_rate_controller, p.pitchrate_p, p.pitchrate_i, 0.0f, + p.pitchrate_awu, p.pitchrate_lim); + pid_set_parameters(&pitch_angle_controller, p.pitch_p, 0.0f, 0.0f, + 0.0f, p.pitch_lim); pid_set_parameters(&heading_controller, ppos.heading_p, 0.0f, 0.0f, 0.0f, 90.0f); +//printf("[fixedwing control debug] p: %8.4f, i: %8.4f, limit: %8.4f \n", +//p.rollrate_p, p.rollrate_i, p.rollrate_lim); } /* if position updated, run position control loop */ @@ -512,36 +514,36 @@ static int att_parameters_init(struct fw_att_control_param_handles *h) { /* PID parameters */ - h->roll_rate_p = param_find("FW_ROLL_RATE_P"); - h->roll_rate_i = param_find(";FW_ROLL_RATE_I"); - h->roll_rate_awu = param_find("FW_ROLL_RATE_AWU"); - h->roll_rate_limit = param_find("FW_ROLL_RATE_LIMIT"); - h->roll_angle_p = param_find("FW_ROLL_ANGLE_P"); - h->roll_angle_limit = param_find("FW_ROLL_ANGLE_LIMIT"); - h->pitch_rate_p = param_find("FW_PITCH_RATE_P"); - h->pitch_rate_i = param_find("FW_PITCH_RATE_I"); - h->pitch_rate_awu = param_find("FW_PITCH_RATE_AWU"); - h->pitch_rate_limit = param_find("FW_PITCH_RATE_LIMIT"); - h->pitch_angle_p = param_find("FW_PITCH_ANGLE_P"); - h->pitch_angle_p = param_find("FW_PITCH_ANGLE_LIMIT"); + h->rollrate_p = param_find("FW_ROLLRATE_P"); + h->rollrate_i = param_find("FW_ROLLRATE_I"); + h->rollrate_awu = param_find("FW_ROLLRATE_AWU"); + h->rollrate_lim = param_find("FW_ROLLRATE_LIM"); + h->roll_p = param_find("FW_ROLL_P"); + h->roll_lim = param_find("FW_ROLL_LIM"); + h->pitchrate_p = param_find("FW_PITCHRATE_P"); + h->pitchrate_i = param_find("FW_PITCHRATE_I"); + h->pitchrate_awu = param_find("FW_PITCHRATE_AWU"); + h->pitchrate_lim = param_find("FW_PITCHRATE_LIM"); + h->pitch_p = param_find("FW_PITCH_P"); + h->pitch_lim = param_find("FW_PITCH_LIM"); return OK; } static int att_parameters_update(const struct fw_att_control_param_handles *h, struct fw_att_control_params *p) { - param_get(h->roll_rate_p, &(p->roll_rate_p)); - param_get(h->roll_rate_i, &(p->roll_rate_i)); - param_get(h->roll_rate_awu, &(p->roll_rate_awu)); - param_get(h->roll_rate_limit, &(p->roll_rate_limit)); - param_get(h->roll_angle_p, &(p->roll_angle_p)); - param_get(h->roll_angle_limit, &(p->roll_angle_limit)); - param_get(h->pitch_rate_p, &(p->pitch_rate_p)); - param_get(h->pitch_rate_i, &(p->pitch_rate_i)); - param_get(h->pitch_rate_awu, &(p->pitch_rate_awu)); - param_get(h->pitch_rate_limit, &(p->pitch_rate_limit)); - param_get(h->pitch_angle_p, &(p->pitch_angle_p)); - param_get(h->pitch_angle_limit, &(p->pitch_angle_limit)); + param_get(h->rollrate_p, &(p->rollrate_p)); + param_get(h->rollrate_i, &(p->rollrate_i)); + param_get(h->rollrate_awu, &(p->rollrate_awu)); + param_get(h->rollrate_lim, &(p->rollrate_lim)); + param_get(h->roll_p, &(p->roll_p)); + param_get(h->roll_lim, &(p->roll_lim)); + param_get(h->pitchrate_p, &(p->pitchrate_p)); + param_get(h->pitchrate_i, &(p->pitchrate_i)); + param_get(h->pitchrate_awu, &(p->pitchrate_awu)); + param_get(h->pitchrate_lim, &(p->pitchrate_lim)); + param_get(h->pitch_p, &(p->pitch_p)); + param_get(h->pitch_lim, &(p->pitch_lim)); return OK; } From 1e0a34a10211dbe9894540a45dcbe428d5ae09bd Mon Sep 17 00:00:00 2001 From: Doug Weibel Date: Sun, 21 Oct 2012 16:39:53 -0600 Subject: [PATCH 4/4] Add functions for computation of the distance and bearing to the nearest point of a line segment or arc segment. --- apps/systemlib/geo/geo.c | 206 ++++++++++++++++++++++++++++++++-- apps/systemlib/geo/geo.h | 23 ++++ nuttx/arch/arm/include/math.h | 4 + 3 files changed, 221 insertions(+), 12 deletions(-) diff --git a/apps/systemlib/geo/geo.c b/apps/systemlib/geo/geo.c index ce46d01cc8..bc467bfa3b 100644 --- a/apps/systemlib/geo/geo.c +++ b/apps/systemlib/geo/geo.c @@ -68,10 +68,10 @@ __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, dou __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next) { - double lat_now_rad = lat_now / 180.0 * M_PI; - double lon_now_rad = lon_now / 180.0 * M_PI; - double lat_next_rad = lat_next / 180.0 * M_PI; - double lon_next_rad = lon_next / 180.0 * M_PI; + double lat_now_rad = lat_now * M_DEG_TO_RAD; + double lon_now_rad = lon_now * M_DEG_TO_RAD; + double lat_next_rad = lat_next * M_DEG_TO_RAD; + double lon_next_rad = lon_next * M_DEG_TO_RAD; double d_lat = lat_next_rad - lat_now_rad; double d_lon = lon_next_rad - lon_now_rad; @@ -79,13 +79,195 @@ __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, doub /* conscious mix of double and float trig function to maximize speed and efficiency */ float theta = atan2f(sin(d_lon) * cos(lat_next_rad) , cos(lat_now_rad) * sin(lat_next_rad) - sin(lat_now_rad) * cos(lat_next_rad) * cos(d_lon)); - if (theta < M_PI_F) { - theta += 2.0f * M_PI_F; - } - - if (theta > M_PI_F) { - theta -= 2.0f * M_PI_F; - } + theta = _wrapPI(theta); return theta; -} \ No newline at end of file +} + +// Additional functions - @author Doug Weibel + +__EXPORT crosstrack_error_s get_distance_to_line(double lat_now, double lon_now, double lat_start, double lon_start, double lat_end, double lon_end) +{ +// This function returns the distance to the nearest point on the track line. Distance is positive if current +// position is right of the track and negative if left of the track as seen from a point on the track line +// headed towards the end point. + + crosstrack_error_s return_var; + float dist_to_end; + float bearing_end; + float bearing_track; + float bearing_diff; + + return_var.error = true; // Set error flag, cleared when valid result calculated. + return_var.past_end = false; + return_var.distance = 0.0f; + return_var.bearing = 0.0f; + + // Return error if arguments are bad + if(lat_now == 0.0d || lon_now == 0.0d || lat_start == 0.0d || lon_start == 0.0d || lat_end == 0.0d || lon_end == 0.0d) return return_var; + + bearing_end = get_bearing_to_next_waypoint(lat_now, lon_now, lat_end, lon_end); + bearing_track = get_bearing_to_next_waypoint(lat_start, lon_start, lat_end, lon_end); + bearing_diff = bearing_track - bearing_end; + bearing_diff = _wrapPI(bearing_diff); + + // Return past_end = true if past end point of line + if(bearing_diff > M_PI_2_F || bearing_diff < -M_PI_2_F) { + return_var.past_end = true; + return_var.error = false; + return return_var; + } + + dist_to_end = get_distance_to_next_waypoint(lat_now, lon_now, lat_end, lon_end); + return_var.distance = (dist_to_end)*sin(bearing_diff); + if(sin(bearing_diff) >=0 ) { + return_var.bearing = _wrapPI(bearing_track - M_PI_2_F); + } else { + return_var.bearing = _wrapPI(bearing_track + M_PI_2_F); + } + return_var.error = false; + + return return_var; + +} + + +__EXPORT crosstrack_error_s get_distance_to_arc(double lat_now, double lon_now, double lat_center, double lon_center, + float radius, float arc_start_bearing, float arc_sweep) +{ + // This function returns the distance to the nearest point on the track arc. Distance is positive if current + // position is right of the arc and negative if left of the arc as seen from the closest point on the arc and + // headed towards the end point. + crosstrack_error_s return_var; + + // Determine if the current position is inside or outside the sector between the line from the center + // to the arc start and the line from the center to the arc end + float bearing_sector_start; + float bearing_sector_end; + float bearing_now = get_bearing_to_next_waypoint(lat_now, lon_now, lat_center, lon_center); + bool in_sector; + + return_var.error = true; // Set error flag, cleared when valid result calculated. + return_var.past_end = false; + return_var.distance = 0.0f; + return_var.bearing = 0.0f; + + // Return error if arguments are bad + if(lat_now == 0.0d || lon_now == 0.0d || lat_center == 0.0d || lon_center == 0.0d || radius == 0.0d) return return_var; + + + if(arc_sweep >= 0) { + bearing_sector_start = arc_start_bearing; + bearing_sector_end = arc_start_bearing + arc_sweep; + if(bearing_sector_end > 2.0f*M_PI_F) bearing_sector_end -= M_TWOPI_F; + } else { + bearing_sector_end = arc_start_bearing; + bearing_sector_start = arc_start_bearing - arc_sweep; + if(bearing_sector_start < 0.0) bearing_sector_start += M_TWOPI_F; + } + in_sector = false; + + // Case where sector does not span zero + if(bearing_sector_end >= bearing_sector_start && bearing_now >= bearing_sector_start && bearing_now <= bearing_sector_end) in_sector = true; + + // Case where sector does span zero + if(bearing_sector_end < bearing_sector_start && ( bearing_now > bearing_sector_start || bearing_now < bearing_sector_end ) ) in_sector = true; + + // If in the sector then calculate distance and bearing to closest point + if(in_sector) { + return_var.past_end = false; + float dist_to_center = get_distance_to_next_waypoint(lat_now, lon_now, lat_center, lon_center); + + if(dist_to_center <= radius) { + return_var.distance = radius - dist_to_center; + return_var.bearing = bearing_now + M_PI_F; + } else { + return_var.distance = dist_to_center - radius; + return_var.bearing = bearing_now; + } + + // If out of the sector then calculate dist and bearing to start or end point + } else { + + // Use the approximation that 111,111 meters in the y direction is 1 degree (of latitude) + // and 111,111 * cos(latitude) meters in the x direction is 1 degree (of longitude) to + // calculate the position of the start and end points. We should not be doing this often + // as this function generally will not be called repeatedly when we are out of the sector. + + // TO DO - this is messed up and won't compile + float start_disp_x = radius * sin(arc_start_bearing); + float start_disp_y = radius * cos(arc_start_bearing); + float end_disp_x = radius * sin(_wrapPI(arc_start_bearing+arc_sweep)); + float end_disp_y = radius * cos(_wrapPI(arc_start_bearing+arc_sweep)); + float lon_start = lon_now + start_disp_x/111111.0d; + float lat_start = lat_now + start_disp_y*cos(lat_now)/111111.0d; + float lon_end = lon_now + end_disp_x/111111.0d; + float lat_end = lat_now + end_disp_y*cos(lat_now)/111111.0d; + float dist_to_start = get_distance_to_next_waypoint(lat_now, lon_now, lat_start, lon_start); + float dist_to_end = get_distance_to_next_waypoint(lat_now, lon_now, lat_end, lon_end); + if(dist_to_start < dist_to_end) { + return_var.distance = dist_to_start; + return_var.bearing = get_bearing_to_next_waypoint(lat_now, lon_now, lat_start, lon_start); + } else { + return_var.past_end = true; + return_var.distance = dist_to_end; + return_var.bearing = get_bearing_to_next_waypoint(lat_now, lon_now, lat_end, lon_end); + } + + } + + return_var.bearing = _wrapPI(return_var.bearing); + return_var.error = false; + return return_var; +} + +float _wrapPI(float bearing) +{ + + while (bearing > M_PI_F) { + bearing = bearing - M_TWOPI_F; + } + while (bearing <= -M_PI_F) { + bearing = bearing + M_TWOPI_F; + } + return bearing; +} + +float _wrap2PI(float bearing) +{ + + while (bearing >= M_TWOPI_F) { + bearing = bearing - M_TWOPI_F; + } + while (bearing < 0.0f) { + bearing = bearing + M_TWOPI_F; + } + return bearing; +} + +float _wrap180(float bearing) +{ + + while (bearing > 180.0f) { + bearing = bearing - 360.0f; + } + while (bearing <= -180.0f) { + bearing = bearing + 360.0f; + } + return bearing; +} + +float _wrap360(float bearing) +{ + + while (bearing >= 360.0f) { + bearing = bearing - 360.0f; + } + while (bearing < 0.0f) { + bearing = bearing + 360.0f; + } + return bearing; +} + + + \ No newline at end of file diff --git a/apps/systemlib/geo/geo.h b/apps/systemlib/geo/geo.h index 0e86b3599d..205afc79e6 100644 --- a/apps/systemlib/geo/geo.h +++ b/apps/systemlib/geo/geo.h @@ -42,8 +42,31 @@ * @author Thomas Gubler * @author Julian Oes * @author Lorenz Meier + * Additional functions - @author Doug Weibel */ + + +#include + +typedef struct { + bool error; // Flag that the calculation failed + bool past_end; // Flag indicating we are past the end of the line/arc segment + float distance; // Distance in meters to closest point on line/arc + float bearing; // Bearing in radians to closest point on line/arc +} crosstrack_error_s; __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next); __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next); + +// + +__EXPORT crosstrack_error_s get_distance_to_line(double lat_now, double lon_now, double lat_start, double lon_start, double lat_end, double lon_end); + +__EXPORT crosstrack_error_s get_distance_to_arc(double lat_now, double lon_now, double lat_center, double lon_center, + float radius, float arc_start_bearing, float arc_sweep); + +float _wrap180(float bearing); +float _wrap360(float bearing); +float _wrapPI(float bearing); +float _wrap2PI(float bearing); \ No newline at end of file diff --git a/nuttx/arch/arm/include/math.h b/nuttx/arch/arm/include/math.h index ef08692432..9de10c58b4 100644 --- a/nuttx/arch/arm/include/math.h +++ b/nuttx/arch/arm/include/math.h @@ -544,6 +544,8 @@ extern int matherr _PARAMS((struct exception *e)); #define M_1_PI 0.31830988618379067154 #define M_2_PI 0.63661977236758134308 #define M_2_SQRTPI 1.12837916709551257390 +#define M_DEG_TO_RAD 0.01745329251994 +#define M_RAD_TO_DEG 57.2957795130823 #define M_SQRT2 1.41421356237309504880 #define M_SQRT1_2 0.70710678118654752440 #define M_LN2LO 1.9082149292705877000E-10 @@ -568,6 +570,8 @@ extern int matherr _PARAMS((struct exception *e)); #define M_1_PI_F 0.31830988618379067154f #define M_2_PI_F 0.63661977236758134308f #define M_2_SQRTPI_F 1.12837916709551257390f +#define M_DEG_TO_RAD_F 0.01745329251994f +#define M_RAD_TO_DEG_F 57.2957795130823f #define M_SQRT2_F 1.41421356237309504880f #define M_SQRT1_2_F 0.70710678118654752440f #define M_LN2LO_F 1.9082149292705877000E-10f