Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Tracker: Changed PID to AC_PID

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

16
AntennaTracker/Parameters.h
View File

@ -63,8 +63,8 @@ public:
k_param_scheduler,
k_param_ins,
k_param_sitl,
k_param_pidPitch2Srv,
k_param_pidYaw2Srv,
k_param_pidPitch_old, // deprecated
k_param_pidYaw_old, // deprecated
k_param_gcs2, // stream rates for uartD
k_param_serial2_baud, // deprecated
@ -107,6 +107,8 @@ public:
//
k_param_channel_yaw = 200,
k_param_channel_pitch,
k_param_pidPitch2Srv,
k_param_pidYaw2Srv,
//
// 220: Waypoint data
@ -156,13 +158,13 @@ public:
AP_Int32 log_bitmask;
// PID controllers
PID pidPitch2Srv;
PID pidYaw2Srv;
// AC_PID controllers
AC_PID pidPitch2Srv;
AC_PID pidYaw2Srv;
Parameters() :
pidPitch2Srv(0.2, 0, 0.05f, 4000.0f),
pidYaw2Srv (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, 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_Declination/AP_Declination.h> // ArduPilot Mega Declination Helper Library
#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_NavEKF/AP_NavEKF.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_Declination
LIBRARIES += DataFlash
LIBRARIES += PID
LIBRARIES += AC_PID
LIBRARIES += AP_Scheduler
LIBRARIES += AP_NavEKF
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
// calculate new servo position
int32_t new_servo_out = channel_pitch.get_servo_out() + g.pidPitch2Srv.get_pid(angle_err);
// initialise limit flags
servo_limit.pitch_lower = false;
servo_limit.pitch_upper = false;
g.pidPitch2Srv.set_input_filter_all(angle_err);
int32_t new_servo_out = channel_pitch.get_servo_out() + g.pidPitch2Srv.get_pid();
// rate limit pitch servo
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) {
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) {
new_servo_out = channel_pitch.get_servo_out() + max_change;
servo_limit.pitch_upper = true;
}
}
channel_pitch.set_servo_out(new_servo_out);
@ -104,11 +99,11 @@ void Tracker::update_pitch_position_servo(float pitch)
// position limit pitch servo
if (channel_pitch.get_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) {
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 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;
// initialise limit flags
servo_limit.yaw_lower = false;
servo_limit.yaw_upper = false;
int16_t new_servo_out;
if (slew_dir != 0) {
new_servo_out = slew_dir * 18000;
g.pidYaw2Srv.reset_I();
} 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);
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) {
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) {
new_servo_out = channel_yaw.get_servo_out() + max_change;
servo_limit.yaw_upper = true;
}
}
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) {
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) {
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 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',
vehicle=vehicle,
libraries=bld.ap_common_vehicle_libraries() + [
'PID',
'AC_PID',
],
use='mavlink',
)