Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

Tracker: Changed PID to AC_PID

Browse Source
This commit is contained in:
stefanlynka 2016-06-16 12:07:07 +09:00 committed by Randy Mackay
parent 39d4fa2cf8
commit 2b7203e4d1
6 changed files with 27 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
AntennaTracker/Parameters.cpp
View File

@ -337,7 +337,7 @@ const AP_Param::Info Tracker::var_info[] = {
// @Range: 0.001 0.1 // @Range: 0.001 0.1
// @Increment: 0.001 // @Increment: 0.001
// @User: Standard // @User: Standard
GGROUP(pidPitch2Srv, "PITCH2SRV_", PID), GGROUP(pidPitch2Srv, "PITCH2SRV_", AC_PID),
// @Param: YAW2SRV_P // @Param: YAW2SRV_P
// @DisplayName: Yaw axis controller P gain // @DisplayName: Yaw axis controller P gain
@ -367,7 +367,7 @@ const AP_Param::Info Tracker::var_info[] = {
// @Range: 0.001 0.1 // @Range: 0.001 0.1
// @Increment: 0.001 // @Increment: 0.001
// @User: Standard // @User: Standard
GGROUP(pidYaw2Srv, "YAW2SRV_", PID), GGROUP(pidYaw2Srv, "YAW2SRV_", AC_PID),
// @Param: CMD_TOTAL // @Param: CMD_TOTAL
// @DisplayName: Number of loaded mission items // @DisplayName: Number of loaded mission items

16
AntennaTracker/Parameters.h
View File

@ -63,8 +63,8 @@ public:
k_param_scheduler, k_param_scheduler,
k_param_ins, k_param_ins,
k_param_sitl, k_param_sitl,
k_param_pidPitch2Srv, k_param_pidPitch_old, // deprecated
k_param_pidYaw2Srv, k_param_pidYaw_old, // deprecated
k_param_gcs2, // stream rates for uartD k_param_gcs2, // stream rates for uartD
k_param_serial2_baud, // deprecated k_param_serial2_baud, // deprecated
@ -107,6 +107,8 @@ public:
// //
k_param_channel_yaw = 200, k_param_channel_yaw = 200,
k_param_channel_pitch, k_param_channel_pitch,
k_param_pidPitch2Srv,
k_param_pidYaw2Srv,
// //
// 220: Waypoint data // 220: Waypoint data
@ -156,13 +158,13 @@ public:
AP_Int32 log_bitmask; AP_Int32 log_bitmask;
// PID controllers // AC_PID controllers
PID pidPitch2Srv; AC_PID pidPitch2Srv;
PID pidYaw2Srv; AC_PID pidYaw2Srv;
Parameters() : Parameters() :
pidPitch2Srv(0.2, 0, 0.05f, 4000.0f), pidPitch2Srv(0.2, 0, 0.05f, 4000.0f, 0.1, 0.02f),
pidYaw2Srv (0.2, 0, 0.05f, 4000.0f) pidYaw2Srv (0.2, 0, 0.05f, 4000.0f, 0.1, 0.02f)
{} {}
}; };

2
AntennaTracker/Tracker.h
View File

@ -46,7 +46,7 @@
#include <AP_SerialManager/AP_SerialManager.h> // Serial manager library #include <AP_SerialManager/AP_SerialManager.h> // Serial manager library
#include <AP_Declination/AP_Declination.h> // ArduPilot Mega Declination Helper Library #include <AP_Declination/AP_Declination.h> // ArduPilot Mega Declination Helper Library
#include <DataFlash/DataFlash.h> #include <DataFlash/DataFlash.h>
#include <PID/PID.h> #include <AC_PID/AC_PID.h>
#include <AP_Scheduler/AP_Scheduler.h> // main loop scheduler #include <AP_Scheduler/AP_Scheduler.h> // main loop scheduler
#include <AP_NavEKF/AP_NavEKF.h> #include <AP_NavEKF/AP_NavEKF.h>
#include <AP_NavEKF2/AP_NavEKF2.h> #include <AP_NavEKF2/AP_NavEKF2.h>

2
AntennaTracker/make.inc
View File

@ -15,7 +15,7 @@ LIBRARIES += GCS_MAVLink
LIBRARIES += AP_SerialManager LIBRARIES += AP_SerialManager
LIBRARIES += AP_Declination LIBRARIES += AP_Declination
LIBRARIES += DataFlash LIBRARIES += DataFlash
LIBRARIES += PID LIBRARIES += AC_PID
LIBRARIES += AP_Scheduler LIBRARIES += AP_Scheduler
LIBRARIES += AP_NavEKF LIBRARIES += AP_NavEKF
LIBRARIES += AP_NavEKF2 LIBRARIES += AP_NavEKF2

35
AntennaTracker/servos.cpp
View File

@ -78,11 +78,8 @@ void Tracker::update_pitch_position_servo(float pitch)
// PITCH2SRV_IMAX 4000.000000 // PITCH2SRV_IMAX 4000.000000
// calculate new servo position // calculate new servo position
int32_t new_servo_out = channel_pitch.get_servo_out() + g.pidPitch2Srv.get_pid(angle_err); g.pidPitch2Srv.set_input_filter_all(angle_err);
int32_t new_servo_out = channel_pitch.get_servo_out() + g.pidPitch2Srv.get_pid();
// initialise limit flags
servo_limit.pitch_lower = false;
servo_limit.pitch_upper = false;
// rate limit pitch servo // rate limit pitch servo
if (g.pitch_slew_time > 0.02f) { if (g.pitch_slew_time > 0.02f) {
@ -92,11 +89,9 @@ void Tracker::update_pitch_position_servo(float pitch)
} }
if (new_servo_out <= channel_pitch.get_servo_out() - max_change) { if (new_servo_out <= channel_pitch.get_servo_out() - max_change) {
new_servo_out = channel_pitch.get_servo_out() - max_change; new_servo_out = channel_pitch.get_servo_out() - max_change;
servo_limit.pitch_lower = true;
} }
if (new_servo_out >= channel_pitch.get_servo_out() + max_change) { if (new_servo_out >= channel_pitch.get_servo_out() + max_change) {
new_servo_out = channel_pitch.get_servo_out() + max_change; new_servo_out = channel_pitch.get_servo_out() + max_change;
servo_limit.pitch_upper = true;
} }
} }
channel_pitch.set_servo_out(new_servo_out); channel_pitch.set_servo_out(new_servo_out);
@ -104,11 +99,11 @@ void Tracker::update_pitch_position_servo(float pitch)
// position limit pitch servo // position limit pitch servo
if (channel_pitch.get_servo_out() <= -pitch_limit_cd) { if (channel_pitch.get_servo_out() <= -pitch_limit_cd) {
channel_pitch.set_servo_out(-pitch_limit_cd); channel_pitch.set_servo_out(-pitch_limit_cd);
servo_limit.pitch_lower = true; g.pidPitch2Srv.reset_I();
} }
if (channel_pitch.get_servo_out() >= pitch_limit_cd) { if (channel_pitch.get_servo_out() >= pitch_limit_cd) {
channel_pitch.set_servo_out(pitch_limit_cd); channel_pitch.set_servo_out(pitch_limit_cd);
servo_limit.pitch_upper = true; g.pidPitch2Srv.reset_I();
} }
} }
@ -146,7 +141,8 @@ void Tracker::update_pitch_cr_servo(float pitch)
{ {
float ahrs_pitch = degrees(ahrs.pitch); float ahrs_pitch = degrees(ahrs.pitch);
float err_cd = (pitch - ahrs_pitch) * 100.0f; float err_cd = (pitch - ahrs_pitch) * 100.0f;
channel_pitch.set_servo_out(g.pidPitch2Srv.get_pid(err_cd)); g.pidPitch2Srv.set_input_filter_all(err_cd);
channel_pitch.set_servo_out(g.pidPitch2Srv.get_pid());
} }
/** /**
@ -256,17 +252,13 @@ void Tracker::update_yaw_position_servo(float yaw)
} }
slew_dir = new_slew_dir; slew_dir = new_slew_dir;
// initialise limit flags
servo_limit.yaw_lower = false;
servo_limit.yaw_upper = false;
int16_t new_servo_out; int16_t new_servo_out;
if (slew_dir != 0) { if (slew_dir != 0) {
new_servo_out = slew_dir * 18000; new_servo_out = slew_dir * 18000;
g.pidYaw2Srv.reset_I(); g.pidYaw2Srv.reset_I();
} else { } else {
float servo_change = g.pidYaw2Srv.get_pid(angle_err); g.pidYaw2Srv.set_input_filter_all(angle_err);
float servo_change = g.pidYaw2Srv.get_pid();
servo_change = constrain_float(servo_change, -18000, 18000); servo_change = constrain_float(servo_change, -18000, 18000);
new_servo_out = constrain_float(channel_yaw.get_servo_out() - servo_change, -18000, 18000); new_servo_out = constrain_float(channel_yaw.get_servo_out() - servo_change, -18000, 18000);
} }
@ -279,23 +271,21 @@ void Tracker::update_yaw_position_servo(float yaw)
} }
if (new_servo_out <= channel_yaw.get_servo_out() - max_change) { if (new_servo_out <= channel_yaw.get_servo_out() - max_change) {
new_servo_out = channel_yaw.get_servo_out() - max_change; new_servo_out = channel_yaw.get_servo_out() - max_change;
servo_limit.yaw_lower = true;
} }
if (new_servo_out >= channel_yaw.get_servo_out() + max_change) { if (new_servo_out >= channel_yaw.get_servo_out() + max_change) {
new_servo_out = channel_yaw.get_servo_out() + max_change; new_servo_out = channel_yaw.get_servo_out() + max_change;
servo_limit.yaw_upper = true;
} }
} }
channel_yaw.set_servo_out(new_servo_out); channel_yaw.set_servo_out(new_servo_out);
// position limit pitch servo // position limit yaw servo
if (channel_yaw.get_servo_out() <= -yaw_limit_cd) { if (channel_yaw.get_servo_out() <= -yaw_limit_cd) {
channel_yaw.set_servo_out(-yaw_limit_cd); channel_yaw.set_servo_out(-yaw_limit_cd);
servo_limit.yaw_lower = true; g.pidYaw2Srv.reset_I();
} }
if (channel_yaw.get_servo_out() >= yaw_limit_cd) { if (channel_yaw.get_servo_out() >= yaw_limit_cd) {
channel_yaw.set_servo_out(yaw_limit_cd); channel_yaw.set_servo_out(yaw_limit_cd);
servo_limit.yaw_upper = true; g.pidYaw2Srv.reset_I();
} }
} }
@ -335,5 +325,6 @@ void Tracker::update_yaw_cr_servo(float yaw)
float yaw_cd = wrap_180_cd(yaw*100.0f); float yaw_cd = wrap_180_cd(yaw*100.0f);
float err_cd = wrap_180_cd(yaw_cd - (float)ahrs_yaw_cd); float err_cd = wrap_180_cd(yaw_cd - (float)ahrs_yaw_cd);
channel_yaw.set_servo_out(g.pidYaw2Srv.get_pid(err_cd)); g.pidYaw2Srv.set_input_filter_all(err_cd);
channel_yaw.set_servo_out(g.pidYaw2Srv.get_pid());
} }

2
AntennaTracker/wscript
View File

@ -7,7 +7,7 @@ def build(bld):
name=vehicle + '_libs', name=vehicle + '_libs',
vehicle=vehicle, vehicle=vehicle,
libraries=bld.ap_common_vehicle_libraries() + [ libraries=bld.ap_common_vehicle_libraries() + [
'PID', 'AC_PID',
], ],
use='mavlink', use='mavlink',
) )