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_L1_Control L1_controller(ahrs);
static AP_TECS TECS_controller(&ahrs, aparm); 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 #if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
SITL sitl; SITL sitl;
#endif #endif

36
ArduPlane/Attitude.pde
View File

@ -77,9 +77,9 @@ static void stabilize_roll(float speed_scaler)
if (control_mode == STABILIZE && channel_roll->control_in != 0) { if (control_mode == STABILIZE && channel_roll->control_in != 0) {
disable_integrator = true; 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, speed_scaler,
disable_integrator); disable_integrator);
} }
/* /*
@ -94,9 +94,9 @@ static void stabilize_pitch(float speed_scaler)
if (control_mode == STABILIZE && channel_pitch->control_in != 0) { if (control_mode == STABILIZE && channel_pitch->control_in != 0) {
disable_integrator = true; 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, speed_scaler,
disable_integrator); disable_integrator);
} }
/* /*
@ -265,16 +265,16 @@ static void stabilize_acro(float speed_scaler)
nav_roll_cd = ahrs.roll_sensor + roll_error_cd; nav_roll_cd = ahrs.roll_sensor + roll_error_cd;
// try to reduce the integrated angular error to zero. We set // try to reduce the integrated angular error to zero. We set
// 'stabilze' to true, which disables the roll integrator // '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, speed_scaler,
true); true);
} else { } else {
/* /*
aileron stick is non-zero, use pure rate control until the aileron stick is non-zero, use pure rate control until the
user releases the stick user releases the stick
*/ */
acro_state.locked_roll = false; 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) { if (pitch_rate == 0) {
@ -289,15 +289,15 @@ static void stabilize_acro(float speed_scaler)
// try to hold the locked pitch. Note that we have the pitch // try to hold the locked pitch. Note that we have the pitch
// integrator enabled, which helps with inverted flight // integrator enabled, which helps with inverted flight
nav_pitch_cd = acro_state.locked_pitch_cd; 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, speed_scaler,
false); false);
} else { } else {
/* /*
user has non-zero pitch input, use a pure rate controller user has non-zero pitch input, use a pure rate controller
*/ */
acro_state.locked_pitch = false; 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 // we are low, with no climb rate, and zero throttle, and very
// low ground speed. Zero the attitude controller // low ground speed. Zero the attitude controller
// integrators. This prevents integrator buildup pre-takeoff. // integrators. This prevents integrator buildup pre-takeoff.
g.rollController.reset_I(); rollController.reset_I();
g.pitchController.reset_I(); pitchController.reset_I();
g.yawController.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) { if (control_mode == STABILIZE && channel_rudder->control_in != 0) {
disable_integrator = true; 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 // add in rudder mixing from roll
channel_rudder->servo_out += channel_roll->servo_out * g.kff_rudder_mix; 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; RC_Channel_aux rc_12;
#endif #endif
// Attitude to servo controllers
//
AP_RollController rollController;
AP_PitchController pitchController;
AP_YawController yawController;
// PID controllers // PID controllers
PID pidWheelSteer; PID pidWheelSteer;
@ -439,11 +433,6 @@ public:
rc_12 (CH_12), rc_12 (CH_12),
#endif #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 // PID controller initial P initial I initial D initial imax
//----------------------------------------------------------------------------------- //-----------------------------------------------------------------------------------
pidWheelSteer (0, 0, 0, 0) pidWheelSteer (0, 0, 0, 0)

6
ArduPlane/Parameters.pde
View File

@ -808,15 +808,15 @@ const AP_Param::Info var_info[] PROGMEM = {
// @Group: RLL2SRV_ // @Group: RLL2SRV_
// @Path: ../libraries/APM_Control/AP_RollController.cpp // @Path: ../libraries/APM_Control/AP_RollController.cpp
GGROUP(rollController, "RLL2SRV_", AP_RollController), GOBJECT(rollController, "RLL2SRV_", AP_RollController),
// @Group: PTCH2SRV_ // @Group: PTCH2SRV_
// @Path: ../libraries/APM_Control/AP_PitchController.cpp // @Path: ../libraries/APM_Control/AP_PitchController.cpp
GGROUP(pitchController, "PTCH2SRV_", AP_PitchController), GOBJECT(pitchController, "PTCH2SRV_", AP_PitchController),
// @Group: YAW2SRV_ // @Group: YAW2SRV_
// @Path: ../libraries/APM_Control/AP_YawController.cpp // @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 // 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 // give AHRS the airspeed sensor
ahrs.set_airspeed(&airspeed); 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 // Do GPS init
g_gps = &g_gps_driver; g_gps = &g_gps_driver;
// GPS Initialization // GPS Initialization
@ -371,9 +366,9 @@ static void set_mode(enum FlightMode mode)
Log_Write_Mode(control_mode); Log_Write_Mode(control_mode);
// reset attitude integrators on mode change // reset attitude integrators on mode change
g.rollController.reset_I(); rollController.reset_I();
g.pitchController.reset_I(); pitchController.reset_I();
g.yawController.reset_I(); yawController.reset_I();
} }
static void check_long_failsafe() static void check_long_failsafe()