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

Plane: separate QSTABILIZE and QHOVER modes

Browse Source
This commit is contained in:
Andrew Tridgell 2015-12-26 18:45:42 +11:00
parent 6468fc6d93
commit ca85c332d6
8 changed files with 141 additions and 36 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
ArduPlane/ArduPlane.cpp
View File

@ -35,11 +35,11 @@
const AP_Scheduler::Task Plane::scheduler_tasks[] = {
SCHED_TASK(read_radio, 50, 700),
SCHED_TASK(check_short_failsafe, 50, 1000),
SCHED_TASK(ahrs_update, 50, 6400),
SCHED_TASK(ahrs_update, 400, 6400),
SCHED_TASK(update_speed_height, 50, 1600),
SCHED_TASK(update_flight_mode, 50, 1400),
SCHED_TASK(stabilize, 50, 3500),
SCHED_TASK(set_servos, 50, 1600),
SCHED_TASK(update_flight_mode, 400, 1400),
SCHED_TASK(stabilize, 400, 3500),
SCHED_TASK(set_servos, 400, 1600),
SCHED_TASK(read_control_switch, 7, 1000),
SCHED_TASK(gcs_retry_deferred, 50, 1000),
SCHED_TASK(update_GPS_50Hz, 50, 2500),
@ -172,6 +172,9 @@ void Plane::ahrs_update()
// frame yaw rate
steer_state.locked_course_err += ahrs.get_yaw_rate_earth() * G_Dt;
steer_state.locked_course_err = wrap_PI(steer_state.locked_course_err);
// update inertial_nav for quadplane
quadplane.inertial_nav.update(G_Dt);
}
/*
@ -686,7 +689,8 @@ void Plane::update_flight_mode(void)
//roll: -13788.000, pitch: -13698.000, thr: 0.000, rud: -13742.000
case HOVER: {
case QSTABILIZE:
case QHOVER: {
// set nav_roll and nav_pitch using sticks
nav_roll_cd = channel_roll->norm_input() * roll_limit_cd;
nav_roll_cd = constrain_int32(nav_roll_cd, -roll_limit_cd, roll_limit_cd);
@ -764,7 +768,8 @@ void Plane::update_navigation()
case AUTOTUNE:
case FLY_BY_WIRE_B:
case CIRCLE:
case HOVER:
case QSTABILIZE:
case QHOVER:
// nothing to do
break;
}

14
ArduPlane/Attitude.cpp
View File

@ -140,7 +140,8 @@ void Plane::stabilize_stick_mixing_direct()
control_mode == AUTOTUNE ||
control_mode == FLY_BY_WIRE_B ||
control_mode == CRUISE ||
control_mode == HOVER ||
control_mode == QSTABILIZE ||
control_mode == QHOVER ||
control_mode == TRAINING) {
return;
}
@ -160,7 +161,8 @@ void Plane::stabilize_stick_mixing_fbw()
control_mode == AUTOTUNE ||
control_mode == FLY_BY_WIRE_B ||
control_mode == CRUISE ||
control_mode == HOVER ||
control_mode == QSTABILIZE ||
control_mode == QHOVER ||
control_mode == TRAINING ||
(control_mode == AUTO && g.auto_fbw_steer)) {
return;
@ -356,8 +358,10 @@ void Plane::stabilize()
stabilize_training(speed_scaler);
} else if (control_mode == ACRO) {
stabilize_acro(speed_scaler);
} else if (control_mode == HOVER) {
quadplane.stabilize_hover();
} else if (control_mode == QSTABILIZE) {
quadplane.control_stabilize();
} else if (control_mode == QHOVER) {
quadplane.control_hover();
} else {
if (g.stick_mixing == STICK_MIXING_FBW && control_mode != STABILIZE) {
stabilize_stick_mixing_fbw();
@ -922,7 +926,7 @@ void Plane::set_servos(void)
guided_throttle_passthru) {
// manual pass through of throttle while in GUIDED
channel_throttle->radio_out = channel_throttle->radio_in;
} else if (control_mode == HOVER) {
} else if (control_mode == QSTABILIZE || control_mode == QHOVER) {
// no forward throttle for now
channel_throttle->servo_out = 0;
channel_throttle->calc_pwm();

6
ArduPlane/GCS_Mavlink.cpp
View File

@ -39,7 +39,8 @@ void Plane::send_heartbeat(mavlink_channel_t chan)
case FLY_BY_WIRE_A:
case AUTOTUNE:
case FLY_BY_WIRE_B:
case HOVER:
case QSTABILIZE:
case QHOVER:
case CRUISE:
base_mode = MAV_MODE_FLAG_STABILIZE_ENABLED;
break;
@ -170,7 +171,8 @@ void Plane::send_extended_status1(mavlink_channel_t chan)
case STABILIZE:
case FLY_BY_WIRE_A:
case AUTOTUNE:
case HOVER:
case QSTABILIZE:
case QHOVER:
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL; // 3D angular rate control
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION; // attitude stabilisation
break;

3
ArduPlane/defines.h
View File

@ -64,7 +64,8 @@ enum FlightMode {
LOITER = 12,
GUIDED = 15,
INITIALISING = 16,
HOVER = 17
QSTABILIZE = 17,
QHOVER = 18
};
// type of stick mixing enabled

14
ArduPlane/events.cpp
View File

@ -13,7 +13,6 @@ void Plane::failsafe_short_on_event(enum failsafe_state fstype)
case MANUAL:
case STABILIZE:
case ACRO:
case HOVER:
case FLY_BY_WIRE_A:
case AUTOTUNE:
case FLY_BY_WIRE_B:
@ -28,6 +27,12 @@ void Plane::failsafe_short_on_event(enum failsafe_state fstype)
}
break;
case QSTABILIZE:
failsafe.saved_mode = control_mode;
failsafe.saved_mode_set = 1;
set_mode(QHOVER);
break;
case AUTO:
case GUIDED:
case LOITER:
@ -44,6 +49,7 @@ void Plane::failsafe_short_on_event(enum failsafe_state fstype)
case CIRCLE:
case RTL:
case QHOVER:
default:
break;
}
@ -62,7 +68,6 @@ void Plane::failsafe_long_on_event(enum failsafe_state fstype)
case MANUAL:
case STABILIZE:
case ACRO:
case HOVER:
case FLY_BY_WIRE_A:
case AUTOTUNE:
case FLY_BY_WIRE_B:
@ -76,6 +81,10 @@ void Plane::failsafe_long_on_event(enum failsafe_state fstype)
}
break;
case QSTABILIZE:
set_mode(QHOVER);
break;
case AUTO:
case GUIDED:
case LOITER:
@ -87,6 +96,7 @@ void Plane::failsafe_long_on_event(enum failsafe_state fstype)
break;
case RTL:
case QHOVER:
default:
break;
}

99
ArduPlane/quadplane.cpp
View File

@ -141,35 +141,35 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = {
// @User: Advanced
AP_GROUPINFO("TRANSITION_MS", 10, QuadPlane, transition_time_ms, 10000),
// @Param: POS_Z_P
// @Param: PZ_P
// @DisplayName: Position (vertical) controller P gain
// @Description: Position (vertical) controller P gain. Converts the difference between the desired altitude and actual altitude into a climb or descent rate which is passed to the throttle rate controller
// @Range: 1.000 3.000
// @User: Standard
AP_SUBGROUPINFO(p_alt_hold, "PZ_", 11, QuadPlane, AC_P),
// @Param: POS_XY_P
// @Param: PXY_P
// @DisplayName: Position (horizonal) controller P gain
// @Description: Loiter position controller P gain. Converts the distance (in the latitude direction) to the target location into a desired speed which is then passed to the loiter latitude rate controller
// @Range: 0.500 2.000
// @User: Standard
AP_SUBGROUPINFO(p_pos_xy, "PXY_", 12, QuadPlane, AC_P),
// @Param: VEL_XY_P
// @Param: VXY_P
// @DisplayName: Velocity (horizontal) P gain
// @Description: Velocity (horizontal) P gain. Converts the difference between desired velocity to a target acceleration
// @Range: 0.1 6.0
// @Increment: 0.1
// @User: Advanced
// @Param: VEL_XY_I
// @Param: VXY_I
// @DisplayName: Velocity (horizontal) I gain
// @Description: Velocity (horizontal) I gain. Corrects long-term difference in desired velocity to a target acceleration
// @Range: 0.02 1.00
// @Increment: 0.01
// @User: Advanced
// @Param: VEL_XY_IMAX
// @Param: VXY_IMAX
// @DisplayName: Velocity (horizontal) integrator maximum
// @Description: Velocity (horizontal) integrator maximum. Constrains the target acceleration that the I gain will output
// @Range: 0 4500
@ -178,39 +178,39 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = {
// @User: Advanced
AP_SUBGROUPINFO(pi_vel_xy, "VXY_", 13, QuadPlane, AC_PI_2D),
// @Param: VEL_Z_P
// @Param: VZ_P
// @DisplayName: Velocity (vertical) P gain
// @Description: Velocity (vertical) P gain. Converts the difference between desired vertical speed and actual speed into a desired acceleration that is passed to the throttle acceleration controller
// @Range: 1.000 8.000
// @User: Standard
AP_SUBGROUPINFO(p_vel_z, "VZ_", 14, QuadPlane, AC_P),
// @Param: ACCEL_Z_P
// @Param: AZ_P
// @DisplayName: Throttle acceleration controller P gain
// @Description: Throttle acceleration controller P gain. Converts the difference between desired vertical acceleration and actual acceleration into a motor output
// @Range: 0.500 1.500
// @User: Standard
// @Param: ACCEL_Z_I
// @Param: AZ_I
// @DisplayName: Throttle acceleration controller I gain
// @Description: Throttle acceleration controller I gain. Corrects long-term difference in desired vertical acceleration and actual acceleration
// @Range: 0.000 3.000
// @User: Standard
// @Param: ACCEL_Z_IMAX
// @Param: AZ_IMAX
// @DisplayName: Throttle acceleration controller I gain maximum
// @Description: Throttle acceleration controller I gain maximum. Constrains the maximum pwm that the I term will generate
// @Range: 0 1000
// @Units: Percent*10
// @User: Standard
// @Param: ACCEL_Z_D
// @Param: AZ_D
// @DisplayName: Throttle acceleration controller D gain
// @Description: Throttle acceleration controller D gain. Compensates for short-term change in desired vertical acceleration vs actual acceleration
// @Range: 0.000 0.400
// @User: Standard
// @Param: ACCEL_Z_FILT_HZ
// @Param: AZ_FILT_HZ
// @DisplayName: Throttle acceleration filter
// @Description: Filter applied to acceleration to reduce noise. Lower values reduce noise but add delay.
// @Range: 1.000 100.000
@ -218,10 +218,28 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = {
// @User: Standard
AP_SUBGROUPINFO(pid_accel_z, "AZ_", 15, QuadPlane, AC_PID),
// @Group: POSCON_
// @Group: P_
// @Path: ../libraries/AC_AttitudeControl/AC_PosControl.cpp
AP_SUBGROUPINFO(pos_control, "P", 16, QuadPlane, AC_PosControl),
// @Param: VELZ_MAX
// @DisplayName: Pilot maximum vertical speed
// @Description: The maximum vertical velocity the pilot may request in cm/s
// @Units: Centimeters/Second
// @Range: 50 500
// @Increment: 10
// @User: Standard
AP_GROUPINFO("VELZ_MAX", 17, QuadPlane, pilot_velocity_z_max, 250),
// @Param: ACCEL_Z
// @DisplayName: Pilot vertical acceleration
// @Description: The vertical acceleration used when pilot is controlling the altitude
// @Units: cm/s/s
// @Range: 50 500
// @Increment: 10
// @User: Standard
AP_GROUPINFO("ACCEL_Z", 18, QuadPlane, pilot_accel_z, 250),
AP_GROUPEND
};
@ -244,17 +262,27 @@ void QuadPlane::setup(void)
pid_rate_pitch.set_dt(plane.ins.get_loop_delta_t());
pid_rate_yaw.set_dt(plane.ins.get_loop_delta_t());
pid_accel_z.set_dt(plane.ins.get_loop_delta_t());
pos_control.set_dt(plane.ins.get_loop_delta_t());
}
// stabilize in hover mode
void QuadPlane::stabilize_hover(void)
// init quadplane stabilize mode
void QuadPlane::init_stabilize(void)
{
// nothing to do
}
// quadplane stabilize mode
void QuadPlane::control_stabilize(void)
{
// max 100 degrees/sec for now
const float yaw_rate_max_dps = 100;
float yaw_rate_ef_cds = plane.channel_rudder->norm_input() * 100 * yaw_rate_max_dps;
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(plane.nav_roll_cd, plane.nav_pitch_cd, yaw_rate_ef_cds, smoothing_gain);
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(plane.nav_roll_cd,
plane.nav_pitch_cd,
yaw_rate_ef_cds,
smoothing_gain);
// output pilot's throttle
int16_t pilot_throttle_scaled = plane.channel_throttle->control_in * 10;
@ -267,6 +295,45 @@ void QuadPlane::stabilize_hover(void)
last_throttle = pilot_throttle_scaled;
}
// init quadplane hover mode
void QuadPlane::init_hover(void)
{
// initialize vertical speeds and leash lengths
pos_control.set_speed_z(-pilot_velocity_z_max, pilot_velocity_z_max);
pos_control.set_accel_z(pilot_accel_z);
// initialise position and desired velocity
pos_control.set_alt_target(inertial_nav.get_altitude());
pos_control.set_desired_velocity_z(inertial_nav.get_velocity_z());
}
void QuadPlane::control_hover(void)
{
// initialize vertical speeds and acceleration
pos_control.set_speed_z(-pilot_velocity_z_max, pilot_velocity_z_max);
pos_control.set_accel_z(pilot_accel_z);
const float yaw_rate_max_dps = 100;
float yaw_rate_ef_cds = plane.channel_rudder->norm_input() * 100 * yaw_rate_max_dps;
// get pilot desired climb rate
float target_climb_rate = pilot_velocity_z_max * (plane.channel_throttle->control_in - 50) / 50.0;
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(plane.nav_roll_cd,
plane.nav_pitch_cd,
yaw_rate_ef_cds,
smoothing_gain);
// call position controller
pos_control.set_alt_target_from_climb_rate_ff(target_climb_rate, plane.G_Dt, false);
pos_control.update_z_controller();
last_run_ms = millis();
// run low level rate controllers that only require IMU data
attitude_control.rate_controller_run();
}
// set motor arming
void QuadPlane::set_armed(bool armed)
{
@ -299,7 +366,7 @@ void QuadPlane::update_transition(void)
*/
void QuadPlane::update(void)
{
if (plane.control_mode != HOVER) {
if (plane.control_mode != QSTABILIZE && plane.control_mode != QHOVER) {
update_transition();
} else {
motors.output();

13
ArduPlane/quadplane.h
View File

@ -21,8 +21,11 @@ public:
// setup quadplane
void setup(void);
// stabilize in hover mode
void stabilize_hover(void);
// main entry points for VTOL flight modes
void init_stabilize(void);
void control_stabilize(void);
void init_hover(void);
void control_hover(void);
// update transition handling
void update(void);
@ -57,6 +60,12 @@ private:
p_alt_hold, p_vel_z, pid_accel_z,
p_pos_xy, pi_vel_xy};
// maximum vertical velocity the pilot may request
AP_Int16 pilot_velocity_z_max;
// vertical acceleration the pilot may request
AP_Int16 pilot_accel_z;
// update transition handling
void update_transition(void);

11
ArduPlane/system.cpp
View File

@ -446,8 +446,14 @@ void Plane::set_mode(enum FlightMode mode)
set_guided_WP();
break;
case HOVER:
case QSTABILIZE:
auto_throttle_mode = false;
quadplane.init_stabilize();
break;
case QHOVER:
auto_throttle_mode = false;
quadplane.init_hover();
break;
}
@ -483,7 +489,8 @@ bool Plane::mavlink_set_mode(uint8_t mode)
case AUTO:
case RTL:
case LOITER:
case HOVER:
case QSTABILIZE:
case QHOVER:
set_mode((enum FlightMode)mode);
return true;
}