fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-25 01:23:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #45 from thomasgubler/master_origin

Browse Source 
re-adding pid limitation & mavlink waypoint handling fix
This commit is contained in:
Lorenz Meier
2012-11-05 13:15:35 -08:00
parent 976545861a 59725ccd3a
commit 7d76a8a57b
2 changed files with 5 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/mavlink/waypoints.c
+3 -4
View File
@@ -360,7 +360,7 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_
float dist = -1.0f; float dist = -1.0f;
if (coordinate_frame == (int)MAV_FRAME_GLOBAL) { if (coordinate_frame == (int)MAV_FRAME_GLOBAL) {
dist = mavlink_wpm_distance_to_point_global_wgs84(wpm->current_active_wp_id, global_pos->lat, global_pos->lon, global_pos->alt); dist = mavlink_wpm_distance_to_point_global_wgs84(wpm->current_active_wp_id, (float)global_pos->lat * 1e-7f, (float)global_pos->lon * 1e-7f, global_pos->alt);
} else if (coordinate_frame == (int)MAV_FRAME_GLOBAL_RELATIVE_ALT) { } else if (coordinate_frame == (int)MAV_FRAME_GLOBAL_RELATIVE_ALT) {
dist = mavlink_wpm_distance_to_point_global_wgs84(wpm->current_active_wp_id, global_pos->lat, global_pos->lon, global_pos->relative_alt); dist = mavlink_wpm_distance_to_point_global_wgs84(wpm->current_active_wp_id, global_pos->lat, global_pos->lon, global_pos->relative_alt);
@@ -376,14 +376,13 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_
if (dist >= 0.f && dist <= orbit /*&& wpm->yaw_reached*/) { //TODO implement yaw if (dist >= 0.f && dist <= orbit /*&& wpm->yaw_reached*/) { //TODO implement yaw
wpm->pos_reached = true; wpm->pos_reached = true;
if (counter % 10 == 0) if (counter % 100 == 0)
printf("Setpoint reached: %0.4f, orbit: %.4f\n", dist, orbit); printf("Setpoint reached: %0.4f, orbit: %.4f\n", dist, orbit);
} }
// else // else
// { // {
// if(counter % 100 == 0) // if(counter % 100 == 0)
// printf("Setpoint not reached yet: %0.4f, orbit: %.4f\n",dist,orbit); // printf("Setpoint not reached yet: %0.4f, orbit: %.4f, coordinate frame: %d\n",dist, orbit, coordinate_frame);
// } // }
} }
apps/systemlib/pid/pid.c
+2 -2
View File
@@ -172,9 +172,9 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo
// Calculate the output. Limit output magnitude to pid->limit // 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 (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;