Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-06 16:12:12 +08:00
Code Issues Actions 30 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/systemlib/geo/geo.c
+194 -12
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)
{
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;
}
}
// 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 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);
apps/systemlib/pid/pid.c
+41 -59
View File
@@ -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);
}
// 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;
}
apps/systemlib/pid/pid.h
+4 -2
View File
@@ -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);