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

Sub: Update to match upstream, part 1

Browse Source
This commit is contained in:
Rustom Jehangir 2016-04-04 20:17:39 -07:00 committed by Andrew Tridgell
parent e0d3eba5a4
commit 684bc249b6
27 changed files with 440 additions and 520 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
ArduSub/ArduSub.cpp
View File

@ -388,9 +388,9 @@ void Sub::ten_hz_logging_loop()
Log_Write_Attitude();
DataFlash.Log_Write_Rate(ahrs, motors, attitude_control, pos_control);
if (should_log(MASK_LOG_PID)) {
DataFlash.Log_Write_PID(LOG_PIDR_MSG, g.pid_rate_roll.get_pid_info() );
DataFlash.Log_Write_PID(LOG_PIDP_MSG, g.pid_rate_pitch.get_pid_info() );
DataFlash.Log_Write_PID(LOG_PIDY_MSG, g.pid_rate_yaw.get_pid_info() );
DataFlash.Log_Write_PID(LOG_PIDR_MSG, attitude_control.get_rate_roll_pid().get_pid_info() );
DataFlash.Log_Write_PID(LOG_PIDP_MSG, attitude_control.get_rate_pitch_pid().get_pid_info() );
DataFlash.Log_Write_PID(LOG_PIDY_MSG, gattitude_control.get_rate_yaw_pid().get_pid_info() );
DataFlash.Log_Write_PID(LOG_PIDA_MSG, g.pid_accel_z.get_pid_info() );
}
}
@ -431,9 +431,9 @@ void Sub::fifty_hz_logging_loop()
Log_Write_Attitude();
DataFlash.Log_Write_Rate(ahrs, motors, attitude_control, pos_control);
if (should_log(MASK_LOG_PID)) {
DataFlash.Log_Write_PID(LOG_PIDR_MSG, g.pid_rate_roll.get_pid_info() );
DataFlash.Log_Write_PID(LOG_PIDP_MSG, g.pid_rate_pitch.get_pid_info() );
DataFlash.Log_Write_PID(LOG_PIDY_MSG, g.pid_rate_yaw.get_pid_info() );
DataFlash.Log_Write_PID(LOG_PIDR_MSG, attitude_control.get_rate_roll_pid().get_pid_info() );
DataFlash.Log_Write_PID(LOG_PIDP_MSG, attitude_control.get_rate_pitch_pid().get_pid_info() );
DataFlash.Log_Write_PID(LOG_PIDY_MSG, attitude_control.get_rate_yaw_pid().get_pid_info() );
DataFlash.Log_Write_PID(LOG_PIDA_MSG, g.pid_accel_z.get_pid_info() );
}
}

38
ArduSub/Attitude.cpp
View File

@ -100,7 +100,7 @@ void Sub::update_thr_average()
{
// ensure throttle_average has been initialised
if( is_zero(throttle_average) ) {
throttle_average = g.throttle_mid;
throttle_average = 0.5f;
// update position controller
pos_control.set_throttle_hover(throttle_average);
}
@ -114,7 +114,7 @@ void Sub::update_thr_average()
float throttle = motors.get_throttle();
// calc average throttle if we are in a level hover
if (throttle > g.throttle_min && abs(climb_rate) < 60 && labs(ahrs.roll_sensor) < 500 && labs(ahrs.pitch_sensor) < 500) {
if (throttle > 0.0f && abs(climb_rate) < 60 && labs(ahrs.roll_sensor) < 500 && labs(ahrs.pitch_sensor) < 500) {
throttle_average = throttle_average * 0.99f + throttle * 0.01f;
// update position controller
pos_control.set_throttle_hover(throttle_average);
@ -134,33 +134,34 @@ void Sub::set_throttle_takeoff()
// get_pilot_desired_throttle - transform pilot's throttle input to make cruise throttle mid stick
// used only for manual throttle modes
// returns throttle output 0 to 1000
int16_t Sub::get_pilot_desired_throttle(int16_t throttle_control)
float Sub::get_pilot_desired_throttle(int16_t throttle_control)
{
int16_t throttle_out;
float throttle_out;
int16_t mid_stick = channel_throttle->get_control_mid();
// ensure reasonable throttle values
throttle_control = constrain_int16(throttle_control,0,1000);
// ensure mid throttle is set within a reasonable range
g.throttle_mid = constrain_int16(g.throttle_mid,g.throttle_min+50,700);
float thr_mid = MAX(0,g.throttle_mid-g.throttle_min) / (float)(1000-g.throttle_min);
// check throttle is above, below or in the deadband
if (throttle_control < mid_stick) {
// below the deadband
throttle_out = g.throttle_min + ((float)(throttle_control-g.throttle_min))*((float)(g.throttle_mid - g.throttle_min))/((float)(mid_stick-g.throttle_min));
throttle_out = ((float)throttle_control)*thr_mid/(float)mid_stick;
}else if(throttle_control > mid_stick) {
// above the deadband
throttle_out = g.throttle_mid + ((float)(throttle_control-mid_stick)) * (float)(1000-g.throttle_mid) / (float)(1000-mid_stick);
throttle_out = (thr_mid) + ((float)(throttle_control-mid_stick)) * (1.0f - thr_mid) / (float)(1000-mid_stick);
}else{
// must be in the deadband
throttle_out = g.throttle_mid;
throttle_out = thr_mid;
}
return throttle_out;
}
// get_pilot_desired_climb_rate - transform pilot's throttle input to
// climb rate in cm/s. we use radio_in instead of control_in to get the full range
// get_pilot_desired_climb_rate - transform pilot's throttle input to climb rate in cm/s
// without any deadzone at the bottom
float Sub::get_pilot_desired_climb_rate(float throttle_control)
{
@ -175,7 +176,7 @@ float Sub::get_pilot_desired_climb_rate(float throttle_control)
float deadband_bottom = mid_stick - g.throttle_deadzone;
// ensure a reasonable throttle value
throttle_control = constrain_float(throttle_control,g.throttle_min,1000.0f);
throttle_control = constrain_float(throttle_control,0.0f,1000.0f);
// ensure a reasonable deadzone
g.throttle_deadzone = constrain_int16(g.throttle_deadzone, 0, 400);
@ -183,7 +184,7 @@ float Sub::get_pilot_desired_climb_rate(float throttle_control)
// check throttle is above, below or in the deadband
if (throttle_control < deadband_bottom) {
// below the deadband
desired_rate = g.pilot_velocity_z_max * (throttle_control-deadband_bottom) / (deadband_bottom-g.throttle_min);
desired_rate = g.pilot_velocity_z_max * (throttle_control-deadband_bottom) / deadband_bottom;
}else if (throttle_control > deadband_top) {
// above the deadband
desired_rate = g.pilot_velocity_z_max * (throttle_control-deadband_top) / (1000.0f-deadband_top);
@ -201,7 +202,7 @@ float Sub::get_pilot_desired_climb_rate(float throttle_control)
// get_non_takeoff_throttle - a throttle somewhere between min and mid throttle which should not lead to a takeoff
float Sub::get_non_takeoff_throttle()
{
return (g.throttle_mid / 2.0f);
return (((float)g.throttle_mid/1000.0f)/2.0f);
}
float Sub::get_takeoff_trigger_throttle()
@ -209,9 +210,8 @@ float Sub::get_takeoff_trigger_throttle()
return channel_throttle->get_control_mid() + g.takeoff_trigger_dz;
}
// get_throttle_pre_takeoff - convert pilot's input throttle to a throttle output before take-off
// get_throttle_pre_takeoff - convert pilot's input throttle to a throttle output (in the range 0 to 1) before take-off
// used only for althold, loiter, hybrid flight modes
// returns throttle output 0 to 1000
float Sub::get_throttle_pre_takeoff(float input_thr)
{
// exit immediately if input_thr is zero
@ -219,14 +219,14 @@ float Sub::get_throttle_pre_takeoff(float input_thr)
return 0.0f;
}
// TODO: does this parameter sanity check really belong here?
g.throttle_mid = constrain_int16(g.throttle_mid,g.throttle_min+50,700);
// ensure reasonable throttle values
input_thr = constrain_float(input_thr,0.0f,1000.0f);
float in_min = g.throttle_min;
float in_min = 0.0f;
float in_max = get_takeoff_trigger_throttle();
// multicopters will output between spin-when-armed and 1/2 of mid throttle
float out_min = motors.get_throttle_warn();
float out_min = 0.0f;
float out_max = get_non_takeoff_throttle();
if ((g.throttle_behavior & THR_BEHAVE_FEEDBACK_FROM_MID_STICK) != 0) {
@ -283,7 +283,7 @@ float Sub::get_surface_tracking_climb_rate(int16_t target_rate, float current_al
void Sub::set_accel_throttle_I_from_pilot_throttle(int16_t pilot_throttle)
{
// shift difference between pilot's throttle and hover throttle into accelerometer I
g.pid_accel_z.set_integrator(pilot_throttle-throttle_average);
g.pid_accel_z.set_integrator((pilot_throttle-throttle_average) * 1000.0f);
}
// updates position controller's maximum altitude using fence and EKF limits

8
ArduSub/GCS_Mavlink.cpp
View File

@ -389,7 +389,7 @@ void NOINLINE Sub::send_vfr_hud(mavlink_channel_t chan)
gps.ground_speed(),
gps.ground_speed(),
(ahrs.yaw_sensor / 100) % 360,
(int16_t)(motors.get_throttle())/10,
(int16_t)(motors.get_throttle() * 100),
current_loc.alt / 100.0f,
climb_rate / 100.0f);
}
@ -434,7 +434,7 @@ void Sub::send_pid_tuning(mavlink_channel_t chan)
{
const Vector3f &gyro = ahrs.get_gyro();
if (g.gcs_pid_mask & 1) {
const DataFlash_Class::PID_Info &pid_info = g.pid_rate_roll.get_pid_info();
const DataFlash_Class::PID_Info &pid_info = attitude_control.get_rate_roll_pid().get_pid_info();
mavlink_msg_pid_tuning_send(chan, PID_TUNING_ROLL,
pid_info.desired*0.01f,
degrees(gyro.x),
@ -447,7 +447,7 @@ void Sub::send_pid_tuning(mavlink_channel_t chan)
}
}
if (g.gcs_pid_mask & 2) {
const DataFlash_Class::PID_Info &pid_info = g.pid_rate_pitch.get_pid_info();
const DataFlash_Class::PID_Info &pid_info = attitude_control.get_rate_pitch_pid().get_pid_info();
mavlink_msg_pid_tuning_send(chan, PID_TUNING_PITCH,
pid_info.desired*0.01f,
degrees(gyro.y),
@ -460,7 +460,7 @@ void Sub::send_pid_tuning(mavlink_channel_t chan)
}
}
if (g.gcs_pid_mask & 4) {
const DataFlash_Class::PID_Info &pid_info = g.pid_rate_yaw.get_pid_info();
const DataFlash_Class::PID_Info &pid_info = attitude_control.get_rate_yaw_pid().get_pid_info();
mavlink_msg_pid_tuning_send(chan, PID_TUNING_YAW,
pid_info.desired*0.01f,
degrees(gyro.z),

14
ArduSub/Log.cpp
View File

@ -298,8 +298,8 @@ void Sub::Log_Write_Nav_Tuning()
struct PACKED log_Control_Tuning {
LOG_PACKET_HEADER;
uint64_t time_us;
int16_t throttle_in;
int16_t angle_boost;
float throttle_in;
float angle_boost;
float throttle_out;
float desired_alt;
float inav_alt;
@ -316,7 +316,7 @@ void Sub::Log_Write_Control_Tuning()
struct log_Control_Tuning pkt = {
LOG_PACKET_HEADER_INIT(LOG_CONTROL_TUNING_MSG),
time_us : AP_HAL::micros64(),
throttle_in : channel_throttle->control_in,
throttle_in : attitude_control.get_throttle_in(),
angle_boost : attitude_control.angle_boost(),
throttle_out : motors.get_throttle(),
desired_alt : pos_control.get_alt_target() / 100.0f,
@ -624,8 +624,8 @@ void Sub::Log_Sensor_Health()
struct PACKED log_Heli {
LOG_PACKET_HEADER;
uint64_t time_us;
int16_t desired_rotor_speed;
int16_t main_rotor_speed;
float desired_rotor_speed;
float main_rotor_speed;
};
// precision landing logging
@ -683,7 +683,7 @@ const struct LogStructure Sub::log_structure[] = {
{ LOG_NAV_TUNING_MSG, sizeof(log_Nav_Tuning),
"NTUN", "Qffffffffff", "TimeUS,DPosX,DPosY,PosX,PosY,DVelX,DVelY,VelX,VelY,DAccX,DAccY" },
{ LOG_CONTROL_TUNING_MSG, sizeof(log_Control_Tuning),
"CTUN", "Qhhfffecchh", "TimeUS,ThrIn,AngBst,ThrOut,DAlt,Alt,BarAlt,DSAlt,SAlt,DCRt,CRt" },
"CTUN", "Qfffffecchh", "TimeUS,ThrIn,AngBst,ThrOut,DAlt,Alt,BarAlt,DSAlt,SAlt,DCRt,CRt" },
{ LOG_PERFORMANCE_MSG, sizeof(log_Performance),
"PM", "QHHIhBH", "TimeUS,NLon,NLoop,MaxT,PMT,I2CErr,INSErr" },
{ LOG_MOTBATT_MSG, sizeof(log_MotBatt),
@ -705,7 +705,7 @@ const struct LogStructure Sub::log_structure[] = {
{ LOG_ERROR_MSG, sizeof(log_Error),
"ERR", "QBB", "TimeUS,Subsys,ECode" },
{ LOG_HELI_MSG, sizeof(log_Heli),
"HELI", "Qhh", "TimeUS,DRRPM,ERRPM" },
"HELI", "Qff", "TimeUS,DRRPM,ERRPM" },
{ LOG_PRECLAND_MSG, sizeof(log_Precland),
"PL", "QBffffff", "TimeUS,Heal,bX,bY,eX,eY,pX,pY" },
};

205
ArduSub/Parameters.cpp
View File

@ -702,99 +702,6 @@ const AP_Param::Info Sub::var_info[] = {
// @User: Advanced
GSCALAR(acro_expo, "ACRO_EXPO", ACRO_EXPO_DEFAULT),
// PID controller
//---------------
// @Param: RATE_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: RATE_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: RATE_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: RATE_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
GGROUP(pid_rate_roll, "RATE_RLL_", AC_PID),
// @Param: RATE_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: RATE_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: RATE_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: RATE_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
GGROUP(pid_rate_pitch, "RATE_PIT_", AC_PID),
// @Param: RATE_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: RATE_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: RATE_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: RATE_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
GGROUP(pid_rate_yaw, "RATE_YAW_", AC_PID),
// @Param: VEL_XY_P
// @DisplayName: Velocity (horizontal) P gain
// @Description: Velocity (horizontal) P gain. Converts the difference between desired velocity to a target acceleration
@ -859,29 +766,6 @@ const AP_Param::Info Sub::var_info[] = {
// @User: Standard
GGROUP(pid_accel_z, "ACCEL_Z_", 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
GGROUP(p_stabilize_roll, "STB_RLL_", 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
GGROUP(p_stabilize_pitch, "STB_PIT_", 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
GGROUP(p_stabilize_yaw, "STB_YAW_", AC_P),
// @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
@ -963,8 +847,8 @@ const AP_Param::Info Sub::var_info[] = {
GOBJECT(circle_nav, "CIRCLE_", AC_Circle),
// @Group: ATC_
// @Path: ../libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
GOBJECT(attitude_control, "ATC_", AC_AttitudeControl),
// @Path: ../libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp
GOBJECT(attitude_control, "ATC_", AC_AttitudeControl_Multi),
// @Group: POSCON_
// @Path: ../libraries/AC_AttitudeControl/AC_PosControl.cpp
@ -1049,29 +933,11 @@ const AP_Param::Info Sub::var_info[] = {
GOBJECT(motors, "H_", AP_MotorsHeli_Single),
#elif FRAME_CONFIG == SINGLE_FRAME
// @Group: SS1_
// @Path: ../libraries/RC_Channel/RC_Channel.cpp
GGROUP(single_servo_1, "SS1_", RC_Channel),
// @Group: SS2_
// @Path: ../libraries/RC_Channel/RC_Channel.cpp
GGROUP(single_servo_2, "SS2_", RC_Channel),
// @Group: SS3_
// @Path: ../libraries/RC_Channel/RC_Channel.cpp
GGROUP(single_servo_3, "SS3_", RC_Channel),
// @Group: SS4_
// @Path: ../libraries/RC_Channel/RC_Channel.cpp
GGROUP(single_servo_4, "SS4_", RC_Channel),
// @Group: MOT_
// @Path: ../libraries/AP_Motors/AP_MotorsSingle.cpp
GOBJECT(motors, "MOT_", AP_MotorsSingle),
#elif FRAME_CONFIG == COAX_FRAME
// @Group: SS1_
// @Path: ../libraries/RC_Channel/RC_Channel.cpp
GGROUP(single_servo_1, "SS1_", RC_Channel),
// @Group: SS2_
// @Path: ../libraries/RC_Channel/RC_Channel.cpp
GGROUP(single_servo_2, "SS2_", RC_Channel),
// @Group: MOT_
// @Path: ../libraries/AP_Motors/AP_MotorsCoax.cpp
GOBJECT(motors, "MOT_", AP_MotorsCoax),
@ -1227,11 +1093,66 @@ void Sub::load_parameters(void)
// save the current format version
g.format_version.set_and_save(Parameters::k_format_version);
cliSerial->println("done.");
} else {
uint32_t before = micros();
// Load all auto-loaded EEPROM variables
AP_Param::load_all();
AP_Param::convert_old_parameters(&conversion_table[0], ARRAY_SIZE(conversion_table));
cliSerial->printf("load_all took %uus\n", (unsigned)(micros() - before));
}
uint32_t before = micros();
// Load all auto-loaded EEPROM variables
AP_Param::load_all();
AP_Param::convert_old_parameters(&conversion_table[0], ARRAY_SIZE(conversion_table));
cliSerial->printf("load_all took %uus\n", (unsigned)(micros() - before));
// upgrade parameters
convert_pid_parameters();
}
// handle conversion of PID gains from Copter-3.3 to Copter-3.4
void Sub::convert_pid_parameters(void)
{
// conversion info
AP_Param::ConversionInfo pid_conversion_info[] = {
{ Parameters::k_param_pid_rate_roll, 0, AP_PARAM_FLOAT, "ATC_RAT_RLL_P" },
{ Parameters::k_param_pid_rate_roll, 1, AP_PARAM_FLOAT, "ATC_RAT_RLL_I" },
{ Parameters::k_param_pid_rate_roll, 2, AP_PARAM_FLOAT, "ATC_RAT_RLL_D" },
{ Parameters::k_param_pid_rate_pitch, 0, AP_PARAM_FLOAT, "ATC_RAT_PIT_P" },
{ Parameters::k_param_pid_rate_pitch, 1, AP_PARAM_FLOAT, "ATC_RAT_PIT_I" },
{ Parameters::k_param_pid_rate_pitch, 2, AP_PARAM_FLOAT, "ATC_RAT_PIT_D" },
{ Parameters::k_param_pid_rate_yaw, 0, AP_PARAM_FLOAT, "ATC_RAT_YAW_P" },
{ Parameters::k_param_pid_rate_yaw, 1, AP_PARAM_FLOAT, "ATC_RAT_YAW_I" },
{ Parameters::k_param_pid_rate_yaw, 2, AP_PARAM_FLOAT, "ATC_RAT_YAW_D" },
};
AP_Param::ConversionInfo imax_conversion_info[] = {
{ Parameters::k_param_pid_rate_roll, 5, AP_PARAM_FLOAT, "ATC_RAT_RLL_IMAX" },
{ Parameters::k_param_pid_rate_pitch, 5, AP_PARAM_FLOAT, "ATC_RAT_PIT_IMAX" },
{ Parameters::k_param_pid_rate_yaw, 5, AP_PARAM_FLOAT, "ATC_RAT_YAW_IMAX" }
};
AP_Param::ConversionInfo filt_conversion_info[] = {
{ Parameters::k_param_pid_rate_roll, 6, AP_PARAM_FLOAT, "ATC_RAT_RLL_FILT" },
{ Parameters::k_param_pid_rate_pitch, 6, AP_PARAM_FLOAT, "ATC_RAT_PIT_FILT" },
{ Parameters::k_param_pid_rate_yaw, 6, AP_PARAM_FLOAT, "ATC_RAT_YAW_FILT" }
};
// gains increase by 27% due to attitude controller's switch to use radians instead of centi-degrees
// and motor libraries switch to accept inputs in -1 to +1 range instead of -4500 ~ +4500
float pid_scaler = 1.27f;
// Multicopter x-frame gains are 40% lower because -1 or +1 input to motors now results in maximum rotation
if (g.frame_orientation == AP_MOTORS_X_FRAME || g.frame_orientation == AP_MOTORS_V_FRAME || g.frame_orientation == AP_MOTORS_H_FRAME) {
pid_scaler = 0.9f;
}
// scale PID gains
uint8_t table_size = ARRAY_SIZE(pid_conversion_info);
for (uint8_t i=0; i<table_size; i++) {
AP_Param::convert_old_parameter(&pid_conversion_info[i], pid_scaler);
}
// reduce IMAX into -1 ~ +1 range
table_size = ARRAY_SIZE(imax_conversion_info);
for (uint8_t i=0; i<table_size; i++) {
AP_Param::convert_old_parameter(&imax_conversion_info[i], 1.0f/4500.0f);
}
// convert filter without scaling
table_size = ARRAY_SIZE(filt_conversion_info);
for (uint8_t i=0; i<table_size; i++) {
AP_Param::convert_old_parameter(&filt_conversion_info[i], 1.0f);
}
}

66
ArduSub/Parameters.h
View File

@ -154,24 +154,24 @@ public:
//
// 75: Singlecopter, CoaxCopter
//
k_param_single_servo_1 = 75,
k_param_single_servo_2,
k_param_single_servo_3,
k_param_single_servo_4, // 78
k_param_single_servo_1 = 75, // remove
k_param_single_servo_2, // remove
k_param_single_servo_3, // remove
k_param_single_servo_4, // 78 - remove
//
// 80: Heli
//
k_param_heli_servo_1 = 80,
k_param_heli_servo_2,
k_param_heli_servo_3,
k_param_heli_servo_4,
k_param_heli_servo_1 = 80, // remove
k_param_heli_servo_2, // remove
k_param_heli_servo_3, // remove
k_param_heli_servo_4, // remove
k_param_heli_pitch_ff, // remove
k_param_heli_roll_ff, // remove
k_param_heli_yaw_ff, // remove
k_param_heli_stab_col_min, // remove
k_param_heli_stab_col_max, // remove
k_param_heli_servo_rsc, // 89 = full!
k_param_heli_servo_rsc, // 89 = full! - remove
//
// 90: misc2
@ -328,12 +328,12 @@ public:
//
k_param_acro_rp_p = 221,
k_param_axis_lock_p, // remove
k_param_pid_rate_roll,
k_param_pid_rate_pitch,
k_param_pid_rate_yaw,
k_param_p_stabilize_roll,
k_param_p_stabilize_pitch,
k_param_p_stabilize_yaw,
k_param_pid_rate_roll, // remove
k_param_pid_rate_pitch, // remove
k_param_pid_rate_yaw, // remove
k_param_p_stabilize_roll, // remove
k_param_p_stabilize_pitch, // remove
k_param_p_stabilize_yaw, // remove
k_param_p_pos_xy,
k_param_p_loiter_lon, // remove
k_param_pid_loiter_rate_lat, // remove
@ -474,16 +474,6 @@ public:
AP_Int8 throw_motor_start;
#if FRAME_CONFIG == SINGLE_FRAME
// Single
RC_Channel single_servo_1, single_servo_2, single_servo_3, single_servo_4; // servos for four flaps
#endif
#if FRAME_CONFIG == COAX_FRAME
// Coax copter flaps
RC_Channel single_servo_1, single_servo_2; // servos for two flaps
#endif
// RC channels
RC_Channel rc_1;
RC_Channel rc_2;
@ -533,9 +523,6 @@ public:
AP_Float acro_expo;
// PI/D controllers
AC_PID pid_rate_roll;
AC_PID pid_rate_pitch;
AC_PID pid_rate_yaw;
AC_PI_2D pi_vel_xy;
AC_P p_vel_z;
@ -546,9 +533,6 @@ public:
#endif
AC_P p_pos_xy;
AC_P p_stabilize_roll;
AC_P p_stabilize_pitch;
AC_P p_stabilize_yaw;
AC_P p_alt_hold;
// Autotune
@ -562,18 +546,6 @@ public:
// above.
Parameters() :
#if FRAME_CONFIG == SINGLE_FRAME
single_servo_1 (CH_1),
single_servo_2 (CH_2),
single_servo_3 (CH_3),
single_servo_4 (CH_4),
#endif
#if FRAME_CONFIG == COAX_FRAME
single_servo_1 (CH_1),
single_servo_2 (CH_2),
#endif
rc_1 (CH_1),
rc_2 (CH_2),
rc_3 (CH_3),
@ -595,10 +567,6 @@ public:
// PID controller initial P initial I initial D initial imax initial filt hz pid rate
//---------------------------------------------------------------------------------------------------------------------------------
pid_rate_roll (RATE_ROLL_P, RATE_ROLL_I, RATE_ROLL_D, RATE_ROLL_IMAX, RATE_ROLL_FILT_HZ, MAIN_LOOP_SECONDS),
pid_rate_pitch (RATE_PITCH_P, RATE_PITCH_I, RATE_PITCH_D, RATE_PITCH_IMAX, RATE_PITCH_FILT_HZ, MAIN_LOOP_SECONDS),
pid_rate_yaw (RATE_YAW_P, RATE_YAW_I, RATE_YAW_D, RATE_YAW_IMAX, RATE_YAW_FILT_HZ, MAIN_LOOP_SECONDS),
pi_vel_xy (VEL_XY_P, VEL_XY_I, VEL_XY_IMAX, VEL_XY_FILT_HZ, WPNAV_LOITER_UPDATE_TIME),
p_vel_z (VEL_Z_P),
@ -612,10 +580,6 @@ public:
//----------------------------------------------------------------------
p_pos_xy (POS_XY_P),
p_stabilize_roll (STABILIZE_ROLL_P),
p_stabilize_pitch (STABILIZE_PITCH_P),
p_stabilize_yaw (STABILIZE_YAW_P),
p_alt_hold (ALT_HOLD_P)
{
}

12
ArduSub/Sub.cpp
View File

@ -29,13 +29,6 @@ Sub::Sub(void) :
FUNCTOR_BIND_MEMBER(&Sub::verify_command_callback, bool, const AP_Mission::Mission_Command &),
FUNCTOR_BIND_MEMBER(&Sub::exit_mission, void)),
control_mode(STABILIZE),
#if FRAME_CONFIG == TRI_FRAME // tri constructor requires additional rc_7 argument to allow tail servo reversing
motors(MAIN_LOOP_RATE),
#elif FRAME_CONFIG == SINGLE_FRAME // single constructor requires extra servos for flaps
motors(g.single_servo_1, g.single_servo_2, g.single_servo_3, g.single_servo_4, MAIN_LOOP_RATE),
#elif FRAME_CONFIG == COAX_FRAME // single constructor requires extra servos for flaps
motors(g.single_servo_1, g.single_servo_2, MAIN_LOOP_RATE),
#else
motors(MAIN_LOOP_RATE),
#endif
scaleLongDown(1),
@ -76,10 +69,9 @@ Sub::Sub(void) :
yaw_look_ahead_bearing(0.0f),
condition_value(0),
condition_start(0),
G_Dt(0.0025f),
G_Dt(MAIN_LOOP_SECONDS),
inertial_nav(ahrs),
attitude_control(ahrs, aparm, motors, g.p_stabilize_roll, g.p_stabilize_pitch, g.p_stabilize_yaw,
g.pid_rate_roll, g.pid_rate_pitch, g.pid_rate_yaw),
attitude_control(ahrs, aparm, motors, MAIN_LOOP_SECONDS),
pos_control(ahrs, inertial_nav, motors, attitude_control,
g.p_alt_hold, g.p_vel_z, g.pid_accel_z,
g.p_pos_xy, g.pi_vel_xy),

8
ArduSub/Sub.h
View File

@ -582,13 +582,13 @@ private:
float get_look_ahead_yaw();
void update_thr_average();
void set_throttle_takeoff();
int16_t get_pilot_desired_throttle(int16_t throttle_control);
float get_pilot_desired_throttle(int16_t throttle_control);
float get_pilot_desired_climb_rate(float throttle_control);
float get_non_takeoff_throttle();
float get_takeoff_trigger_throttle();
float get_throttle_pre_takeoff(float input_thr);
float get_surface_tracking_climb_rate(int16_t target_rate, float current_alt_target, float dt);
void set_accel_throttle_I_from_pilot_throttle(int16_t pilot_throttle);
void set_accel_throttle_I_from_pilot_throttle(float pilot_throttle);
void update_poscon_alt_max();
void rotate_body_frame_to_NE(float &x, float &y);
void gcs_send_heartbeat(void);
@ -642,6 +642,7 @@ private:
void Log_Read(uint16_t log_num, uint16_t start_page, uint16_t end_page);
void start_logging() ;
void load_parameters(void);
void convert_pid_parameters(void);
void userhook_init();
void userhook_FastLoop();
void userhook_50Hz();
@ -728,7 +729,7 @@ private:
void circle_run();
bool drift_init(bool ignore_checks);
void drift_run();
int16_t get_throttle_assist(float velz, int16_t pilot_throttle_scaled);
float get_throttle_assist(float velz, float pilot_throttle_scaled);
bool flip_init(bool ignore_checks);
void flip_run();
bool guided_init(bool ignore_checks);
@ -888,6 +889,7 @@ private:
JSButton* get_button(uint8_t index);
void set_throttle_and_failsafe(uint16_t throttle_pwm);
void set_throttle_zero_flag(int16_t throttle_control);
void radio_passthrough_to_motors();
void init_barometer(bool full_calibration);
void read_barometer(void);
void init_sonar(void);

2
ArduSub/arming_checks.cpp
View File

@ -302,7 +302,7 @@ bool Sub::pre_arm_checks(bool display_failure)
}
// acro balance parameter check
if ((g.acro_balance_roll > g.p_stabilize_roll.kP()) || (g.acro_balance_pitch > g.p_stabilize_pitch.kP())) {
if ((g.acro_balance_roll > attitude_control.get_angle_roll_p().kP()) || (g.acro_balance_pitch > attitude_control.get_angle_pitch_p().kP())) {
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: ACRO_BAL_ROLL/PITCH");
}

78
ArduSub/config.h
View File

@ -80,18 +80,6 @@
# define SURFACE_DEPTH_DEFAULT -10.0 // pressure sensor reading 10cm depth means craft is considered surfaced
#endif
/////////////////////////////////////////////////////////////////////////////////
// Y6 defaults
//#if FRAME_CONFIG == Y6_FRAME
// # define RATE_ROLL_P 0.1f
// # define RATE_ROLL_D 0.006f
// # define RATE_PITCH_P 0.1f
// # define RATE_PITCH_D 0.006f
// # define RATE_YAW_P 0.150f
// # define RATE_YAW_I 0.015f
//#endif
//////////////////////////////////////////////////////////////////////////////
// PWM control
// default RC speed in Hz
@ -412,19 +400,6 @@
#define ACRO_EXPO_DEFAULT 0.3f
#endif
// Stabilize (angle controller) gains
#ifndef STABILIZE_ROLL_P
# define STABILIZE_ROLL_P 4.5f
#endif
#ifndef STABILIZE_PITCH_P
# define STABILIZE_PITCH_P 4.5f
#endif
#ifndef STABILIZE_YAW_P
# define STABILIZE_YAW_P 4.5f
#endif
// RTL Mode
#ifndef RTL_ALT_FINAL
# define RTL_ALT_FINAL 0 // the altitude the vehicle will move to as the final stage of Returning to Launch. Set to zero to land.
@ -477,59 +452,6 @@
# define ANGLE_RATE_MAX 18000 // default maximum rotation rate in roll/pitch axis requested by angle controller used in stabilize, loiter, rtl, auto flight modes
#endif
//////////////////////////////////////////////////////////////////////////////
// Rate controller gains
//
#ifndef RATE_ROLL_P
# define RATE_ROLL_P 0.150f
#endif
#ifndef RATE_ROLL_I
# define RATE_ROLL_I 0.100f
#endif
#ifndef RATE_ROLL_D
# define RATE_ROLL_D 0.004f
#endif
#ifndef RATE_ROLL_IMAX
# define RATE_ROLL_IMAX 2000
#endif
#ifndef RATE_ROLL_FILT_HZ
# define RATE_ROLL_FILT_HZ 20.0f
#endif
#ifndef RATE_PITCH_P
# define RATE_PITCH_P 0.150f
#endif
#ifndef RATE_PITCH_I
# define RATE_PITCH_I 0.100f
#endif
#ifndef RATE_PITCH_D
# define RATE_PITCH_D 0.004f
#endif
#ifndef RATE_PITCH_IMAX
# define RATE_PITCH_IMAX 2000
#endif
#ifndef RATE_PITCH_FILT_HZ
# define RATE_PITCH_FILT_HZ 20.0f
#endif
#ifndef RATE_YAW_P
# define RATE_YAW_P 0.200f
#endif
#ifndef RATE_YAW_I
# define RATE_YAW_I 0.020f
#endif
#ifndef RATE_YAW_D
# define RATE_YAW_D 0.000f
#endif
#ifndef RATE_YAW_IMAX
# define RATE_YAW_IMAX 1000
#endif
#ifndef RATE_YAW_FILT_HZ
# define RATE_YAW_FILT_HZ 5.0f
#endif
//////////////////////////////////////////////////////////////////////////////
// Loiter position control gains
//

27
ArduSub/control_acro.cpp
View File

@ -9,14 +9,14 @@
// acro_init - initialise acro controller
bool Sub::acro_init(bool ignore_checks)
{
// if landed and the mode we're switching from does not have manual throttle and the throttle stick is too high
if (motors.armed() && ap.land_complete && !mode_has_manual_throttle(control_mode) && (g.rc_3.control_in > get_non_takeoff_throttle())) {
return false;
}
// set target altitude to zero for reporting
pos_control.set_alt_target(0);
// if landed and the mode we're switching from does not have manual throttle and the throttle stick is too high
if (motors.armed() && ap.land_complete && !mode_has_manual_throttle(control_mode) && (get_pilot_desired_throttle(channel_throttle->control_in) > get_non_takeoff_throttle())) {
return false;
}
// set target altitude to zero for reporting
pos_control.set_alt_target(0);
return true;
return true;
}
// acro_run - runs the acro controller
@ -24,18 +24,17 @@ bool Sub::acro_init(bool ignore_checks)
void Sub::acro_run()
{
float target_roll, target_pitch, target_yaw;
int16_t pilot_throttle_scaled;
float pilot_throttle_scaled;
// if motors not running reset angle targets
if(!motors.armed()) {
// if not armed set throttle to zero and exit immediately
if (!motors.armed() || !motors.get_interlock()) {
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
// slow start if landed
if (ap.land_complete) {
motors.slow_start(true);
}
return;
}
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// convert the input to the desired body frame rate
get_pilot_desired_angle_rates(channel_roll->control_in, channel_pitch->control_in, channel_yaw->control_in, target_roll, target_pitch, target_yaw);

22
ArduSub/control_althold.cpp
View File

@ -49,13 +49,15 @@ void Sub::althold_run()
float target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->control_in);
target_climb_rate = constrain_float(target_climb_rate, -g.pilot_velocity_z_max, g.pilot_velocity_z_max);
//bool takeoff_triggered = (ap.land_complete && (channel_throttle->control_in > get_takeoff_trigger_throttle()));
//bool takeoff_triggered = (ap.land_complete && (channel_throttle->control_in > get_takeoff_trigger_throttle()) && motors.spool_up_complete());
// // Alt Hold State Machine Determination
if(!ap.auto_armed) {
althold_state = AltHold_Disarmed;
// } else if (!motors.get_interlock()){
// althold_state = AltHold_MotorStop;
// if (!motors.armed() || !motors.get_interlock()) {
// althold_state = AltHold_MotorStop;
// } else if (!ap.auto_armed){
// althold_state = AltHold_Disarmed;
// } else if (takeoff_state.running || takeoff_triggered){
// althold_state = AltHold_Takeoff;
// } else if (ap.land_complete){
@ -68,6 +70,7 @@ void Sub::althold_run()
switch (althold_state) {
case AltHold_Disarmed:
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
// Multicopter do not stabilize roll/pitch/yaw when disarmed
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
@ -76,6 +79,7 @@ void Sub::althold_run()
case AltHold_MotorStop:
// Multicopter do not stabilize roll/pitch/yaw when motor are stopped
motors.set_desired_spool_state(AP_Motors::DESIRED_SHUT_DOWN);
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->control_in)-throttle_average);
break;
@ -94,6 +98,9 @@ void Sub::althold_run()
// get take-off adjusted pilot and takeoff climb rates
takeoff_get_climb_rates(target_climb_rate, takeoff_climb_rate);
// set motors to full range
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
@ -105,12 +112,19 @@ void Sub::althold_run()
case AltHold_Landed:
// Multicopter do not stabilize roll/pitch/yaw when disarmed
attitude_control.set_throttle_out_unstabilized(get_throttle_pre_takeoff(channel_throttle->control_in),true,g.throttle_filt);
attitude_control.set_throttle_out(get_throttle_pre_takeoff(channel_throttle->control_in),false,g.throttle_filt);
// if throttle zero reset attitude and exit immediately
if (ap.throttle_zero) {
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
} else {
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
}
pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->control_in)-throttle_average);
break;
case AltHold_Flying:
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());

32
ArduSub/control_auto.cpp
View File

@ -112,10 +112,11 @@ void Sub::auto_takeoff_start(float final_alt_above_home)
void Sub::auto_takeoff_run()
{
// if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
if(!ap.auto_armed || !motors.get_interlock()) {
if (!motors.armed() || !ap.auto_armed || !motors.get_interlock()) {
// initialise wpnav targets
wp_nav.shift_wp_origin_to_current_pos();
// multicopters do not stabilize roll/pitch/yaw when disarmed
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
// reset attitude control targets
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
// clear i term when we're taking off
@ -130,6 +131,9 @@ void Sub::auto_takeoff_run()
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in);
}
// set motors to full range
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// run waypoint controller
wp_nav.update_wpnav();
@ -160,11 +164,12 @@ void Sub::auto_wp_start(const Vector3f& destination)
void Sub::auto_wp_run()
{
// if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
if(!ap.auto_armed || !motors.get_interlock()) {
if (!motors.armed() || !ap.auto_armed || !motors.get_interlock()) {
// To-Do: reset waypoint origin to current location because copter is probably on the ground so we don't want it lurching left or right on take-off
// (of course it would be better if people just used take-off)
// call attitude controller
// multicopters do not stabilize roll/pitch/yaw when disarmed
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
// clear i term when we're taking off
@ -182,6 +187,9 @@ void Sub::auto_wp_run()
}
}
// set motors to full range
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// run waypoint controller
wp_nav.update_wpnav();
@ -221,11 +229,12 @@ void Sub::auto_spline_start(const Vector3f& destination, bool stopped_at_start,
void Sub::auto_spline_run()
{
// if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
if(!ap.auto_armed || !motors.get_interlock()) {
if (!motors.armed() || !ap.auto_armed || !motors.get_interlock()) {
// To-Do: reset waypoint origin to current location because copter is probably on the ground so we don't want it lurching left or right on take-off
// (of course it would be better if people just used take-off)
// multicopters do not stabilize roll/pitch/yaw when disarmed
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
// clear i term when we're taking off
set_throttle_takeoff();
@ -242,6 +251,9 @@ void Sub::auto_spline_run()
}
}
// set motors to full range
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// run waypoint controller
wp_nav.update_spline();
@ -292,7 +304,8 @@ void Sub::auto_land_run()
float target_yaw_rate = 0;
// if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
if(!ap.auto_armed || ap.land_complete) {
if (!motors.armed() || !ap.auto_armed || ap.land_complete) {
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
// multicopters do not stabilize roll/pitch/yaw when disarmed
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
@ -321,6 +334,9 @@ void Sub::auto_land_run()
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in);
}
// set motors to full range
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// process roll, pitch inputs
wp_nav.set_pilot_desired_acceleration(roll_control, pitch_control);
@ -460,8 +476,9 @@ bool Sub::auto_loiter_start()
void Sub::auto_loiter_run()
{
// if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
if(!ap.auto_armed || ap.land_complete || !motors.get_interlock()) {
// multicopters do not stabilize roll/pitch/yaw when disarmed
if (!motors.armed() || !ap.auto_armed || ap.land_complete || !motors.get_interlock()) {
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
// multicopters do not stabilize roll/pitch/yaw when disarmed
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
return;
@ -473,6 +490,9 @@ void Sub::auto_loiter_run()
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in);
}
// set motors to full range
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// run waypoint and z-axis postion controller
wp_nav.update_wpnav();
pos_control.update_z_controller();

231
ArduSub/control_autotune.cpp
View File

@ -268,7 +268,8 @@ void Copter::autotune_run()
// if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
// this should not actually be possible because of the autotune_init() checks
if (!ap.auto_armed || !motors.get_interlock()) {
if (!motors.armed() || !ap.auto_armed || !motors.get_interlock()) {
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->control_in)-throttle_average);
return;
@ -296,8 +297,13 @@ void Copter::autotune_run()
// reset target lean angles and heading while landed
if (ap.land_complete) {
if (ap.throttle_zero) {
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
} else {
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
}
// move throttle to between minimum and non-takeoff-throttle to keep us on the ground
attitude_control.set_throttle_out_unstabilized(get_throttle_pre_takeoff(channel_throttle->control_in),true,g.throttle_filt);
attitude_control.set_throttle_out(get_throttle_pre_takeoff(channel_throttle->control_in),false,g.throttle_filt);
pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->control_in)-throttle_average);
}else{
// check if pilot is overriding the controls
@ -321,6 +327,9 @@ void Copter::autotune_run()
}
}
// set motors to full range
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// if pilot override call attitude controller
if (autotune_state.pilot_override || autotune_state.mode != AUTOTUNE_MODE_TUNING) {
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
@ -384,7 +393,7 @@ void Copter::autotune_attitude_control()
autotune_target_angle = constrain_float(attitude_control.max_angle_step_bf_roll(), AUTOTUNE_TARGET_MIN_ANGLE_RLLPIT_CD, AUTOTUNE_TARGET_ANGLE_RLLPIT_CD);
autotune_start_rate = ToDeg(ahrs.get_gyro().x) * 100.0f;
autotune_start_angle = ahrs.roll_sensor;
rotation_rate_filt.set_cutoff_frequency(g.pid_rate_roll.filt_hz()*2.0f);
rotation_rate_filt.set_cutoff_frequency(attitude_control.get_rate_roll_pid().filt_hz()*2.0f);
if ((autotune_state.tune_type == AUTOTUNE_TYPE_SP_DOWN) || (autotune_state.tune_type == AUTOTUNE_TYPE_SP_UP)) {
rotation_rate_filt.reset(autotune_start_rate);
} else {
@ -396,7 +405,7 @@ void Copter::autotune_attitude_control()
autotune_target_angle = constrain_float(attitude_control.max_angle_step_bf_pitch(), AUTOTUNE_TARGET_MIN_ANGLE_RLLPIT_CD, AUTOTUNE_TARGET_ANGLE_RLLPIT_CD);
autotune_start_rate = ToDeg(ahrs.get_gyro().y) * 100.0f;
autotune_start_angle = ahrs.pitch_sensor;
rotation_rate_filt.set_cutoff_frequency(g.pid_rate_pitch.filt_hz()*2.0f);
rotation_rate_filt.set_cutoff_frequency(attitude_control.get_rate_pitch_pid().filt_hz()*2.0f);
if ((autotune_state.tune_type == AUTOTUNE_TYPE_SP_DOWN) || (autotune_state.tune_type == AUTOTUNE_TYPE_SP_UP)) {
rotation_rate_filt.reset(autotune_start_rate);
} else {
@ -758,35 +767,35 @@ void Copter::autotune_backup_gains_and_initialise()
orig_bf_feedforward = attitude_control.get_bf_feedforward();
// backup original pids and initialise tuned pid values
orig_roll_rp = g.pid_rate_roll.kP();
orig_roll_ri = g.pid_rate_roll.kI();
orig_roll_rd = g.pid_rate_roll.kD();
orig_roll_sp = g.p_stabilize_roll.kP();
orig_roll_rp = attitude_control.get_rate_roll_pid().kP();
orig_roll_ri = attitude_control.get_rate_roll_pid().kI();
orig_roll_rd = attitude_control.get_rate_roll_pid().kD();
orig_roll_sp = attitude_control.get_angle_roll_p().kP();
orig_roll_accel = attitude_control.get_accel_roll_max();
tune_roll_rp = g.pid_rate_roll.kP();
tune_roll_rd = g.pid_rate_roll.kD();
tune_roll_sp = g.p_stabilize_roll.kP();
tune_roll_rp = attitude_control.get_rate_roll_pid().kP();
tune_roll_rd = attitude_control.get_rate_roll_pid().kD();
tune_roll_sp = attitude_control.get_angle_roll_p().kP();
tune_roll_accel = attitude_control.get_accel_roll_max();
orig_pitch_rp = g.pid_rate_pitch.kP();
orig_pitch_ri = g.pid_rate_pitch.kI();
orig_pitch_rd = g.pid_rate_pitch.kD();
orig_pitch_sp = g.p_stabilize_pitch.kP();
orig_pitch_rp = attitude_control.get_rate_pitch_pid().kP();
orig_pitch_ri = attitude_control.get_rate_pitch_pid().kI();
orig_pitch_rd = attitude_control.get_rate_pitch_pid().kD();
orig_pitch_sp = attitude_control.get_angle_pitch_p().kP();
orig_pitch_accel = attitude_control.get_accel_pitch_max();
tune_pitch_rp = g.pid_rate_pitch.kP();
tune_pitch_rd = g.pid_rate_pitch.kD();
tune_pitch_sp = g.p_stabilize_pitch.kP();
tune_pitch_rp = attitude_control.get_rate_pitch_pid().kP();
tune_pitch_rd = attitude_control.get_rate_pitch_pid().kD();
tune_pitch_sp = attitude_control.get_angle_pitch_p().kP();
tune_pitch_accel = attitude_control.get_accel_pitch_max();
orig_yaw_rp = g.pid_rate_yaw.kP();
orig_yaw_ri = g.pid_rate_yaw.kI();
orig_yaw_rd = g.pid_rate_yaw.kD();
orig_yaw_rLPF = g.pid_rate_yaw.filt_hz();
orig_yaw_rp = attitude_control.get_rate_yaw_pid().kP();
orig_yaw_ri = attitude_control.get_rate_yaw_pid().kI();
orig_yaw_rd = attitude_control.get_rate_yaw_pid().kD();
orig_yaw_rLPF = attitude_control.get_rate_yaw_pid().filt_hz();
orig_yaw_accel = attitude_control.get_accel_yaw_max();
orig_yaw_sp = g.p_stabilize_yaw.kP();
tune_yaw_rp = g.pid_rate_yaw.kP();
tune_yaw_rLPF = g.pid_rate_yaw.filt_hz();
tune_yaw_sp = g.p_stabilize_yaw.kP();
orig_yaw_sp = attitude_control.get_angle_yaw_p().kP();
tune_yaw_rp = attitude_control.get_rate_yaw_pid().kP();
tune_yaw_rLPF = attitude_control.get_rate_yaw_pid().filt_hz();
tune_yaw_sp = attitude_control.get_angle_yaw_p().kP();
tune_yaw_accel = attitude_control.get_accel_yaw_max();
Log_Write_Event(DATA_AUTOTUNE_INITIALISED);
@ -799,29 +808,29 @@ void Copter::autotune_load_orig_gains()
attitude_control.bf_feedforward(orig_bf_feedforward);
if (autotune_roll_enabled()) {
if (!is_zero(orig_roll_rp)) {
g.pid_rate_roll.kP(orig_roll_rp);
g.pid_rate_roll.kI(orig_roll_ri);
g.pid_rate_roll.kD(orig_roll_rd);
g.p_stabilize_roll.kP(orig_roll_sp);
attitude_control.get_rate_roll_pid().kP(orig_roll_rp);
attitude_control.get_rate_roll_pid().kI(orig_roll_ri);
attitude_control.get_rate_roll_pid().kD(orig_roll_rd);
attitude_control.get_angle_roll_p().kP(orig_roll_sp);
attitude_control.set_accel_roll_max(orig_roll_accel);
}
}
if (autotune_pitch_enabled()) {
if (!is_zero(orig_pitch_rp)) {
g.pid_rate_pitch.kP(orig_pitch_rp);
g.pid_rate_pitch.kI(orig_pitch_ri);
g.pid_rate_pitch.kD(orig_pitch_rd);
g.p_stabilize_pitch.kP(orig_pitch_sp);
attitude_control.get_rate_pitch_pid().kP(orig_pitch_rp);
attitude_control.get_rate_pitch_pid().kI(orig_pitch_ri);
attitude_control.get_rate_pitch_pid().kD(orig_pitch_rd);
attitude_control.get_angle_pitch_p().kP(orig_pitch_sp);
attitude_control.set_accel_pitch_max(orig_pitch_accel);
}
}
if (autotune_yaw_enabled()) {
if (!is_zero(orig_yaw_rp)) {
g.pid_rate_yaw.kP(orig_yaw_rp);
g.pid_rate_yaw.kI(orig_yaw_ri);
g.pid_rate_yaw.kD(orig_yaw_rd);
g.pid_rate_yaw.filt_hz(orig_yaw_rLPF);
g.p_stabilize_yaw.kP(orig_yaw_sp);
attitude_control.get_rate_yaw_pid().kP(orig_yaw_rp);
attitude_control.get_rate_yaw_pid().kI(orig_yaw_ri);
attitude_control.get_rate_yaw_pid().kD(orig_yaw_rd);
attitude_control.get_rate_yaw_pid().filt_hz(orig_yaw_rLPF);
attitude_control.get_angle_yaw_p().kP(orig_yaw_sp);
attitude_control.set_accel_yaw_max(orig_yaw_accel);
}
}
@ -837,29 +846,29 @@ void Copter::autotune_load_tuned_gains()
}
if (autotune_roll_enabled()) {
if (!is_zero(tune_roll_rp)) {
g.pid_rate_roll.kP(tune_roll_rp);
g.pid_rate_roll.kI(tune_roll_rp*AUTOTUNE_PI_RATIO_FINAL);
g.pid_rate_roll.kD(tune_roll_rd);
g.p_stabilize_roll.kP(tune_roll_sp);
attitude_control.get_rate_roll_pid().kP(tune_roll_rp);
attitude_control.get_rate_roll_pid().kI(tune_roll_rp*AUTOTUNE_PI_RATIO_FINAL);
attitude_control.get_rate_roll_pid().kD(tune_roll_rd);
attitude_control.get_angle_roll_p().kP(tune_roll_sp);
attitude_control.set_accel_roll_max(tune_roll_accel);
}
}
if (autotune_pitch_enabled()) {
if (!is_zero(tune_pitch_rp)) {
g.pid_rate_pitch.kP(tune_pitch_rp);
g.pid_rate_pitch.kI(tune_pitch_rp*AUTOTUNE_PI_RATIO_FINAL);
g.pid_rate_pitch.kD(tune_pitch_rd);
g.p_stabilize_pitch.kP(tune_pitch_sp);
attitude_control.get_rate_pitch_pid().kP(tune_pitch_rp);
attitude_control.get_rate_pitch_pid().kI(tune_pitch_rp*AUTOTUNE_PI_RATIO_FINAL);
attitude_control.get_rate_pitch_pid().kD(tune_pitch_rd);
attitude_control.get_angle_pitch_p().kP(tune_pitch_sp);
attitude_control.set_accel_pitch_max(tune_pitch_accel);
}
}
if (autotune_yaw_enabled()) {
if (!is_zero(tune_yaw_rp)) {
g.pid_rate_yaw.kP(tune_yaw_rp);
g.pid_rate_yaw.kI(tune_yaw_rp*AUTOTUNE_YAW_PI_RATIO_FINAL);
g.pid_rate_yaw.kD(0.0f);
g.pid_rate_yaw.filt_hz(tune_yaw_rLPF);
g.p_stabilize_yaw.kP(tune_yaw_sp);
attitude_control.get_rate_yaw_pid().kP(tune_yaw_rp);
attitude_control.get_rate_yaw_pid().kI(tune_yaw_rp*AUTOTUNE_YAW_PI_RATIO_FINAL);
attitude_control.get_rate_yaw_pid().kD(0.0f);
attitude_control.get_rate_yaw_pid().filt_hz(tune_yaw_rLPF);
attitude_control.get_angle_yaw_p().kP(tune_yaw_sp);
attitude_control.set_accel_yaw_max(tune_yaw_accel);
}
}
@ -873,23 +882,23 @@ void Copter::autotune_load_intra_test_gains()
// sanity check the gains
attitude_control.bf_feedforward(true);
if (autotune_roll_enabled()) {
g.pid_rate_roll.kP(orig_roll_rp);
g.pid_rate_roll.kI(orig_roll_rp*AUTOTUNE_PI_RATIO_FOR_TESTING);
g.pid_rate_roll.kD(orig_roll_rd);
g.p_stabilize_roll.kP(orig_roll_sp);
attitude_control.get_rate_roll_pid().kP(orig_roll_rp);
attitude_control.get_rate_roll_pid().kI(orig_roll_rp*AUTOTUNE_PI_RATIO_FOR_TESTING);
attitude_control.get_rate_roll_pid().kD(orig_roll_rd);
attitude_control.get_angle_roll_p().kP(orig_roll_sp);
}
if (autotune_pitch_enabled()) {
g.pid_rate_pitch.kP(orig_pitch_rp);
g.pid_rate_pitch.kI(orig_pitch_rp*AUTOTUNE_PI_RATIO_FOR_TESTING);
g.pid_rate_pitch.kD(orig_pitch_rd);
g.p_stabilize_pitch.kP(orig_pitch_sp);
attitude_control.get_rate_pitch_pid().kP(orig_pitch_rp);
attitude_control.get_rate_pitch_pid().kI(orig_pitch_rp*AUTOTUNE_PI_RATIO_FOR_TESTING);
attitude_control.get_rate_pitch_pid().kD(orig_pitch_rd);
attitude_control.get_angle_pitch_p().kP(orig_pitch_sp);
}
if (autotune_yaw_enabled()) {
g.pid_rate_yaw.kP(orig_yaw_rp);
g.pid_rate_yaw.kI(orig_yaw_rp*AUTOTUNE_PI_RATIO_FOR_TESTING);
g.pid_rate_yaw.kD(orig_yaw_rd);
g.pid_rate_yaw.filt_hz(orig_yaw_rLPF);
g.p_stabilize_yaw.kP(orig_yaw_sp);
attitude_control.get_rate_yaw_pid().kP(orig_yaw_rp);
attitude_control.get_rate_yaw_pid().kI(orig_yaw_rp*AUTOTUNE_PI_RATIO_FOR_TESTING);
attitude_control.get_rate_yaw_pid().kD(orig_yaw_rd);
attitude_control.get_rate_yaw_pid().filt_hz(orig_yaw_rLPF);
attitude_control.get_angle_yaw_p().kP(orig_yaw_sp);
}
}
@ -899,23 +908,23 @@ void Copter::autotune_load_twitch_gains()
{
switch (autotune_state.axis) {
case AUTOTUNE_AXIS_ROLL:
g.pid_rate_roll.kP(tune_roll_rp);
g.pid_rate_roll.kI(tune_roll_rp*0.01f);
g.pid_rate_roll.kD(tune_roll_rd);
g.p_stabilize_roll.kP(tune_roll_sp);
attitude_control.get_rate_roll_pid().kP(tune_roll_rp);
attitude_control.get_rate_roll_pid().kI(tune_roll_rp*0.01f);
attitude_control.get_rate_roll_pid().kD(tune_roll_rd);
attitude_control.get_angle_roll_p().kP(tune_roll_sp);
break;
case AUTOTUNE_AXIS_PITCH:
g.pid_rate_pitch.kP(tune_pitch_rp);
g.pid_rate_pitch.kI(tune_pitch_rp*0.01f);
g.pid_rate_pitch.kD(tune_pitch_rd);
g.p_stabilize_pitch.kP(tune_pitch_sp);
attitude_control.get_rate_pitch_pid().kP(tune_pitch_rp);
attitude_control.get_rate_pitch_pid().kI(tune_pitch_rp*0.01f);
attitude_control.get_rate_pitch_pid().kD(tune_pitch_rd);
attitude_control.get_angle_pitch_p().kP(tune_pitch_sp);
break;
case AUTOTUNE_AXIS_YAW:
g.pid_rate_yaw.kP(tune_yaw_rp);
g.pid_rate_yaw.kI(tune_yaw_rp*0.01f);
g.pid_rate_yaw.kD(0.0f);
g.pid_rate_yaw.filt_hz(tune_yaw_rLPF);
g.p_stabilize_yaw.kP(tune_yaw_sp);
attitude_control.get_rate_yaw_pid().kP(tune_yaw_rp);
attitude_control.get_rate_yaw_pid().kI(tune_yaw_rp*0.01f);
attitude_control.get_rate_yaw_pid().kD(0.0f);
attitude_control.get_rate_yaw_pid().filt_hz(tune_yaw_rLPF);
attitude_control.get_angle_yaw_p().kP(tune_yaw_sp);
break;
}
}
@ -936,67 +945,67 @@ void Copter::autotune_save_tuning_gains()
// sanity check the rate P values
if (autotune_roll_enabled() && !is_zero(tune_roll_rp)) {
// rate roll gains
g.pid_rate_roll.kP(tune_roll_rp);
g.pid_rate_roll.kI(tune_roll_rp*AUTOTUNE_PI_RATIO_FINAL);
g.pid_rate_roll.kD(tune_roll_rd);
g.pid_rate_roll.save_gains();
attitude_control.get_rate_roll_pid().kP(tune_roll_rp);
attitude_control.get_rate_roll_pid().kI(tune_roll_rp*AUTOTUNE_PI_RATIO_FINAL);
attitude_control.get_rate_roll_pid().kD(tune_roll_rd);
attitude_control.get_rate_roll_pid().save_gains();
// stabilize roll
g.p_stabilize_roll.kP(tune_roll_sp);
g.p_stabilize_roll.save_gains();
attitude_control.get_angle_roll_p().kP(tune_roll_sp);
attitude_control.get_angle_roll_p().save_gains();
// acceleration roll
attitude_control.save_accel_roll_max(tune_roll_accel);
// resave pids to originals in case the autotune is run again
orig_roll_rp = g.pid_rate_roll.kP();
orig_roll_ri = g.pid_rate_roll.kI();
orig_roll_rd = g.pid_rate_roll.kD();
orig_roll_sp = g.p_stabilize_roll.kP();
orig_roll_rp = attitude_control.get_rate_roll_pid().kP();
orig_roll_ri = attitude_control.get_rate_roll_pid().kI();
orig_roll_rd = attitude_control.get_rate_roll_pid().kD();
orig_roll_sp = attitude_control.get_angle_roll_p().kP();
}
if (autotune_pitch_enabled() && !is_zero(tune_pitch_rp)) {
// rate pitch gains
g.pid_rate_pitch.kP(tune_pitch_rp);
g.pid_rate_pitch.kI(tune_pitch_rp*AUTOTUNE_PI_RATIO_FINAL);
g.pid_rate_pitch.kD(tune_pitch_rd);
g.pid_rate_pitch.save_gains();
attitude_control.get_rate_pitch_pid().kP(tune_pitch_rp);
attitude_control.get_rate_pitch_pid().kI(tune_pitch_rp*AUTOTUNE_PI_RATIO_FINAL);
attitude_control.get_rate_pitch_pid().kD(tune_pitch_rd);
attitude_control.get_rate_pitch_pid().save_gains();
// stabilize pitch
g.p_stabilize_pitch.kP(tune_pitch_sp);
g.p_stabilize_pitch.save_gains();
attitude_control.get_angle_pitch_p().kP(tune_pitch_sp);
attitude_control.get_angle_pitch_p().save_gains();
// acceleration pitch
attitude_control.save_accel_pitch_max(tune_pitch_accel);
// resave pids to originals in case the autotune is run again
orig_pitch_rp = g.pid_rate_pitch.kP();
orig_pitch_ri = g.pid_rate_pitch.kI();
orig_pitch_rd = g.pid_rate_pitch.kD();
orig_pitch_sp = g.p_stabilize_pitch.kP();
orig_pitch_rp = attitude_control.get_rate_pitch_pid().kP();
orig_pitch_ri = attitude_control.get_rate_pitch_pid().kI();
orig_pitch_rd = attitude_control.get_rate_pitch_pid().kD();
orig_pitch_sp = attitude_control.get_angle_pitch_p().kP();
}
if (autotune_yaw_enabled() && !is_zero(tune_yaw_rp)) {
// rate yaw gains
g.pid_rate_yaw.kP(tune_yaw_rp);
g.pid_rate_yaw.kI(tune_yaw_rp*AUTOTUNE_YAW_PI_RATIO_FINAL);
g.pid_rate_yaw.kD(0.0f);
g.pid_rate_yaw.filt_hz(tune_yaw_rLPF);
g.pid_rate_yaw.save_gains();
attitude_control.get_rate_yaw_pid().kP(tune_yaw_rp);
attitude_control.get_rate_yaw_pid().kI(tune_yaw_rp*AUTOTUNE_YAW_PI_RATIO_FINAL);
attitude_control.get_rate_yaw_pid().kD(0.0f);
attitude_control.get_rate_yaw_pid().filt_hz(tune_yaw_rLPF);
attitude_control.get_rate_yaw_pid().save_gains();
// stabilize yaw
g.p_stabilize_yaw.kP(tune_yaw_sp);
g.p_stabilize_yaw.save_gains();
attitude_control.get_angle_yaw_p().kP(tune_yaw_sp);
attitude_control.get_angle_yaw_p().save_gains();
// acceleration yaw
attitude_control.save_accel_yaw_max(tune_yaw_accel);
// resave pids to originals in case the autotune is run again
orig_yaw_rp = g.pid_rate_yaw.kP();
orig_yaw_ri = g.pid_rate_yaw.kI();
orig_yaw_rd = g.pid_rate_yaw.kD();
orig_yaw_rLPF = g.pid_rate_yaw.filt_hz();
orig_yaw_sp = g.p_stabilize_yaw.kP();
orig_yaw_rp = attitude_control.get_rate_yaw_pid().kP();
orig_yaw_ri = attitude_control.get_rate_yaw_pid().kI();
orig_yaw_rd = attitude_control.get_rate_yaw_pid().kD();
orig_yaw_rLPF = attitude_control.get_rate_yaw_pid().filt_hz();
orig_yaw_sp = attitude_control.get_angle_yaw_p().kP();
}
// update GCS and log save gains event
autotune_update_gcs(AUTOTUNE_MESSAGE_SAVED_GAINS);
@ -1308,4 +1317,4 @@ void Copter::autotune_twitching_measure_acceleration(float &rate_of_change, floa
}
}
#endif // AUTOTUNE_ENABLED == ENABLED
#endif // AUTOTUNE_ENABLED == ENABLED

6
ArduSub/control_brake.cpp
View File

@ -36,8 +36,9 @@ bool Sub::brake_init(bool ignore_checks)
void Sub::brake_run()
{
// if not auto armed set throttle to zero and exit immediately
if(!ap.auto_armed) {
if (!motors.armed() || !ap.auto_armed || !motors.get_interlock()) {
wp_nav.init_brake_target(BRAKE_MODE_DECEL_RATE);
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
// multicopters do not stabilize roll/pitch/yaw when disarmed
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
@ -55,6 +56,9 @@ void Sub::brake_run()
init_disarm_motors();
}
// set motors to full range
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// run brake controller
wp_nav.update_brake(ekfGndSpdLimit, ekfNavVelGainScaler);

6
ArduSub/control_circle.cpp
View File

@ -42,8 +42,9 @@ void Sub::circle_run()
pos_control.set_accel_z(g.pilot_accel_z);
// if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
if(!ap.auto_armed || ap.land_complete || !motors.get_interlock()) {
if (!motors.armed() || !ap.auto_armed || ap.land_complete || !motors.get_interlock()) {
// To-Do: add some initialisation of position controllers
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
// multicopters do not stabilize roll/pitch/yaw when disarmed
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
@ -71,6 +72,9 @@ void Sub::circle_run()
}
}
// set motors to full range
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// run circle controller
circle_nav.update();

27
ArduSub/control_drift.cpp
View File

@ -18,14 +18,14 @@
#endif
#ifndef DRIFT_THR_ASSIST_MAX
# define DRIFT_THR_ASSIST_MAX 300.0f // maximum assistance throttle assist will provide
# define DRIFT_THR_ASSIST_MAX 0.3f // maximum assistance throttle assist will provide
#endif
#ifndef DRIFT_THR_MIN
# define DRIFT_THR_MIN 213 // throttle assist will be active when pilot's throttle is above this value
# define DRIFT_THR_MIN 0.213f // throttle assist will be active when pilot's throttle is above this value
#endif
#ifndef DRIFT_THR_MAX
# define DRIFT_THR_MAX 787 // throttle assist will be active when pilot's throttle is below this value
# define DRIFT_THR_MAX 0.787f // throttle assist will be active when pilot's throttle is below this value
#endif
// drift_init - initialise drift controller
@ -46,10 +46,11 @@ void Sub::drift_run()
static float roll_input = 0.0f;
float target_roll, target_pitch;
float target_yaw_rate;
int16_t pilot_throttle_scaled;
float pilot_throttle_scaled;
// if not armed or motor interlock not enabled or landed and throttle at zero, set throttle to zero and exit immediately
if(!motors.armed() || !motors.get_interlock() || (ap.land_complete && ap.throttle_zero)) {
// if landed and throttle at zero, set throttle to zero and exit immediately
if (!motors.armed() || !motors.get_interlock() || (ap.land_complete && ap.throttle_zero)) {
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
return;
}
@ -100,25 +101,21 @@ void Sub::drift_run()
attitude_control.set_throttle_out(get_throttle_assist(vel.z, pilot_throttle_scaled), true, g.throttle_filt);
}
// get_throttle_assist - return throttle output (range 0 ~ 1000) based on pilot input and z-axis velocity
int16_t Sub::get_throttle_assist(float velz, int16_t pilot_throttle_scaled)
// get_throttle_assist - return throttle output (range 0 ~ 1) based on pilot input and z-axis velocity
float Sub::get_throttle_assist(float velz, float pilot_throttle_scaled)
{
// throttle assist - adjusts throttle to slow the vehicle's vertical velocity
// Only active when pilot's throttle is between 213 ~ 787
// Assistance is strongest when throttle is at mid, drops linearly to no assistance at 213 and 787
float thr_assist = 0.0f;
if (pilot_throttle_scaled > g.throttle_min && pilot_throttle_scaled < THR_MAX &&
pilot_throttle_scaled > DRIFT_THR_MIN && pilot_throttle_scaled < DRIFT_THR_MAX) {
if (pilot_throttle_scaled > DRIFT_THR_MIN && pilot_throttle_scaled < DRIFT_THR_MAX) {
// calculate throttle assist gain
thr_assist = 1.2f - ((float)abs(pilot_throttle_scaled - 500) / 240.0f);
thr_assist = 1.2f - ((float)fabsf(pilot_throttle_scaled - 0.5f) / 0.24f);
thr_assist = constrain_float(thr_assist, 0.0f, 1.0f) * -DRIFT_THR_ASSIST_GAIN * velz;
// ensure throttle assist never adjusts the throttle by more than 300 pwm
thr_assist = constrain_float(thr_assist, -DRIFT_THR_ASSIST_MAX, DRIFT_THR_ASSIST_MAX);
// ensure throttle assist never pushes throttle below throttle_min or above throttle_max
thr_assist = constrain_float(thr_assist, g.throttle_min - pilot_throttle_scaled, THR_MAX - pilot_throttle_scaled);
}
return pilot_throttle_scaled + thr_assist;
return constrain_float(pilot_throttle_scaled + thr_assist, 0.0f, 1.0f);
}

4
ArduSub/control_flip.cpp
View File

@ -96,7 +96,7 @@ bool Sub::flip_init(bool ignore_checks)
// should be called at 100hz or more
void Sub::flip_run()
{
int16_t throttle_out;
float throttle_out;
float recovery_angle;
// if pilot inputs roll > 40deg or timeout occurs abandon flip
@ -220,7 +220,7 @@ void Sub::flip_run()
}
// output pilot's throttle without angle boost
if (throttle_out == 0) {
if (is_zero(throttle_out)) {
attitude_control.set_throttle_out_unstabilized(0,false,g.throttle_filt);
} else {
attitude_control.set_throttle_out(throttle_out, false, g.throttle_filt);

30
ArduSub/control_guided.cpp
View File

@ -251,7 +251,8 @@ void Sub::guided_run()
void Sub::guided_takeoff_run()
{
// if not auto armed or motors not enabled set throttle to zero and exit immediately
if (!ap.auto_armed || !motors.get_interlock()) {
if (!motors.armed() || !ap.auto_armed || !motors.get_interlock()) {
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
// multicopters do not stabilize roll/pitch/yaw when disarmed
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
@ -265,6 +266,9 @@ void Sub::guided_takeoff_run()
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in);
}
// set motors to full range
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// run waypoint controller
wp_nav.update_wpnav();
@ -280,7 +284,8 @@ void Sub::guided_takeoff_run()
void Sub::guided_pos_control_run()
{
// if not auto armed or motors not enabled set throttle to zero and exit immediately
if (!ap.auto_armed || !motors.get_interlock() || ap.land_complete) {
if (!motors.armed() || !ap.auto_armed || !motors.get_interlock() || ap.land_complete) {
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
// multicopters do not stabilize roll/pitch/yaw when disarmed
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
@ -297,6 +302,9 @@ void Sub::guided_pos_control_run()
}
}
// set motors to full range
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// run waypoint controller
wp_nav.update_wpnav();
@ -318,9 +326,10 @@ void Sub::guided_pos_control_run()
void Sub::guided_vel_control_run()
{
// if not auto armed or motors not enabled set throttle to zero and exit immediately
if (!ap.auto_armed || !motors.get_interlock() || ap.land_complete) {
if (!motors.armed() || !ap.auto_armed || !motors.get_interlock() || ap.land_complete) {
// initialise velocity controller
pos_control.init_vel_controller_xyz();
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
// multicopters do not stabilize roll/pitch/yaw when disarmed
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
@ -337,6 +346,9 @@ void Sub::guided_vel_control_run()
}
}
// set motors to full range
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// set velocity to zero if no updates received for 3 seconds
uint32_t tnow = millis();
if (tnow - vel_update_time_ms > GUIDED_POSVEL_TIMEOUT_MS && !pos_control.get_desired_velocity().is_zero()) {
@ -361,10 +373,11 @@ void Sub::guided_vel_control_run()
void Sub::guided_posvel_control_run()
{
// if not auto armed or motors not enabled set throttle to zero and exit immediately
if (!ap.auto_armed || !motors.get_interlock() || ap.land_complete) {
if (!motors.armed() || !ap.auto_armed || !motors.get_interlock() || ap.land_complete) {
// set target position and velocity to current position and velocity
pos_control.set_pos_target(inertial_nav.get_position());
pos_control.set_desired_velocity(Vector3f(0,0,0));
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
// multicopters do not stabilize roll/pitch/yaw when disarmed
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
@ -382,6 +395,9 @@ void Sub::guided_posvel_control_run()
}
}
// set motors to full range
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// set velocity to zero if no updates received for 3 seconds
uint32_t tnow = millis();
if (tnow - posvel_update_time_ms > GUIDED_POSVEL_TIMEOUT_MS && !posvel_vel_target_cms.is_zero()) {
@ -426,7 +442,8 @@ void Sub::guided_posvel_control_run()
void Sub::guided_angle_control_run()
{
// if not auto armed or motors not enabled set throttle to zero and exit immediately
if (!ap.auto_armed || !motors.get_interlock() || ap.land_complete) {
if (!motors.armed() || !ap.auto_armed || !motors.get_interlock() || ap.land_complete) {
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
// multicopters do not stabilize roll/pitch/yaw when disarmed
attitude_control.set_throttle_out_unstabilized(0.0f,true,g.throttle_filt);
@ -459,6 +476,9 @@ void Sub::guided_angle_control_run()
climb_rate_cms = 0.0f;
}
// set motors to full range
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_yaw(roll_in, pitch_in, yaw_in, true);

12
ArduSub/control_land.cpp
View File

@ -56,7 +56,8 @@ void Sub::land_gps_run()
float target_yaw_rate = 0;
// if not auto armed or landed or motor interlock not enabled set throttle to zero and exit immediately
if(!ap.auto_armed || ap.land_complete || !motors.get_interlock()) {
if (!motors.armed() || !ap.auto_armed || ap.land_complete || !motors.get_interlock()) {
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
// multicopters do not stabilize roll/pitch/yaw when disarmed
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
@ -101,6 +102,9 @@ void Sub::land_gps_run()
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in);
}
// set motors to full range
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// process roll, pitch inputs
wp_nav.set_pilot_desired_acceleration(roll_control, pitch_control);
@ -157,7 +161,8 @@ void Sub::land_nogps_run()
}
// if not auto armed or landed or motor interlock not enabled set throttle to zero and exit immediately
if(!ap.auto_armed || ap.at_surface || !motors.get_interlock()) {
if (!motors.armed() || !ap.auto_armed || ap.land_complete || !motors.get_interlock()) {
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
// multicopters do not stabilize roll/pitch/yaw when disarmed
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
@ -175,6 +180,9 @@ void Sub::land_nogps_run()
return;
}
// set motors to full range
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());

16
ArduSub/control_loiter.cpp
View File

@ -83,6 +83,7 @@ void Sub::loiter_run()
case Loiter_Disarmed:
wp_nav.init_loiter_target();
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
// multicopters do not stabilize roll/pitch/yaw when disarmed
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
@ -90,6 +91,7 @@ void Sub::loiter_run()
break;
case Loiter_MotorStop:
motors.set_desired_spool_state(AP_Motors::DESIRED_SHUT_DOWN);
wp_nav.init_loiter_target();
// multicopters do not stabilize roll/pitch/yaw when motors are stopped
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
@ -109,6 +111,9 @@ void Sub::loiter_run()
// get takeoff adjusted pilot and takeoff climb rates
takeoff_get_climb_rates(target_climb_rate, takeoff_climb_rate);
// set motors to full range
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// run loiter controller
wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
@ -124,13 +129,20 @@ void Sub::loiter_run()
case Loiter_Landed:
wp_nav.init_loiter_target();
// if throttle zero reset attitude and exit immediately
if (ap.throttle_zero) {
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
}else{
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
}
// multicopters do not stabilize roll/pitch/yaw when disarmed
// move throttle to between minimum and non-takeoff-throttle to keep us on the ground
attitude_control.set_throttle_out_unstabilized(get_throttle_pre_takeoff(channel_throttle->control_in),true,g.throttle_filt);
attitude_control.set_throttle_out(get_throttle_pre_takeoff(channel_throttle->control_in),false,g.throttle_filt);
pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->control_in)-throttle_average);
break;
case Loiter_Flying:
// set motors to full range
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// run loiter controller
wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);

14
ArduSub/control_poshold.cpp
View File

@ -141,7 +141,8 @@ void Copter::poshold_run()
pos_control.set_accel_z(g.pilot_accel_z);
// if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
if(!ap.auto_armed || !motors.get_interlock()) {
if (!motors.armed() || !ap.auto_armed || !motors.get_interlock()) {
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
wp_nav.init_loiter_target();
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->control_in)-throttle_average);
@ -183,9 +184,15 @@ void Copter::poshold_run()
// if landed initialise loiter targets, set throttle to zero and exit
if (ap.land_complete) {
// if throttle zero reset attitude and exit immediately
if (ap.throttle_zero) {
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
}else{
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
}
wp_nav.init_loiter_target();
// move throttle to between minimum and non-takeoff-throttle to keep us on the ground
attitude_control.set_throttle_out_unstabilized(get_throttle_pre_takeoff(channel_throttle->control_in),true,g.throttle_filt);
attitude_control.set_throttle_out(get_throttle_pre_takeoff(channel_throttle->control_in),false,g.throttle_filt);
pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->control_in)-throttle_average);
return;
}else{
@ -389,6 +396,9 @@ void Copter::poshold_run()
break;
}
// set motors to full range
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
//
// Shared roll & pitch states (POSHOLD_BRAKE_TO_LOITER and POSHOLD_LOITER)
//

24
ArduSub/control_rtl.cpp
View File

@ -140,7 +140,8 @@ void Sub::rtl_return_start()
void Sub::rtl_climb_return_run()
{
// if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
if(!ap.auto_armed || !motors.get_interlock()) {
if (!motors.armed() || !ap.auto_armed || !motors.get_interlock()) {
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
// multicopters do not stabilize roll/pitch/yaw when disarmed
// reset attitude control targets
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
@ -159,6 +160,9 @@ void Sub::rtl_climb_return_run()
}
}
// set motors to full range
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// run waypoint controller
wp_nav.update_wpnav();
@ -198,7 +202,8 @@ void Sub::rtl_loiterathome_start()
void Sub::rtl_loiterathome_run()
{
// if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
if(!ap.auto_armed || !motors.get_interlock()) {
if (!motors.armed() || !ap.auto_armed || !motors.get_interlock()) {
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
// multicopters do not stabilize roll/pitch/yaw when disarmed
// reset attitude control targets
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
@ -217,6 +222,9 @@ void Sub::rtl_loiterathome_run()
}
}
// set motors to full range
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// run waypoint controller
wp_nav.update_wpnav();
@ -270,7 +278,8 @@ void Sub::rtl_descent_run()
float target_yaw_rate = 0;
// if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
if(!ap.auto_armed || !motors.get_interlock()) {
if (!motors.armed() || !ap.auto_armed || !motors.get_interlock()) {
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
// multicopters do not stabilize roll/pitch/yaw when disarmed
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
@ -294,6 +303,9 @@ void Sub::rtl_descent_run()
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in);
}
// set motors to full range
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// process roll, pitch inputs
wp_nav.set_pilot_desired_acceleration(roll_control, pitch_control);
@ -334,7 +346,8 @@ void Sub::rtl_land_run()
int16_t roll_control = 0, pitch_control = 0;
float target_yaw_rate = 0;
// if not auto armed or landing completed or motor interlock not enabled set throttle to zero and exit immediately
if(!ap.auto_armed || ap.land_complete || !motors.get_interlock()) {
if (!motors.armed() || !ap.auto_armed || ap.land_complete || !motors.get_interlock()) {
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
// multicopters do not stabilize roll/pitch/yaw when disarmed
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
@ -378,6 +391,9 @@ void Sub::rtl_land_run()
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in);
}
// set motors to full range
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// process pilot's roll and pitch input
wp_nav.set_pilot_desired_acceleration(roll_control, pitch_control);

12
ArduSub/control_sport.cpp
View File

@ -33,7 +33,8 @@ void Sub::sport_run()
pos_control.set_accel_z(g.pilot_accel_z);
// if not armed or throttle at zero, set throttle to zero and exit immediately
if(!motors.armed()) {
if (!motors.armed() || !motors.get_interlock()) {
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->control_in)-throttle_average);
return;
@ -91,10 +92,17 @@ void Sub::sport_run()
// reset target lean angles and heading while landed
if (ap.land_complete) {
if (ap.throttle_zero) {
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
}else{
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
}
// move throttle to between minimum and non-takeoff-throttle to keep us on the ground
attitude_control.set_throttle_out_unstabilized(get_throttle_pre_takeoff(channel_throttle->control_in),true,g.throttle_filt);
attitude_control.set_throttle_out(get_throttle_pre_takeoff(channel_throttle->control_in),false,g.throttle_filt);
pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->control_in)-throttle_average);
}else{
// set motors to full range
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// call attitude controller
attitude_control.input_euler_rate_roll_pitch_yaw(target_roll_rate, target_pitch_rate, target_yaw_rate);

15
ArduSub/control_stabilize.cpp
View File

@ -10,7 +10,7 @@
bool Sub::stabilize_init(bool ignore_checks)
{
// if landed and the mode we're switching from does not have manual throttle and the throttle stick is too high
if (motors.armed() && ap.land_complete && !mode_has_manual_throttle(control_mode) && (g.rc_3.control_in > get_non_takeoff_throttle())) {
if (motors.armed() && ap.land_complete && !mode_has_manual_throttle(control_mode) && (get_pilot_desired_throttle(channel_throttle->control_in) > get_non_takeoff_throttle())) {
return false;
}
// set target altitude to zero for reporting
@ -25,18 +25,17 @@ void Sub::stabilize_run()
{
float target_roll, target_pitch;
float target_yaw_rate;
int16_t pilot_throttle_scaled;
float pilot_throttle_scaled;
// if not armed or throttle at zero, set throttle to zero and exit immediately
if(!motors.armed()) {
// if not armed set throttle to zero and exit immediately
if (!motors.armed() || ap.throttle_zero || !motors.get_interlock()) {
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
// slow start if landed
if (ap.land_complete) {
motors.slow_start(true);
}
return;
}
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// apply SIMPLE mode transform to pilot inputs
update_simple_mode();

3
ArduSub/system.cpp
View File

@ -192,8 +192,7 @@ void Sub::init_ardupilot()
ahrs.set_optflow(&optflow);
#endif
// initialise attitude and position controllers
attitude_control.set_dt(MAIN_LOOP_SECONDS);
// initialise position controllers
pos_control.set_dt(MAIN_LOOP_SECONDS);
// init the optical flow sensor

36
ArduSub/tuning.cpp
View File

@ -25,36 +25,36 @@ void Sub::tuning() {
// Roll, Pitch tuning
case TUNING_STABILIZE_ROLL_PITCH_KP:
g.p_stabilize_roll.kP(tuning_value);
g.p_stabilize_pitch.kP(tuning_value);
attitude_control.get_angle_roll_p().kP(tuning_value);
attitude_control.get_angle_pitch_p().kP(tuning_value);
break;
case TUNING_RATE_ROLL_PITCH_KP:
g.pid_rate_roll.kP(tuning_value);
g.pid_rate_pitch.kP(tuning_value);
attitude_control.get_rate_roll_pid().kP(tuning_value);
attitude_control.get_rate_pitch_pid().kP(tuning_value);
break;
case TUNING_RATE_ROLL_PITCH_KI:
g.pid_rate_roll.kI(tuning_value);
g.pid_rate_pitch.kI(tuning_value);
attitude_control.get_rate_roll_pid().kI(tuning_value);
attitude_control.get_rate_pitch_pid().kI(tuning_value);
break;
case TUNING_RATE_ROLL_PITCH_KD:
g.pid_rate_roll.kD(tuning_value);
g.pid_rate_pitch.kD(tuning_value);
attitude_control.get_rate_roll_pid().kD(tuning_value);
attitude_control.get_rate_pitch_pid().kD(tuning_value);
break;
// Yaw tuning
case TUNING_STABILIZE_YAW_KP:
g.p_stabilize_yaw.kP(tuning_value);
attitude_control.get_angle_yaw_p().kP(tuning_value);
break;
case TUNING_YAW_RATE_KP:
g.pid_rate_yaw.kP(tuning_value);
attitude_control.get_rate_yaw_pid().kP(tuning_value);
break;
case TUNING_YAW_RATE_KD:
g.pid_rate_yaw.kD(tuning_value);
attitude_control.get_rate_yaw_pid().kD(tuning_value);
break;
// Altitude and throttle tuning
@ -158,27 +158,27 @@ void Sub::tuning() {
break;
case TUNING_RATE_PITCH_KP:
g.pid_rate_pitch.kP(tuning_value);
attitude_control.get_rate_pitch_pid().kP(tuning_value);
break;
case TUNING_RATE_PITCH_KI:
g.pid_rate_pitch.kI(tuning_value);
attitude_control.get_rate_pitch_pid().kI(tuning_value);
break;
case TUNING_RATE_PITCH_KD:
g.pid_rate_pitch.kD(tuning_value);
attitude_control.get_rate_pitch_pid().kD(tuning_value);
break;
case TUNING_RATE_ROLL_KP:
g.pid_rate_roll.kP(tuning_value);
attitude_control.get_rate_roll_pid().kP(tuning_value);
break;
case TUNING_RATE_ROLL_KI:
g.pid_rate_roll.kI(tuning_value);
attitude_control.get_rate_roll_pid().kI(tuning_value);
break;
case TUNING_RATE_ROLL_KD:
g.pid_rate_roll.kD(tuning_value);
attitude_control.get_rate_roll_pid().kD(tuning_value);
break;
case TUNING_RATE_MOT_YAW_HEADROOM:
@ -186,7 +186,7 @@ void Sub::tuning() {
break;
case TUNING_RATE_YAW_FILT:
g.pid_rate_yaw.filt_hz(tuning_value);
attitude_control.get_rate_yaw_pid().filt_hz(tuning_value);
break;
}
}