Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-03 12:14:10 -04:00
Code Issues Packages Projects Releases Wiki Activity

Plane: update for new APM_Control API

Browse Source
This commit is contained in:
Andrew Tridgell 2013-08-14 14:57:41 +10:00
parent 144516c941
commit accfd46633
5 changed files with 30 additions and 40 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
ArduPlane/ArduPlane.pde
View File

@ -249,6 +249,12 @@ AP_AHRS_DCM ahrs(&ins, g_gps);
static AP_L1_Control L1_controller(ahrs);
static AP_TECS TECS_controller(&ahrs, aparm);
// Attitude to servo controllers
static AP_RollController rollController(ahrs, aparm);
static AP_PitchController pitchController(ahrs, aparm);
static AP_YawController yawController(ahrs, aparm);
#if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
SITL sitl;
#endif

20
ArduPlane/Attitude.pde
View File

@ -77,7 +77,7 @@ static void stabilize_roll(float speed_scaler)
if (control_mode == STABILIZE && channel_roll->control_in != 0) {
disable_integrator = true;
}
channel_roll->servo_out = g.rollController.get_servo_out(nav_roll_cd - ahrs.roll_sensor,
channel_roll->servo_out = rollController.get_servo_out(nav_roll_cd - ahrs.roll_sensor,
speed_scaler,
disable_integrator);
}
@ -94,7 +94,7 @@ static void stabilize_pitch(float speed_scaler)
if (control_mode == STABILIZE && channel_pitch->control_in != 0) {
disable_integrator = true;
}
channel_pitch->servo_out = g.pitchController.get_servo_out(demanded_pitch - ahrs.pitch_sensor,
channel_pitch->servo_out = pitchController.get_servo_out(demanded_pitch - ahrs.pitch_sensor,
speed_scaler,
disable_integrator);
}
@ -265,7 +265,7 @@ static void stabilize_acro(float speed_scaler)
nav_roll_cd = ahrs.roll_sensor + roll_error_cd;
// try to reduce the integrated angular error to zero. We set
// 'stabilze' to true, which disables the roll integrator
channel_roll->servo_out = g.rollController.get_servo_out(roll_error_cd,
channel_roll->servo_out = rollController.get_servo_out(roll_error_cd,
speed_scaler,
true);
} else {
@ -274,7 +274,7 @@ static void stabilize_acro(float speed_scaler)
user releases the stick
*/
acro_state.locked_roll = false;
channel_roll->servo_out = g.rollController.get_rate_out(roll_rate, speed_scaler);
channel_roll->servo_out = rollController.get_rate_out(roll_rate, speed_scaler);
}
if (pitch_rate == 0) {
@ -289,7 +289,7 @@ static void stabilize_acro(float speed_scaler)
// try to hold the locked pitch. Note that we have the pitch
// integrator enabled, which helps with inverted flight
nav_pitch_cd = acro_state.locked_pitch_cd;
channel_pitch->servo_out = g.pitchController.get_servo_out(nav_pitch_cd - ahrs.pitch_sensor,
channel_pitch->servo_out = pitchController.get_servo_out(nav_pitch_cd - ahrs.pitch_sensor,
speed_scaler,
false);
} else {
@ -297,7 +297,7 @@ static void stabilize_acro(float speed_scaler)
user has non-zero pitch input, use a pure rate controller
*/
acro_state.locked_pitch = false;
channel_pitch->servo_out = g.pitchController.get_rate_out(pitch_rate, speed_scaler);
channel_pitch->servo_out = pitchController.get_rate_out(pitch_rate, speed_scaler);
}
/*
@ -345,9 +345,9 @@ static void stabilize()
// we are low, with no climb rate, and zero throttle, and very
// low ground speed. Zero the attitude controller
// integrators. This prevents integrator buildup pre-takeoff.
g.rollController.reset_I();
g.pitchController.reset_I();
g.yawController.reset_I();
rollController.reset_I();
pitchController.reset_I();
yawController.reset_I();
}
}
@ -386,7 +386,7 @@ static void calc_nav_yaw(float speed_scaler, float ch4_inf)
if (control_mode == STABILIZE && channel_rudder->control_in != 0) {
disable_integrator = true;
}
channel_rudder->servo_out = g.yawController.get_servo_out(speed_scaler, disable_integrator);
channel_rudder->servo_out = yawController.get_servo_out(speed_scaler, disable_integrator);
// add in rudder mixing from roll
channel_rudder->servo_out += channel_roll->servo_out * g.kff_rudder_mix;

11
ArduPlane/Parameters.h
View File

@ -408,12 +408,6 @@ public:
RC_Channel_aux rc_12;
#endif
// Attitude to servo controllers
//
AP_RollController rollController;
AP_PitchController pitchController;
AP_YawController yawController;
// PID controllers
PID pidWheelSteer;
@ -439,11 +433,6 @@ public:
rc_12 (CH_12),
#endif
// pass the aircraft parameters structure into the attitude controllers
rollController(aparm),
pitchController(aparm),
yawController(aparm),
// PID controller initial P initial I initial D initial imax
//-----------------------------------------------------------------------------------
pidWheelSteer (0, 0, 0, 0)

6
ArduPlane/Parameters.pde
View File

@ -808,15 +808,15 @@ const AP_Param::Info var_info[] PROGMEM = {
// @Group: RLL2SRV_
// @Path: ../libraries/APM_Control/AP_RollController.cpp
GGROUP(rollController, "RLL2SRV_", AP_RollController),
GOBJECT(rollController, "RLL2SRV_", AP_RollController),
// @Group: PTCH2SRV_
// @Path: ../libraries/APM_Control/AP_PitchController.cpp
GGROUP(pitchController, "PTCH2SRV_", AP_PitchController),
GOBJECT(pitchController, "PTCH2SRV_", AP_PitchController),
// @Group: YAW2SRV_
// @Path: ../libraries/APM_Control/AP_YawController.cpp
GGROUP(yawController, "YAW2SRV_", AP_YawController),
GOBJECT(yawController, "YAW2SRV_", AP_YawController),
// variables not in the g class which contain EEPROM saved variables

11
ArduPlane/system.pde
View File

@ -177,11 +177,6 @@ static void init_ardupilot()
// give AHRS the airspeed sensor
ahrs.set_airspeed(&airspeed);
// the axis controllers need access to the AHRS system
g.rollController.set_ahrs(&ahrs);
g.pitchController.set_ahrs(&ahrs);
g.yawController.set_ahrs(&ahrs);
// Do GPS init
g_gps = &g_gps_driver;
// GPS Initialization
@ -371,9 +366,9 @@ static void set_mode(enum FlightMode mode)
Log_Write_Mode(control_mode);
// reset attitude integrators on mode change
g.rollController.reset_I();
g.pitchController.reset_I();
g.yawController.reset_I();
rollController.reset_I();
pitchController.reset_I();
yawController.reset_I();
}
static void check_long_failsafe()