diff options
-rw-r--r-- | apps/fixedwing_control/fixedwing_control.c | 164 | ||||
-rw-r--r-- | apps/multirotor_att_control/multirotor_attitude_control.c | 51 | ||||
-rw-r--r-- | apps/systemlib/geo/geo.c | 202 | ||||
-rw-r--r-- | apps/systemlib/geo/geo.h | 23 | ||||
-rw-r--r-- | apps/systemlib/pid/pid.c | 78 | ||||
-rw-r--r-- | apps/systemlib/pid/pid.h | 6 | ||||
-rw-r--r-- | nuttx/arch/arm/include/math.h | 4 |
7 files changed, 385 insertions, 143 deletions
diff --git a/apps/fixedwing_control/fixedwing_control.c b/apps/fixedwing_control/fixedwing_control.c index ad08247e1..8629c2f11 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 <oivan@ethz.ch> + * Modifications: Doug Weibel <douglas.weibel@colorado.edu> * * 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_ROLLRATE_P, 0.3f); +// Need to add functionality to suppress integrator windup while on the ground +// 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_PITCHRATE_P, 0.3f); +// Need to add functionality to suppress integrator windup while on the ground +// 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_pos_p; - float roll_pos_i; - float roll_pos_awu; - float roll_pos_lim; + 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_pos_p; - param_t roll_pos_i; - param_t roll_pos_awu; - param_t roll_pos_lim; + 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; }; -// 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.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_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.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_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, - 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] = { @@ -251,11 +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_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.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 */ @@ -359,14 +415,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 +513,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->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_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->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; } diff --git a/apps/multirotor_att_control/multirotor_attitude_control.c b/apps/multirotor_att_control/multirotor_attitude_control.c index a08ee5e46..8c0e10fc3 100644 --- a/apps/multirotor_att_control/multirotor_attitude_control.c +++ b/apps/multirotor_att_control/multirotor_attitude_control.c @@ -60,11 +60,9 @@ PARAM_DEFINE_FLOAT(MC_YAWPOS_D, 0.0f); PARAM_DEFINE_FLOAT(MC_YAWPOS_AWU, 1.0f); PARAM_DEFINE_FLOAT(MC_YAWPOS_LIM, 3.0f); - - -PARAM_DEFINE_FLOAT(MC_ATT_P, 0.2f); /* 0.15 F405 Flamewheel */ +PARAM_DEFINE_FLOAT(MC_ATT_P, 0.2f); PARAM_DEFINE_FLOAT(MC_ATT_I, 0.0f); -PARAM_DEFINE_FLOAT(MC_ATT_D, 0.05f); /* 0.025 F405 Flamewheel */ +PARAM_DEFINE_FLOAT(MC_ATT_D, 0.05f); PARAM_DEFINE_FLOAT(MC_ATT_AWU, 0.05f); PARAM_DEFINE_FLOAT(MC_ATT_LIM, 0.4f); @@ -78,12 +76,6 @@ struct mc_att_control_params { float yaw_awu; float yaw_lim; - // float yawrate_p; - // float yawrate_i; - // float yawrate_d; - // float yawrate_awu; - // float yawrate_lim; - float att_p; float att_i; float att_d; @@ -101,12 +93,6 @@ struct mc_att_control_param_handles { param_t yaw_awu; param_t yaw_lim; - // param_t yawrate_p; - // param_t yawrate_i; - // param_t yawrate_d; - // param_t yawrate_awu; - // param_t yawrate_lim; - param_t att_p; param_t att_i; param_t att_d; @@ -139,12 +125,6 @@ static int parameters_init(struct mc_att_control_param_handles *h) h->yaw_awu = param_find("MC_YAWPOS_AWU"); h->yaw_lim = param_find("MC_YAWPOS_LIM"); - // h->yawrate_p = param_find("MC_YAWRATE_P"); - // h->yawrate_i = param_find("MC_YAWRATE_I"); - // h->yawrate_d = param_find("MC_YAWRATE_D"); - // h->yawrate_awu = param_find("MC_YAWRATE_AWU"); - // h->yawrate_lim = param_find("MC_YAWRATE_LIM"); - h->att_p = param_find("MC_ATT_P"); h->att_i = param_find("MC_ATT_I"); h->att_d = param_find("MC_ATT_D"); @@ -165,12 +145,6 @@ static int parameters_update(const struct mc_att_control_param_handles *h, struc param_get(h->yaw_awu, &(p->yaw_awu)); param_get(h->yaw_lim, &(p->yaw_lim)); - // param_get(h->yawrate_p, &(p->yawrate_p)); - // param_get(h->yawrate_i, &(p->yawrate_i)); - // param_get(h->yawrate_d, &(p->yawrate_d)); - // param_get(h->yawrate_awu, &(p->yawrate_awu)); - // param_get(h->yawrate_lim, &(p->yawrate_lim)); - param_get(h->att_p, &(p->att_p)); param_get(h->att_i, &(p->att_i)); param_get(h->att_d, &(p->att_d)); @@ -199,8 +173,6 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s static int motor_skip_counter = 0; - // static PID_t yaw_pos_controller; -// static PID_t yaw_speed_controller; static PID_t pitch_controller; static PID_t roll_controller; @@ -214,14 +186,10 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s parameters_init(&h); 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); - // pid_init(&yaw_speed_controller, p.yawrate_p, p.yawrate_i, p.yawrate_d, p.yawrate_awu, - // PID_MODE_DERIVATIV_SET); pid_init(&pitch_controller, p.att_p, p.att_i, p.att_d, p.att_awu, - PID_MODE_DERIVATIV_SET); + p.att_lim, PID_MODE_DERIVATIV_SET); pid_init(&roll_controller, p.att_p, p.att_i, p.att_d, p.att_awu, - PID_MODE_DERIVATIV_SET); + p.att_lim, PID_MODE_DERIVATIV_SET); initialized = true; } @@ -230,12 +198,12 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s if (motor_skip_counter % 1000 == 0) { /* 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); printf("att ctrl: delays: %d us sens->ctrl, rate: %d Hz, input: %d Hz\n", sensor_delay, (int)(1.0f/deltaT), (int)(1.0f/dT_input)); + + pid_set_parameters(&pitch_controller, p.att_p, p.att_i, p.att_d, p.att_awu, p.att_lim); + pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, p.att_awu, p.att_lim); } /* calculate current control outputs */ @@ -251,10 +219,7 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s /* control yaw rate */ rates_sp->yaw = p.yaw_p * atan2f(sinf(att->yaw - att_sp->yaw_body), cosf(att->yaw - att_sp->yaw_body)); - - rates_sp->thrust = att_sp->thrust; - motor_skip_counter++; } diff --git a/apps/systemlib/geo/geo.c b/apps/systemlib/geo/geo.c index ce46d01cc..bc467bfa3 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; + theta = _wrapPI(theta); + + return theta; +} + +// Additional functions - @author Doug Weibel <douglas.weibel@colorado.edu> + +__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; - if (theta > M_PI_F) { - theta -= 2.0f * M_PI_F; + // 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 theta; -}
\ No newline at end of file + } + + 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 0e86b3599..205afc79e 100644 --- a/apps/systemlib/geo/geo.h +++ b/apps/systemlib/geo/geo.h @@ -42,8 +42,31 @@ * @author Thomas Gubler <thomasgubler@student.ethz.ch> * @author Julian Oes <joes@student.ethz.ch> * @author Lorenz Meier <lm@inf.ethz.ch> + * Additional functions - @author Doug Weibel <douglas.weibel@colorado.edu> */ + + +#include <stdbool.h> + +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/apps/systemlib/pid/pid.c b/apps/systemlib/pid/pid.c index cff5e6bbe..f9b50d030 100644 --- a/apps/systemlib/pid/pid.c +++ b/apps/systemlib/pid/pid.c @@ -43,12 +43,13 @@ #include <math.h> __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); + + if (isfinite(error)) { // Why is this necessary? DEW + pid->error_previous = error; } - // 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; - } - } + // 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; } - 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; + // 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 { - p = error; - } - - if (isfinite(error)) { - pid->error_previous = error; + 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 098b2bef8..b51afef9e 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); diff --git a/nuttx/arch/arm/include/math.h b/nuttx/arch/arm/include/math.h index ef0869243..9de10c58b 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 |