Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Copter: replace TUNE_LOW/HIGH params with TUNE_MIN/MAX 

change from uint16 to floats to ease setup
also add check that no function assigned to rc6
This commit is contained in:
Randy Mackay 2019-04-03 21:32:26 +09:00
parent 83f6bed1dc
commit a5bdd12bfa
5 changed files with 66 additions and 61 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduCopter/Copter.h
View File

@ -721,7 +721,7 @@ private:
void Log_Write_Data(uint8_t id, int16_t value);
void Log_Write_Data(uint8_t id, uint16_t value);
void Log_Write_Data(uint8_t id, float value);
void Log_Write_Parameter_Tuning(uint8_t param, float tuning_val, int16_t control_in, int16_t tune_low, int16_t tune_high);
void Log_Write_Parameter_Tuning(uint8_t param, float tuning_val, float tune_min, float tune_max);
void Log_Sensor_Health();
#if FRAME_CONFIG == HELI_FRAME
void Log_Write_Heli(void);

16
ArduCopter/Log.cpp
View File

@ -234,21 +234,19 @@ struct PACKED log_ParameterTuning {
uint64_t time_us;
uint8_t parameter; // parameter we are tuning, e.g. 39 is CH6_CIRCLE_RATE
float tuning_value; // normalized value used inside tuning() function
int16_t control_in; // raw tune input value
int16_t tuning_low; // tuning low end value
int16_t tuning_high; // tuning high end value
float tuning_min; // tuning minimum value
float tuning_max; // tuning maximum value
};
void Copter::Log_Write_Parameter_Tuning(uint8_t param, float tuning_val, int16_t control_in, int16_t tune_low, int16_t tune_high)
void Copter::Log_Write_Parameter_Tuning(uint8_t param, float tuning_val, float tune_min, float tune_max)
{
struct log_ParameterTuning pkt_tune = {
LOG_PACKET_HEADER_INIT(LOG_PARAMTUNE_MSG),
time_us : AP_HAL::micros64(),
parameter : param,
tuning_value : tuning_val,
control_in : control_in,
tuning_low : tune_low,
tuning_high : tune_high
tuning_min : tune_min,
tuning_max : tune_max
};
logger.WriteBlock(&pkt_tune, sizeof(pkt_tune));
@ -388,7 +386,7 @@ void Copter::Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_tar
const struct LogStructure Copter::log_structure[] = {
LOG_COMMON_STRUCTURES,
{ LOG_PARAMTUNE_MSG, sizeof(log_ParameterTuning),
"PTUN", "QBfHHH", "TimeUS,Param,TunVal,CtrlIn,TunLo,TunHi", "s-----", "F-----" },
"PTUN", "QBfff", "TimeUS,Param,TunVal,TunMin,TunMax", "s----", "F----" },
{ LOG_CONTROL_TUNING_MSG, sizeof(log_Control_Tuning),
"CTUN", "Qffffffefcfhh", "TimeUS,ThI,ABst,ThO,ThH,DAlt,Alt,BAlt,DSAlt,SAlt,TAlt,DCRt,CRt", "s----mmmmmmnn", "F----00B0BBBB" },
{ LOG_MOTBATT_MSG, sizeof(log_MotBatt),
@ -443,7 +441,7 @@ void Copter::Log_Write_Data(uint8_t id, uint32_t value) {}
void Copter::Log_Write_Data(uint8_t id, int16_t value) {}
void Copter::Log_Write_Data(uint8_t id, uint16_t value) {}
void Copter::Log_Write_Data(uint8_t id, float value) {}
void Copter::Log_Write_Parameter_Tuning(uint8_t param, float tuning_val, int16_t control_in, int16_t tune_low, int16_t tune_high) {}
void Copter::Log_Write_Parameter_Tuning(uint8_t param, float tuning_val, float tune_min, float tune_max) {}
void Copter::Log_Sensor_Health() {}
void Copter::Log_Write_Precland() {}
void Copter::Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target, const Vector3f& vel_target) {}

42
ArduCopter/Parameters.cpp
View File

@ -337,20 +337,6 @@ const AP_Param::Info Copter::var_info[] = {
// @Values: 0:None,1:Stab Roll/Pitch kP,4:Rate Roll/Pitch kP,5:Rate Roll/Pitch kI,21:Rate Roll/Pitch kD,3:Stab Yaw kP,6:Rate Yaw kP,26:Rate Yaw kD,56:Rate Yaw Filter,55:Motor Yaw Headroom,14:AltHold kP,7:Throttle Rate kP,34:Throttle Accel kP,35:Throttle Accel kI,36:Throttle Accel kD,12:Loiter Pos kP,22:Velocity XY kP,28:Velocity XY kI,10:WP Speed,25:Acro RollPitch kP,40:Acro Yaw kP,45:RC Feel,13:Heli Ext Gyro,38:Declination,39:Circle Rate,41:RangeFinder Gain,46:Rate Pitch kP,47:Rate Pitch kI,48:Rate Pitch kD,49:Rate Roll kP,50:Rate Roll kI,51:Rate Roll kD,52:Rate Pitch FF,53:Rate Roll FF,54:Rate Yaw FF,57:Winch
GSCALAR(radio_tuning, "TUNE", 0),
// @Param: TUNE_LOW
// @DisplayName: Tuning minimum
// @Description: The minimum value that will be applied to the parameter currently being tuned with the transmitter's channel 6 knob
// @User: Standard
// @Range: 0 32767
GSCALAR(radio_tuning_low, "TUNE_LOW", 0),
// @Param: TUNE_HIGH
// @DisplayName: Tuning maximum
// @Description: The maximum value that will be applied to the parameter currently being tuned with the transmitter's channel 6 knob
// @User: Standard
// @Range: 0 32767
GSCALAR(radio_tuning_high, "TUNE_HIGH", 1000),
// @Param: FRAME_TYPE
// @DisplayName: Frame Type (+, X, V, etc)
// @Description: Controls motor mixing for multicopters. Not used for Tri or Traditional Helicopters.
@ -927,11 +913,23 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
// Scripting is intentionally not showing up in the parameter docs until it is a more standard feature
AP_SUBGROUPINFO(scripting, "SCR_", 30, ParametersG2, AP_Scripting),
#endif
// @Param: TUNE_MIN
// @DisplayName: Tuning minimum
// @Description: Minimum value that the parameter currently being tuned with the transmitter's channel 6 knob will be set to
// @User: Standard
AP_GROUPINFO("TUNE_MIN", 31, ParametersG2, tuning_min, 0),
// @Param: TUNE_MAX
// @DisplayName: Tuning maximum
// @Description: Maximum value that the parameter currently being tuned with the transmitter's channel 6 knob will be set to
// @User: Standard
AP_GROUPINFO("TUNE_MAX", 32, ParametersG2, tuning_max, 0),
AP_GROUPEND
};
// These CHx_OPT param descriptions are here so that users of beta Mission Planner (which uses the master branch as its source of descriptions)
// These param descriptions are here so that users of beta Mission Planner (which uses the master branch as its source of descriptions)
// can get them. These lines can be removed once Copter-3.7.0-beta testing begins or we improve the source of descriptions for GCSs.
//
// @Param: CH7_OPT
@ -970,6 +968,18 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
// @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 13:Super Simple Mode, 14:Acro Trainer, 15:Sprayer, 16:Auto, 17:AutoTune, 18:Land, 19:Gripper, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 29:Landing Gear, 30:Lost Copter Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake, 37:Throw, 38:ADSB-Avoidance, 39:PrecLoiter, 40:Object Avoidance, 41:ArmDisarm, 42:SmartRTL, 43:InvertedFlight, 44:Winch Enable, 45:WinchControl
// @User: Standard
// @Param: TUNE_LOW
// @DisplayName: Tuning minimum
// @Description: The minimum value that will be applied to the parameter currently being tuned with the transmitter's channel 6 knob
// @User: Standard
// @Range: 0 32767
// @Param: TUNE_HIGH
// @DisplayName: Tuning maximum
// @Description: The maximum value that will be applied to the parameter currently being tuned with the transmitter's channel 6 knob
// @User: Standard
// @Range: 0 32767
/*
constructor for g2 object
*/

8
ArduCopter/Parameters.h
View File

@ -285,8 +285,8 @@ public:
k_param_throttle_trim, // remove
k_param_esc_calibrate,
k_param_radio_tuning,
k_param_radio_tuning_high,
k_param_radio_tuning_low,
k_param_radio_tuning_high_old, // unused
k_param_radio_tuning_low_old, // unused
k_param_rc_speed = 192,
k_param_failsafe_battery_enabled, // unused - moved to AP_BattMonitor
k_param_throttle_mid, // remove
@ -428,8 +428,6 @@ public:
AP_Int32 log_bitmask;
AP_Int8 esc_calibrate;
AP_Int8 radio_tuning;
AP_Int16 radio_tuning_high;
AP_Int16 radio_tuning_low;
AP_Int8 frame_type;
AP_Int8 disarm_delay;
@ -587,6 +585,8 @@ public:
AP_Scripting scripting;
#endif // ENABLE_SCRIPTING
AP_Float tuning_min;
AP_Float tuning_max;
};
extern const AP_Param::Info var_info[];

59
ArduCopter/tuning.cpp
View File

@ -7,23 +7,28 @@
// tuning - updates parameters based on the ch6 tuning knob's position
// should be called at 3.3hz
void Copter::tuning() {
RC_Channel *rc6 = rc().channel(CH_6);
void Copter::tuning()
{
const RC_Channel *rc6 = rc().channel(CH_6);
// exit immediately if not using tuning function, or when radio failsafe is invoked, so tuning values are not set to zero
if ((g.radio_tuning <= 0) || failsafe.radio || failsafe.radio_counter != 0 || rc6->get_radio_in() == 0) {
// exit immediately if the tuning function is not set or min and max are both zero
if ((g.radio_tuning <= 0) || (is_zero(g2.tuning_min.get()) && is_zero(g2.tuning_max.get()))) {
return;
}
uint16_t radio_in = rc6->get_radio_in();
float v = constrain_float((radio_in - rc6->get_radio_min()) / float(rc6->get_radio_max() - rc6->get_radio_min()), 0, 1);
int16_t control_in = g.radio_tuning_low + v * (g.radio_tuning_high - g.radio_tuning_low);
float tuning_value = control_in / 1000.0f;
// Tuning Value should never be outside the bounds of the specified low and high value
tuning_value = constrain_float(tuning_value, g.radio_tuning_low/1000.0f, g.radio_tuning_high/1000.0f);
// exit immediately when radio failsafe is invoked or transmitter has not been turned on
if (failsafe.radio || failsafe.radio_counter != 0 || rc6->get_radio_in() == 0) {
return;
}
Log_Write_Parameter_Tuning(g.radio_tuning, tuning_value, control_in, g.radio_tuning_low, g.radio_tuning_high);
// exit immediately if a function is assigned to channel 6
if ((RC_Channel::aux_func_t)rc6->option.get() != RC_Channel::AUX_FUNC::DO_NOTHING) {
return;
}
const uint16_t radio_in = rc6->get_radio_in();
float tuning_value = linear_interpolate(g2.tuning_min, g2.tuning_max, radio_in, rc6->get_radio_min(), rc6->get_radio_max());
Log_Write_Parameter_Tuning(g.radio_tuning, tuning_value, g2.tuning_min, g2.tuning_max);
switch(g.radio_tuning) {
@ -96,8 +101,7 @@ void Copter::tuning() {
break;
case TUNING_WP_SPEED:
// set waypoint navigation horizontal speed to 0 ~ 1000 cm/s
wp_nav->set_speed_xy(control_in);
wp_nav->set_speed_xy(tuning_value);
break;
// Acro roll pitch gain
@ -112,7 +116,7 @@ void Copter::tuning() {
#if FRAME_CONFIG == HELI_FRAME
case TUNING_HELI_EXTERNAL_GYRO:
motors->ext_gyro_gain((float)control_in / 1000.0f);
motors->ext_gyro_gain(tuning_value);
break;
case TUNING_RATE_PITCH_FF:
@ -129,14 +133,12 @@ void Copter::tuning() {
#endif
case TUNING_DECLINATION:
// set declination to +-20degrees
compass.set_declination(ToRad((2.0f * control_in - g.radio_tuning_high)/100.0f), false); // 2nd parameter is false because we do not want to save to eeprom because this would have a performance impact
compass.set_declination(ToRad(tuning_value), false); // 2nd parameter is false because we do not want to save to eeprom because this would have a performance impact
break;
#if MODE_CIRCLE_ENABLED == ENABLED
case TUNING_CIRCLE_RATE:
// set circle rate up to approximately 45 deg/sec in either direction
circle_nav->set_rate((float)control_in/25.0f-20.0f);
circle_nav->set_rate(tuning_value);
break;
#endif
@ -179,8 +181,7 @@ void Copter::tuning() {
#endif
case TUNING_RC_FEEL_RP:
// convert from control_in to input time constant
attitude_control->set_input_tc(1.0f / (2.0f + MAX((control_in * 0.01f), 0.0f)));
attitude_control->set_input_tc(tuning_value);
break;
case TUNING_RATE_PITCH_KP:
@ -209,7 +210,7 @@ void Copter::tuning() {
#if FRAME_CONFIG != HELI_FRAME
case TUNING_RATE_MOT_YAW_HEADROOM:
motors->set_yaw_headroom(tuning_value*1000);
motors->set_yaw_headroom(tuning_value);
break;
#endif
@ -218,17 +219,13 @@ void Copter::tuning() {
break;
#if WINCH_ENABLED == ENABLED
case TUNING_WINCH: {
float desired_rate = 0.0f;
if (v > 0.6f) {
desired_rate = g2.winch.get_rate_max() * (v - 0.6f) / 0.4f;
case TUNING_WINCH:
// add small deadzone
if (fabsf(tuning_value) < 0.05f) {
tuning_value = 0;
}
if (v < 0.4f) {
desired_rate = g2.winch.get_rate_max() * (v - 0.4) / 0.4f;
}
g2.winch.set_desired_rate(desired_rate);
g2.winch.set_desired_rate(tuning_value);
break;
}
#endif
}
}