mirror of https://github.com/ArduPilot/ardupilot
Plane: Initial implementation of quadplane
adds "HOVER" mode
This commit is contained in:
Andrew Tridgell
parent
b187a0c6eb
commit
6468fc6d93
|
|
@ -684,6 +684,21 @@ void Plane::update_flight_mode(void)
|
|||
steering_control.steering = steering_control.rudder = channel_rudder->pwm_to_angle();
|
||||
break;
|
||||
//roll: -13788.000, pitch: -13698.000, thr: 0.000, rud: -13742.000
|
||||
|
||||
|
||||
case HOVER: {
|
||||
// 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);
|
||||
float pitch_input = channel_pitch->norm_input();
|
||||
if (pitch_input > 0) {
|
||||
nav_pitch_cd = pitch_input * aparm.pitch_limit_max_cd;
|
||||
} else {
|
||||
nav_pitch_cd = -(pitch_input * pitch_limit_min_cd);
|
||||
}
|
||||
nav_pitch_cd = constrain_int32(nav_pitch_cd, pitch_limit_min_cd, aparm.pitch_limit_max_cd.get());
|
||||
break;
|
||||
}
|
||||
|
||||
case INITIALISING:
|
||||
// handled elsewhere
|
||||
|
|
@ -749,6 +764,7 @@ void Plane::update_navigation()
|
|||
case AUTOTUNE:
|
||||
case FLY_BY_WIRE_B:
|
||||
case CIRCLE:
|
||||
case HOVER:
|
||||
// nothing to do
|
||||
break;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -140,6 +140,7 @@ void Plane::stabilize_stick_mixing_direct()
|
|||
control_mode == AUTOTUNE ||
|
||||
control_mode == FLY_BY_WIRE_B ||
|
||||
control_mode == CRUISE ||
|
||||
control_mode == HOVER ||
|
||||
control_mode == TRAINING) {
|
||||
return;
|
||||
}
|
||||
|
|
@ -159,6 +160,7 @@ void Plane::stabilize_stick_mixing_fbw()
|
|||
control_mode == AUTOTUNE ||
|
||||
control_mode == FLY_BY_WIRE_B ||
|
||||
control_mode == CRUISE ||
|
||||
control_mode == HOVER ||
|
||||
control_mode == TRAINING ||
|
||||
(control_mode == AUTO && g.auto_fbw_steer)) {
|
||||
return;
|
||||
|
|
@ -354,6 +356,8 @@ 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 (g.stick_mixing == STICK_MIXING_FBW && control_mode != STABILIZE) {
|
||||
stabilize_stick_mixing_fbw();
|
||||
|
|
@ -760,6 +764,9 @@ void Plane::set_servos(void)
|
|||
{
|
||||
int16_t last_throttle = channel_throttle->radio_out;
|
||||
|
||||
// do any transition updates for quadplane
|
||||
quadplane.update();
|
||||
|
||||
if (control_mode == AUTO && auto_state.idle_mode) {
|
||||
// special handling for balloon launch
|
||||
set_servos_idle();
|
||||
|
|
@ -915,6 +922,10 @@ 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) {
|
||||
// no forward throttle for now
|
||||
channel_throttle->servo_out = 0;
|
||||
channel_throttle->calc_pwm();
|
||||
} else {
|
||||
// normal throttle calculation based on servo_out
|
||||
channel_throttle->calc_pwm();
|
||||
|
|
|
|||
|
|
@ -39,6 +39,7 @@ void Plane::send_heartbeat(mavlink_channel_t chan)
|
|||
case FLY_BY_WIRE_A:
|
||||
case AUTOTUNE:
|
||||
case FLY_BY_WIRE_B:
|
||||
case HOVER:
|
||||
case CRUISE:
|
||||
base_mode = MAV_MODE_FLAG_STABILIZE_ENABLED;
|
||||
break;
|
||||
|
|
@ -169,6 +170,7 @@ void Plane::send_extended_status1(mavlink_channel_t chan)
|
|||
case STABILIZE:
|
||||
case FLY_BY_WIRE_A:
|
||||
case AUTOTUNE:
|
||||
case HOVER:
|
||||
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;
|
||||
|
|
|
|||
|
|
@ -1034,6 +1034,10 @@ const AP_Param::Info Plane::var_info[] = {
|
|||
// @Path: ../libraries/AP_ADSB/AP_ADSB.cpp
|
||||
GOBJECT(adsb, "ADSB_", AP_ADSB),
|
||||
|
||||
// @Group: Q_
|
||||
// @Path: quadplane.cpp
|
||||
GOBJECT(quadplane, "Q_", QuadPlane),
|
||||
|
||||
// RC channel
|
||||
//-----------
|
||||
// @Group: RC1_
|
||||
|
|
|
|||
|
|
@ -276,6 +276,7 @@ public:
|
|||
k_param_kff_pitch_to_throttle, // unused
|
||||
k_param_kff_throttle_to_pitch,
|
||||
k_param_scaling_speed,
|
||||
k_param_quadplane,
|
||||
|
||||
//
|
||||
// 210: flight modes
|
||||
|
|
|
|||
|
|
@ -96,6 +96,8 @@
|
|||
#include <AP_Parachute/AP_Parachute.h>
|
||||
#include <AP_ADSB/AP_ADSB.h>
|
||||
|
||||
#include "quadplane.h"
|
||||
|
||||
// Configuration
|
||||
#include "config.h"
|
||||
|
||||
|
|
@ -135,6 +137,7 @@ public:
|
|||
friend class GCS_MAVLINK;
|
||||
friend class Parameters;
|
||||
friend class AP_Arming_Plane;
|
||||
friend class QuadPlane;
|
||||
|
||||
Plane(void);
|
||||
|
||||
|
|
@ -712,7 +715,9 @@ private:
|
|||
// time that rudder arming has been running
|
||||
uint32_t rudder_arm_timer;
|
||||
|
||||
|
||||
// support for quadcopter-plane
|
||||
QuadPlane quadplane{ahrs};
|
||||
|
||||
void demo_servos(uint8_t i);
|
||||
void adjust_nav_pitch_throttle(void);
|
||||
void update_load_factor(void);
|
||||
|
|
|
|||
|
|
@ -63,7 +63,8 @@ enum FlightMode {
|
|||
RTL = 11,
|
||||
LOITER = 12,
|
||||
GUIDED = 15,
|
||||
INITIALISING = 16
|
||||
INITIALISING = 16,
|
||||
HOVER = 17
|
||||
};
|
||||
|
||||
// type of stick mixing enabled
|
||||
|
|
|
|||
|
|
@ -13,6 +13,7 @@ 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:
|
||||
|
|
@ -61,6 +62,7 @@ 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:
|
||||
|
|
|
|||
|
|
@ -49,4 +49,7 @@ LIBRARIES += AP_RSSI
|
|||
LIBRARIES += AP_RPM
|
||||
LIBRARIES += AP_Parachute
|
||||
LIBRARIES += AP_ADSB
|
||||
|
||||
LIBRARIES += AP_Motors
|
||||
LIBRARIES += AC_AttitudeControl
|
||||
LIBRARIES += AC_PID
|
||||
LIBRARIES += AP_InertialNav
|
||||
|
|
|
|||
|
|
@ -0,0 +1,307 @@
|
|||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||
|
||||
#include "Plane.h"
|
||||
|
||||
const AP_Param::GroupInfo QuadPlane::var_info[] = {
|
||||
|
||||
// @Group: MOT_
|
||||
// @Path: ../libraries/AP_Motors/AP_MotorsMulticopter.cpp
|
||||
AP_SUBGROUPINFO(motors, "M_", 1, QuadPlane, AP_MotorsQuad),
|
||||
|
||||
// @Param: RT_RLL_P
|
||||
// @DisplayName: Roll axis rate controller P gain
|
||||
// @Description: Roll axis rate controller P gain. Converts the difference between desired roll rate and actual roll rate into a motor speed output
|
||||
// @Range: 0.08 0.30
|
||||
// @Increment: 0.005
|
||||
// @User: Standard
|
||||
|
||||
// @Param: RT_RLL_I
|
||||
// @DisplayName: Roll axis rate controller I gain
|
||||
// @Description: Roll axis rate controller I gain. Corrects long-term difference in desired roll rate vs actual roll rate
|
||||
// @Range: 0.01 0.5
|
||||
// @Increment: 0.01
|
||||
// @User: Standard
|
||||
|
||||
// @Param: RT_RLL_IMAX
|
||||
// @DisplayName: Roll axis rate controller I gain maximum
|
||||
// @Description: Roll axis rate controller I gain maximum. Constrains the maximum motor output that the I gain will output
|
||||
// @Range: 0 4500
|
||||
// @Increment: 10
|
||||
// @Units: Percent*10
|
||||
// @User: Standard
|
||||
|
||||
// @Param: RT_RLL_D
|
||||
// @DisplayName: Roll axis rate controller D gain
|
||||
// @Description: Roll axis rate controller D gain. Compensates for short-term change in desired roll rate vs actual roll rate
|
||||
// @Range: 0.001 0.02
|
||||
// @Increment: 0.001
|
||||
// @User: Standard
|
||||
AP_SUBGROUPINFO(pid_rate_roll, "RT_RLL_", 2, QuadPlane, AC_PID),
|
||||
|
||||
// @Param: RT_PIT_P
|
||||
// @DisplayName: Pitch axis rate controller P gain
|
||||
// @Description: Pitch axis rate controller P gain. Converts the difference between desired pitch rate and actual pitch rate into a motor speed output
|
||||
// @Range: 0.08 0.30
|
||||
// @Increment: 0.005
|
||||
// @User: Standard
|
||||
|
||||
// @Param: RT_PIT_I
|
||||
// @DisplayName: Pitch axis rate controller I gain
|
||||
// @Description: Pitch axis rate controller I gain. Corrects long-term difference in desired pitch rate vs actual pitch rate
|
||||
// @Range: 0.01 0.5
|
||||
// @Increment: 0.01
|
||||
// @User: Standard
|
||||
|
||||
// @Param: RT_PIT_IMAX
|
||||
// @DisplayName: Pitch axis rate controller I gain maximum
|
||||
// @Description: Pitch axis rate controller I gain maximum. Constrains the maximum motor output that the I gain will output
|
||||
// @Range: 0 4500
|
||||
// @Increment: 10
|
||||
// @Units: Percent*10
|
||||
// @User: Standard
|
||||
|
||||
// @Param: RT_PIT_D
|
||||
// @DisplayName: Pitch axis rate controller D gain
|
||||
// @Description: Pitch axis rate controller D gain. Compensates for short-term change in desired pitch rate vs actual pitch rate
|
||||
// @Range: 0.001 0.02
|
||||
// @Increment: 0.001
|
||||
// @User: Standard
|
||||
AP_SUBGROUPINFO(pid_rate_pitch, "RT_PIT_", 3, QuadPlane, AC_PID),
|
||||
|
||||
// @Param: RT_YAW_P
|
||||
// @DisplayName: Yaw axis rate controller P gain
|
||||
// @Description: Yaw axis rate controller P gain. Converts the difference between desired yaw rate and actual yaw rate into a motor speed output
|
||||
// @Range: 0.150 0.50
|
||||
// @Increment: 0.005
|
||||
// @User: Standard
|
||||
|
||||
// @Param: RT_YAW_I
|
||||
// @DisplayName: Yaw axis rate controller I gain
|
||||
// @Description: Yaw axis rate controller I gain. Corrects long-term difference in desired yaw rate vs actual yaw rate
|
||||
// @Range: 0.010 0.05
|
||||
// @Increment: 0.01
|
||||
// @User: Standard
|
||||
|
||||
// @Param: RT_YAW_IMAX
|
||||
// @DisplayName: Yaw axis rate controller I gain maximum
|
||||
// @Description: Yaw axis rate controller I gain maximum. Constrains the maximum motor output that the I gain will output
|
||||
// @Range: 0 4500
|
||||
// @Increment: 10
|
||||
// @Units: Percent*10
|
||||
// @User: Standard
|
||||
|
||||
// @Param: RT_YAW_D
|
||||
// @DisplayName: Yaw axis rate controller D gain
|
||||
// @Description: Yaw axis rate controller D gain. Compensates for short-term change in desired yaw rate vs actual yaw rate
|
||||
// @Range: 0.000 0.02
|
||||
// @Increment: 0.001
|
||||
// @User: Standard
|
||||
AP_SUBGROUPINFO(pid_rate_yaw, "RT_YAW_", 4, QuadPlane, AC_PID),
|
||||
|
||||
// P controllers
|
||||
//--------------
|
||||
// @Param: STB_RLL_P
|
||||
// @DisplayName: Roll axis stabilize controller P gain
|
||||
// @Description: Roll axis stabilize (i.e. angle) controller P gain. Converts the error between the desired roll angle and actual angle to a desired roll rate
|
||||
// @Range: 3.000 12.000
|
||||
// @User: Standard
|
||||
AP_SUBGROUPINFO(p_stabilize_roll, "STB_R_", 5, QuadPlane, AC_P),
|
||||
|
||||
// @Param: STB_PIT_P
|
||||
// @DisplayName: Pitch axis stabilize controller P gain
|
||||
// @Description: Pitch axis stabilize (i.e. angle) controller P gain. Converts the error between the desired pitch angle and actual angle to a desired pitch rate
|
||||
// @Range: 3.000 12.000
|
||||
// @User: Standard
|
||||
AP_SUBGROUPINFO(p_stabilize_pitch, "STB_P_", 6, QuadPlane, AC_P),
|
||||
|
||||
// @Param: STB_YAW_P
|
||||
// @DisplayName: Yaw axis stabilize controller P gain
|
||||
// @Description: Yaw axis stabilize (i.e. angle) controller P gain. Converts the error between the desired yaw angle and actual angle to a desired yaw rate
|
||||
// @Range: 3.000 6.000
|
||||
// @User: Standard
|
||||
AP_SUBGROUPINFO(p_stabilize_yaw, "STB_Y_", 7, QuadPlane, AC_P),
|
||||
|
||||
// @Group: ATC_
|
||||
// @Path: ../libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
|
||||
AP_SUBGROUPINFO(attitude_control, "A_", 8, QuadPlane, AC_AttitudeControl),
|
||||
|
||||
// @Param: ANGLE_MAX
|
||||
// @DisplayName: Angle Max
|
||||
// @Description: Maximum lean angle in all flight modes
|
||||
// @Units: Centi-degrees
|
||||
// @Range: 1000 8000
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("ANGLE_MAX", 9, QuadPlane, aparm.angle_max, 4500),
|
||||
|
||||
// @Param: TRANSITION_MS
|
||||
// @DisplayName: Transition time
|
||||
// @Description: Transition time in milliseconds
|
||||
// @Units: milli-seconds
|
||||
// @Range: 0 30000
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("TRANSITION_MS", 10, QuadPlane, transition_time_ms, 10000),
|
||||
|
||||
// @Param: POS_Z_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
|
||||
// @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
|
||||
// @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
|
||||
// @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
|
||||
// @DisplayName: Velocity (horizontal) integrator maximum
|
||||
// @Description: Velocity (horizontal) integrator maximum. Constrains the target acceleration that the I gain will output
|
||||
// @Range: 0 4500
|
||||
// @Increment: 10
|
||||
// @Units: cm/s/s
|
||||
// @User: Advanced
|
||||
AP_SUBGROUPINFO(pi_vel_xy, "VXY_", 13, QuadPlane, AC_PI_2D),
|
||||
|
||||
// @Param: VEL_Z_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
|
||||
// @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
|
||||
// @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
|
||||
// @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
|
||||
// @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
|
||||
// @DisplayName: Throttle acceleration filter
|
||||
// @Description: Filter applied to acceleration to reduce noise. Lower values reduce noise but add delay.
|
||||
// @Range: 1.000 100.000
|
||||
// @Units: Hz
|
||||
// @User: Standard
|
||||
AP_SUBGROUPINFO(pid_accel_z, "AZ_", 15, QuadPlane, AC_PID),
|
||||
|
||||
// @Group: POSCON_
|
||||
// @Path: ../libraries/AC_AttitudeControl/AC_PosControl.cpp
|
||||
AP_SUBGROUPINFO(pos_control, "P", 16, QuadPlane, AC_PosControl),
|
||||
|
||||
AP_GROUPEND
|
||||
};
|
||||
|
||||
QuadPlane::QuadPlane(AP_AHRS_NavEKF &_ahrs) :
|
||||
ahrs(_ahrs)
|
||||
{
|
||||
AP_Param::setup_object_defaults(this, var_info);
|
||||
}
|
||||
|
||||
void QuadPlane::setup(void)
|
||||
{
|
||||
motors.set_frame_orientation(AP_MOTORS_QUADPLANE);
|
||||
motors.set_throttle_range(0, 1000, 2000);
|
||||
motors.set_hover_throttle(500);
|
||||
motors.set_update_rate(490);
|
||||
motors.set_interlock(true);
|
||||
motors.set_loop_rate(plane.ins.get_sample_rate());
|
||||
attitude_control.set_dt(plane.ins.get_loop_delta_t());
|
||||
pid_rate_roll.set_dt(plane.ins.get_loop_delta_t());
|
||||
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());
|
||||
}
|
||||
|
||||
// stabilize in hover mode
|
||||
void QuadPlane::stabilize_hover(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);
|
||||
|
||||
// output pilot's throttle
|
||||
int16_t pilot_throttle_scaled = plane.channel_throttle->control_in * 10;
|
||||
attitude_control.set_throttle_out(pilot_throttle_scaled, true, 0);
|
||||
|
||||
// run low level rate controllers that only require IMU data
|
||||
attitude_control.rate_controller_run();
|
||||
|
||||
last_run_ms = millis();
|
||||
last_throttle = pilot_throttle_scaled;
|
||||
}
|
||||
|
||||
// set motor arming
|
||||
void QuadPlane::set_armed(bool armed)
|
||||
{
|
||||
motors.armed(armed);
|
||||
if (armed) {
|
||||
motors.enable();
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
update for transition from quadplane
|
||||
*/
|
||||
void QuadPlane::update_transition(void)
|
||||
{
|
||||
if (millis() - last_run_ms < (unsigned)transition_time_ms && plane.control_mode != MANUAL) {
|
||||
// we are transitioning. Call attitude controller
|
||||
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(plane.nav_roll_cd, plane.nav_pitch_cd, 0, smoothing_gain);
|
||||
// and degrade throttle
|
||||
int16_t throttle_scaled = last_throttle * (transition_time_ms - (millis() - last_run_ms)) / (float)transition_time_ms;
|
||||
attitude_control.set_throttle_out(throttle_scaled, true, 0);
|
||||
motors.output();
|
||||
} else {
|
||||
motors.output_min();
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
update motor output for quadplane
|
||||
*/
|
||||
void QuadPlane::update(void)
|
||||
{
|
||||
if (plane.control_mode != HOVER) {
|
||||
update_transition();
|
||||
} else {
|
||||
motors.output();
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,72 @@
|
|||
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||
|
||||
#include <AP_Motors/AP_Motors.h>
|
||||
#include <AC_PID/AC_PID.h>
|
||||
#include <AC_AttitudeControl/AC_AttitudeControl_Multi.h> // Attitude control library
|
||||
#include <AP_InertialNav/AP_InertialNav.h>
|
||||
#include <AC_AttitudeControl/AC_PosControl.h>
|
||||
|
||||
/*
|
||||
QuadPlane specific functionality
|
||||
*/
|
||||
class QuadPlane
|
||||
{
|
||||
public:
|
||||
friend class Plane;
|
||||
QuadPlane(AP_AHRS_NavEKF &_ahrs);
|
||||
|
||||
// var_info for holding Parameter information
|
||||
static const struct AP_Param::GroupInfo var_info[];
|
||||
|
||||
// setup quadplane
|
||||
void setup(void);
|
||||
|
||||
// stabilize in hover mode
|
||||
void stabilize_hover(void);
|
||||
|
||||
// update transition handling
|
||||
void update(void);
|
||||
|
||||
// set motor arming
|
||||
void set_armed(bool armed);
|
||||
|
||||
private:
|
||||
AP_AHRS_NavEKF &ahrs;
|
||||
AP_Vehicle::MultiCopter aparm;
|
||||
AP_MotorsQuad motors{50};
|
||||
AC_PID pid_rate_roll {0.15, 0.1, 0.004, 2000, 20, 0.02};
|
||||
AC_PID pid_rate_pitch{0.15, 0.1, 0.004, 2000, 20, 0.02};
|
||||
AC_PID pid_rate_yaw {0.15, 0.1, 0.004, 2000, 20, 0.02};
|
||||
AC_P p_stabilize_roll{4.5};
|
||||
AC_P p_stabilize_pitch{4.5};
|
||||
AC_P p_stabilize_yaw{4.5};
|
||||
|
||||
AC_AttitudeControl_Multi attitude_control{ahrs, aparm, motors,
|
||||
p_stabilize_roll, p_stabilize_pitch, p_stabilize_yaw,
|
||||
pid_rate_roll, pid_rate_pitch, pid_rate_yaw};
|
||||
|
||||
AP_InertialNav_NavEKF inertial_nav{ahrs};
|
||||
|
||||
AC_P p_pos_xy{1};
|
||||
AC_P p_alt_hold{1};
|
||||
AC_P p_vel_z{5};
|
||||
AC_PID pid_accel_z{0.5, 1, 0, 800, 20, 0.02};
|
||||
AC_PI_2D pi_vel_xy{1.0, 0.5, 1000, 5, 0.02};
|
||||
|
||||
AC_PosControl pos_control{ahrs, inertial_nav, motors, attitude_control,
|
||||
p_alt_hold, p_vel_z, pid_accel_z,
|
||||
p_pos_xy, pi_vel_xy};
|
||||
|
||||
// update transition handling
|
||||
void update_transition(void);
|
||||
|
||||
AP_Int16 transition_time_ms;
|
||||
|
||||
// last time quadplane was active, used for transition
|
||||
uint32_t last_run_ms;
|
||||
|
||||
// last throttle value when active
|
||||
int16_t last_throttle;
|
||||
|
||||
const float smoothing_gain = 6;
|
||||
};
|
||||
|
|
@ -233,6 +233,8 @@ void Plane::init_ardupilot()
|
|||
|
||||
startup_ground();
|
||||
|
||||
quadplane.setup();
|
||||
|
||||
// choose the nav controller
|
||||
set_nav_controller();
|
||||
|
||||
|
|
@ -396,7 +398,7 @@ void Plane::set_mode(enum FlightMode mode)
|
|||
acro_state.locked_roll = false;
|
||||
acro_state.locked_pitch = false;
|
||||
break;
|
||||
|
||||
|
||||
case CRUISE:
|
||||
auto_throttle_mode = true;
|
||||
cruise_state.locked_heading = false;
|
||||
|
|
@ -443,6 +445,10 @@ void Plane::set_mode(enum FlightMode mode)
|
|||
guided_WP_loc = current_loc;
|
||||
set_guided_WP();
|
||||
break;
|
||||
|
||||
case HOVER:
|
||||
auto_throttle_mode = false;
|
||||
break;
|
||||
}
|
||||
|
||||
// start with throttle suppressed in auto_throttle modes
|
||||
|
|
@ -477,6 +483,7 @@ bool Plane::mavlink_set_mode(uint8_t mode)
|
|||
case AUTO:
|
||||
case RTL:
|
||||
case LOITER:
|
||||
case HOVER:
|
||||
set_mode((enum FlightMode)mode);
|
||||
return true;
|
||||
}
|
||||
|
|
@ -743,6 +750,7 @@ void Plane::change_arm_state(void)
|
|||
Log_Arm_Disarm();
|
||||
hal.util->set_soft_armed(arming.is_armed() &&
|
||||
hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED);
|
||||
quadplane.set_armed(hal.util->get_soft_armed());
|
||||
}
|
||||
|
||||
/*
|
||||
|
|
|
|||
Loading…
Reference in New Issue