Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-04 05:05:19 +08:00
Code Issues Actions 12 Packages Projects Releases Wiki Activity

Merged with fixed-wing stabilization work, multirotor control tested

Browse Source
This commit is contained in:
Lorenz Meier
2012-10-22 14:42:50 +02:00
parent 9e8a02b928 1e0a34a102
commit 64c5096c9f
7 changed files with 398 additions and 156 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/fixedwing_control/fixedwing_control.c
+121 -37
View File
@@ -2,6 +2,7 @@
* *
* Copyright (C) 2012 PX4 Development Team. All rights reserved. * Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: @author Ivan Ovinnikov <oivan@ethz.ch> * 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 * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * modification, are permitted provided that the following conditions
@@ -31,7 +32,7 @@
* POSSIBILITY OF SUCH DAMAGE. * POSSIBILITY OF SUCH DAMAGE.
* *
****************************************************************************/ ****************************************************************************/
// Workflow test comment - DEW
/** /**
* @file fixedwing_control.c * @file fixedwing_control.c
* Implementation of a fixed wing attitude and position controller. * Implementation of a fixed wing attitude and position controller.
@@ -86,27 +87,61 @@ static void usage(const char *reason);
* Controller parameters, accessible via MAVLink * Controller parameters, accessible via MAVLink
* *
*/ */
PARAM_DEFINE_FLOAT(FW_ROLL_POS_P, 0.3f); // Roll control parameters
PARAM_DEFINE_FLOAT(FW_ROLL_POS_I, 0.002f); PARAM_DEFINE_FLOAT(FW_ROLLRATE_P, 0.3f);
PARAM_DEFINE_FLOAT(FW_ROLL_POS_AWU, 0.2f); // Need to add functionality to suppress integrator windup while on the ground
PARAM_DEFINE_FLOAT(FW_ROLL_POS_LIM, 0.6f); // 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 { struct fw_att_control_params {
float roll_pos_p; float rollrate_p;
float roll_pos_i; float rollrate_i;
float roll_pos_awu; float rollrate_awu;
float roll_pos_lim; 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 { struct fw_att_control_param_handles {
param_t roll_pos_p; param_t rollrate_p;
param_t roll_pos_i; param_t rollrate_i;
param_t roll_pos_awu; param_t rollrate_awu;
param_t roll_pos_lim; 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_P, 0.1f);
PARAM_DEFINE_FLOAT(FW_HEADING_LIM, 0.15f); 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. * response.
* *
* @param argc number of arguments * @param argc number of arguments
@@ -223,19 +258,31 @@ int fixedwing_control_thread_main(int argc, char *argv[])
pos_parameters_init(&hpos); pos_parameters_init(&hpos);
pos_parameters_update(&hpos, &ppos); pos_parameters_update(&hpos, &ppos);
PID_t roll_pos_controller; // TO_DO Fix output limit functionallity of PID controller or add that function elsewhere
pid_init(&roll_pos_controller, p.roll_pos_p, p.roll_pos_i, 0.0f, p.roll_pos_awu, PID_t roll_rate_controller;
PID_MODE_DERIVATIV_SET); 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_t heading_controller;
pid_init(&heading_controller, ppos.heading_p, 0.0f, 0.0f, 0.0f, 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 // XXX remove in production
/* advertise debug value */ /* advertise debug value */
struct debug_key_value_s dbg = { .key = "", .value = 0.0f }; struct debug_key_value_s dbg = { .key = "", .value = 0.0f };
orb_advert_t pub_dbg = orb_advertise(ORB_ID(debug_key_value), &dbg); 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) { while(!thread_should_exit) {
struct pollfd fds[1] = { struct pollfd fds[1] = {
@@ -251,11 +298,20 @@ int fixedwing_control_thread_main(int argc, char *argv[])
} else { } else {
// FIXME SUBSCRIBE // FIXME SUBSCRIBE
if (loopcounter % 20 == 0) { if (loopcounter % 100 == 0) {
att_parameters_update(&h, &p); att_parameters_update(&h, &p);
pos_parameters_update(&hpos, &ppos); 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(&roll_rate_controller, p.rollrate_p, p.rollrate_i, 0.0f,
pid_set_parameters(&heading_controller, ppos.heading_p, 0.0f, 0.0f, 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 */ /* if position updated, run position control loop */
@@ -359,12 +415,23 @@ int fixedwing_control_thread_main(int argc, char *argv[])
printf("[fixedwing control] roll sp: %8.4f, \n", att_sp.roll_body); printf("[fixedwing control] roll sp: %8.4f, \n", att_sp.roll_body);
} }
// actuators.control[0] = pid_calculate(&roll_pos_controller, att_sp.roll_body, // actuator control[0] is aileron (or elevon roll control)
// att.roll, att.rollspeed, deltaTpos); // Commanded roll rate from P controller on roll angle
actuators.control[0] = pid_calculate(&roll_pos_controller, att_sp.roll_body, float roll_rate_command = pid_calculate(&roll_angle_controller, att_sp.roll_body,
att.roll, att.rollspeed, deltaTpos); att.roll, 0.0f, deltaTpos);
actuators.control[1] = 0; // actuator control from PI controller on roll rate
actuators.control[2] = 0; 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; actuators.control[3] = att_sp.thrust;
/* publish attitude setpoint (for MAVLink) */ /* publish attitude setpoint (for MAVLink) */
@@ -446,20 +513,37 @@ int fixedwing_control_main(int argc, char *argv[])
static int att_parameters_init(struct fw_att_control_param_handles *h) static int att_parameters_init(struct fw_att_control_param_handles *h)
{ {
/* PID parameters */ /* PID parameters */
h->roll_pos_p = param_find("FW_ROLL_POS_P");
h->roll_pos_i = param_find("FW_ROLL_POS_I"); h->rollrate_p = param_find("FW_ROLLRATE_P");
h->roll_pos_lim = param_find("FW_ROLL_POS_LIM"); h->rollrate_i = param_find("FW_ROLLRATE_I");
h->roll_pos_awu = param_find("FW_ROLL_POS_AWU"); 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; return OK;
} }
static int att_parameters_update(const struct fw_att_control_param_handles *h, struct fw_att_control_params *p) 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->rollrate_p, &(p->rollrate_p));
param_get(h->roll_pos_i, &(p->roll_pos_i)); param_get(h->rollrate_i, &(p->rollrate_i));
param_get(h->roll_pos_lim, &(p->roll_pos_lim)); param_get(h->rollrate_awu, &(p->rollrate_awu));
param_get(h->roll_pos_awu, &(p->roll_pos_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; return OK;
} }
apps/multirotor_att_control/multirotor_attitude_control.c
+8 -43
View File
@@ -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_AWU, 1.0f);
PARAM_DEFINE_FLOAT(MC_YAWPOS_LIM, 3.0f); PARAM_DEFINE_FLOAT(MC_YAWPOS_LIM, 3.0f);
PARAM_DEFINE_FLOAT(MC_ATT_P, 0.2f);
PARAM_DEFINE_FLOAT(MC_ATT_P, 0.2f); /* 0.15 F405 Flamewheel */
PARAM_DEFINE_FLOAT(MC_ATT_I, 0.0f); 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_AWU, 0.05f);
PARAM_DEFINE_FLOAT(MC_ATT_LIM, 0.4f); PARAM_DEFINE_FLOAT(MC_ATT_LIM, 0.4f);
@@ -78,12 +76,6 @@ struct mc_att_control_params {
float yaw_awu; float yaw_awu;
float yaw_lim; float yaw_lim;
// float yawrate_p;
// float yawrate_i;
// float yawrate_d;
// float yawrate_awu;
// float yawrate_lim;
float att_p; float att_p;
float att_i; float att_i;
float att_d; float att_d;
@@ -101,12 +93,6 @@ struct mc_att_control_param_handles {
param_t yaw_awu; param_t yaw_awu;
param_t yaw_lim; 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_p;
param_t att_i; param_t att_i;
param_t att_d; 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_awu = param_find("MC_YAWPOS_AWU");
h->yaw_lim = param_find("MC_YAWPOS_LIM"); 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_p = param_find("MC_ATT_P");
h->att_i = param_find("MC_ATT_I"); h->att_i = param_find("MC_ATT_I");
h->att_d = param_find("MC_ATT_D"); 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_awu, &(p->yaw_awu));
param_get(h->yaw_lim, &(p->yaw_lim)); 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_p, &(p->att_p));
param_get(h->att_i, &(p->att_i)); param_get(h->att_i, &(p->att_i));
param_get(h->att_d, &(p->att_d)); 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 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 pitch_controller;
static PID_t roll_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_init(&h);
parameters_update(&h, &p); 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_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_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; initialized = true;
} }
@@ -230,12 +198,12 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
if (motor_skip_counter % 1000 == 0) { if (motor_skip_counter % 1000 == 0) {
/* update parameters from storage */ /* update parameters from storage */
parameters_update(&h, &p); parameters_update(&h, &p);
/* apply parameters */ /* 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)); 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 */ /* calculate current control outputs */
@@ -251,10 +219,7 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
/* control yaw rate */ /* 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->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; rates_sp->thrust = att_sp->thrust;
motor_skip_counter++; motor_skip_counter++;
} }
apps/systemlib/geo/geo.c
+193 -11
View File
@@ -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) __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 lat_now_rad = lat_now * M_DEG_TO_RAD;
double lon_now_rad = lon_now / 180.0 * M_PI; double lon_now_rad = lon_now * M_DEG_TO_RAD;
double lat_next_rad = lat_next / 180.0 * M_PI; double lat_next_rad = lat_next * M_DEG_TO_RAD;
double lon_next_rad = lon_next / 180.0 * M_PI; double lon_next_rad = lon_next * M_DEG_TO_RAD;
double d_lat = lat_next_rad - lat_now_rad; double d_lat = lat_next_rad - lat_now_rad;
double d_lon = lon_next_rad - lon_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 */ /* 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)); 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 = _wrapPI(theta);
theta += 2.0f * M_PI_F;
}
if (theta > M_PI_F) {
theta -= 2.0f * M_PI_F;
}
return 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;
// 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;
}
apps/systemlib/geo/geo.h
+23
View File
@@ -42,8 +42,31 @@
* @author Thomas Gubler <thomasgubler@student.ethz.ch> * @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch> * @author Julian Oes <joes@student.ethz.ch>
* @author Lorenz Meier <lm@inf.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_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 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);
apps/systemlib/pid/pid.c
+40 -58
View File
@@ -43,12 +43,13 @@
#include <math.h> #include <math.h>
__EXPORT void pid_init(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,
uint8_t mode) float limit, uint8_t mode)
{ {
pid->kp = kp; pid->kp = kp;
pid->ki = ki; pid->ki = ki;
pid->kd = kd; pid->kd = kd;
pid->intmax = intmax; pid->intmax = intmax;
pid->limit = limit;
pid->mode = mode; pid->mode = mode;
pid->count = 0; pid->count = 0;
pid->saturated = 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->error_previous = 0;
pid->integral = 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; int ret = 0;
@@ -86,7 +87,12 @@ __EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float
ret = 1; ret = 1;
} }
// pid->limit = limit; if (isfinite(limit)) {
pid->limit = limit;
} else {
ret = 1;
}
return ret; return ret;
} }
@@ -122,72 +128,48 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo
float i, d; float i, d;
pid->sp = sp; pid->sp = sp;
// Calculated current error value
float error = pid->sp - val; float error = pid->sp - val;
if (pid->saturated && (pid->integral * error > 0)) { if (isfinite(error)) { // Why is this necessary? DEW
//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)) {
pid->error_previous = error; 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); 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)) { if (isfinite(output)) {
pid->last_output = output; pid->last_output = output;
} }
if (!isfinite(pid->integral)) {
pid->integral = 0;
}
return pid->last_output; return pid->last_output;
} }
apps/systemlib/pid/pid.h
+4 -2
View File
@@ -49,6 +49,8 @@
#define PID_MODE_DERIVATIV_CALC 0 #define PID_MODE_DERIVATIV_CALC 0
/* Use PID_MODE_DERIVATIV_SET if you have the derivative already (Gyros, Kalman) */ /* Use PID_MODE_DERIVATIV_SET if you have the derivative already (Gyros, Kalman) */
#define PID_MODE_DERIVATIV_SET 1 #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 { typedef struct {
float kp; float kp;
@@ -65,8 +67,8 @@ typedef struct {
uint8_t saturated; uint8_t saturated;
} PID_t; } PID_t;
__EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, uint8_t mode); __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); __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); //void pid_set(PID_t *pid, float sp);
__EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt); __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt);
nuttx/arch/arm/include/math.h
+4
View File
@@ -544,6 +544,8 @@ extern int matherr _PARAMS((struct exception *e));
#define M_1_PI 0.31830988618379067154 #define M_1_PI 0.31830988618379067154
#define M_2_PI 0.63661977236758134308 #define M_2_PI 0.63661977236758134308
#define M_2_SQRTPI 1.12837916709551257390 #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_SQRT2 1.41421356237309504880
#define M_SQRT1_2 0.70710678118654752440 #define M_SQRT1_2 0.70710678118654752440
#define M_LN2LO 1.9082149292705877000E-10 #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_1_PI_F 0.31830988618379067154f
#define M_2_PI_F 0.63661977236758134308f #define M_2_PI_F 0.63661977236758134308f
#define M_2_SQRTPI_F 1.12837916709551257390f #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_SQRT2_F 1.41421356237309504880f
#define M_SQRT1_2_F 0.70710678118654752440f #define M_SQRT1_2_F 0.70710678118654752440f
#define M_LN2LO_F 1.9082149292705877000E-10f #define M_LN2LO_F 1.9082149292705877000E-10f