aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

APM: added RDRSTEER_ PID for steering on ground

Browse Source 
this allows for rolling takeoff with steering, and use of rudder in
landing
This commit is contained in:
Andrew Tridgell 2012-08-15 11:19:12 +10:00
parent 52064772ee
commit e26e8b3b67
5 changed files with 23 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
ArduPlane/Attitude.pde
View File

@ -172,16 +172,19 @@ static void calc_throttle()
// ----------------------------------------------------------------------------------------
static void calc_nav_yaw(float speed_scaler)
{
#if HIL_MODE != HIL_MODE_ATTITUDE
Vector3f temp = imu.get_accel();
long error = (long)(-temp.y*100.0);
// always do rudder mixing from roll
g.channel_rudder.servo_out = g.kff_rudder_mix * g.channel_roll.servo_out;
// Control is a feedforward from the aileron control + a PID to coordinate the turn (drive y axis accel to zero)
g.channel_rudder.servo_out = g.kff_rudder_mix * g.channel_roll.servo_out + g.pidServoRudder.get_pid(error, speed_scaler);
#else
g.channel_rudder.servo_out = g.kff_rudder_mix * g.channel_roll.servo_out;
// XXX probably need something here based on heading
#endif
if (hold_course != -1) {
// steering on or close to ground
g.channel_rudder.servo_out += g.pidRdrSteer.get_pid(bearing_error_cd);
} else {
// a PID to coordinate the turn (drive y axis accel to zero)
Vector3f temp = imu.get_accel();
int32_t error = -temp.y*100.0;
g.channel_rudder.servo_out += g.pidServoRudder.get_pid(error, speed_scaler);
}
}

5
ArduPlane/Parameters.h
View File

@ -198,6 +198,7 @@ public:
k_param_pidServoRudder,
k_param_pidTeThrottle,
k_param_pidNavPitchAltitude,
k_param_pidRdrSteer,
// 254,255: reserved
};
@ -343,6 +344,7 @@ public:
PID pidServoRudder;
PID pidTeThrottle;
PID pidNavPitchAltitude;
PID pidRdrSteer;
Parameters() :
// variable default
@ -369,7 +371,8 @@ public:
pidNavPitchAirspeed (NAV_PITCH_ASP_P, NAV_PITCH_ASP_I, NAV_PITCH_ASP_D, NAV_PITCH_ASP_INT_MAX_CMSEC),
pidServoRudder (SERVO_YAW_P, SERVO_YAW_I, SERVO_YAW_D, SERVO_YAW_INT_MAX),
pidTeThrottle (THROTTLE_TE_P, THROTTLE_TE_I, THROTTLE_TE_D, THROTTLE_TE_INT_MAX),
pidNavPitchAltitude (NAV_PITCH_ALT_P, NAV_PITCH_ALT_I, NAV_PITCH_ALT_D, NAV_PITCH_ALT_INT_MAX_CM)
pidNavPitchAltitude (NAV_PITCH_ALT_P, NAV_PITCH_ALT_I, NAV_PITCH_ALT_D, NAV_PITCH_ALT_INT_MAX_CM),
pidRdrSteer (0, 0, 0, 0)
{}
};

1
ArduPlane/Parameters.pde
View File

@ -515,6 +515,7 @@ const AP_Param::Info var_info[] PROGMEM = {
GGROUP(pidServoRudder, "YW2SRV_", PID),
GGROUP(pidTeThrottle, "ENRGY2THR_", PID),
GGROUP(pidNavPitchAltitude, "ALT2PTCH_", PID),
GGROUP(pidRdrSteer, "RDRSTEER_", PID),
// variables not in the g class which contain EEPROM saved variables

12
ArduPlane/commands_logic.pde
View File

@ -283,14 +283,11 @@ static void do_loiter_time()
/********************************************************************************/
static bool verify_takeoff()
{
if (g_gps->ground_speed > 300){
if (ahrs.yaw_initialised()) {
if (hold_course == -1) {
// save our current course to take off
if(g.compass_enabled) {
hold_course = ahrs.yaw_sensor;
} else {
hold_course = g_gps->ground_course;
}
hold_course = ahrs.yaw_sensor;
gcs_send_text_fmt(PSTR("Holding course %ld"), hold_course);
}
}
@ -335,10 +332,11 @@ static bool verify_land()
// sudden large roll correction which is very nasty at
// this point in the landing.
hold_course = ahrs.yaw_sensor;
gcs_send_text_fmt(PSTR("Holding course %ld"), hold_course);
}
}
if (hold_course != -1){
if (hold_course != -1) {
// recalc bearing error with hold_course;
nav_bearing_cd = hold_course;
// recalc bearing error

19
ArduPlane/navigation.pde
View File

@ -105,24 +105,7 @@ static void calc_gndspeed_undershoot()
static void calc_bearing_error()
{
if(takeoff_complete == true || g.compass_enabled == true) {
/*
most of the time we use the yaw sensor for heading, even if
we don't have a compass. The yaw sensor is drift corrected
in the DCM library. We only use the gps ground course
directly if we haven't completed takeoff, as the yaw drift
correction won't have had a chance to kick in. Drift
correction using the GPS typically takes 10 seconds or so
for a 180 degree correction.
*/
bearing_error_cd = nav_bearing_cd - ahrs.yaw_sensor;
} else {
// TODO: we need to use the Yaw gyro for in between GPS reads,
// maybe as an offset from a saved gryo value.
bearing_error_cd = nav_bearing_cd - g_gps->ground_course;
}
bearing_error_cd = nav_bearing_cd - ahrs.yaw_sensor;
bearing_error_cd = wrap_180_cd(bearing_error_cd);
}