AP_Motors: Tradheli-complete conversion of throttle curve params to percent

add conversion for throttle curve params for upgrade
This commit is contained in:
bnsgeyer 2019-02-15 20:52:09 -05:00 committed by Randy Mackay
parent 3629541a20
commit 4120e29614
3 changed files with 34 additions and 25 deletions

View File

@ -1339,5 +1339,17 @@ void Copter::convert_tradheli_parameters(void)
AP_Param::convert_old_parameter(&dualheli_conversion_info[i], 1.0f);
}
}
const AP_Param::ConversionInfo allheli_conversion_info[] = {
{ Parameters::k_param_motors, 1280, AP_PARAM_INT16, "H_RSC_CRV_000" },
{ Parameters::k_param_motors, 1344, AP_PARAM_INT16, "H_RSC_CRV_025" },
{ Parameters::k_param_motors, 1408, AP_PARAM_INT16, "H_RSC_CRV_050" },
{ Parameters::k_param_motors, 1472, AP_PARAM_INT16, "H_RSC_CRV_075" },
{ Parameters::k_param_motors, 1536, AP_PARAM_INT16, "H_RSC_CRV_100" },
};
// convert dual heli parameters without scaling
uint8_t table_size = ARRAY_SIZE(allheli_conversion_info);
for (uint8_t i=0; i<table_size; i++) {
AP_Param::convert_old_parameter(&allheli_conversion_info[i], 0.1f);
}
}
#endif

View File

@ -29,41 +29,41 @@ const AP_Param::GroupInfo RSCThrCrvInt16Param::var_info[] = {
// @User: Advanced
AP_GROUPINFO_FLAGS("ENABLE", 1, RSCThrCrvInt16Param, enable, 0, AP_PARAM_FLAG_ENABLE),
// @Param: 0
// @Param: 000
// @DisplayName: Throttle Servo Position in percent for 0 percent collective
// @Description: Throttle Servo Position in percent for 0 percent collective. This is on a scale from 0 to 100, where 100 is full throttle and 0 is zero throttle. Actual PWM values are controlled by SERVOX_MIN and SERVOX_MAX. The 0 percent collective is defined by H_COL_MIN and 100 percent collective is defined by H_COL_MAX.
// @Range: 0 100
// @Increment: 1
// @User: Standard
AP_GROUPINFO("0", 2, RSCThrCrvInt16Param, thrcrv[0], AP_MOTORS_HELI_RSC_THRCRV_0_DEFAULT),
AP_GROUPINFO("000", 2, RSCThrCrvInt16Param, thrcrv[0], AP_MOTORS_HELI_RSC_THRCRV_0_DEFAULT),
// @Param: 25
// @DisplayName: Throttle Servo Position for 25 percent collective
// @Description: Throttle Servo Position for 25 percent collective. This is on a scale from 0 to 1000, where 1000 is full throttle and 0 is zero throttle. Actual PWM values are controlled by SERVOX_MIN and SERVOX_MAX. The 0 percent collective is defined by H_COL_MIN and 100 percent collective is defined by H_COL_MAX.
// @Param: 025
// @DisplayName: Throttle Servo Position in percent for 25 percent collective
// @Description: Throttle Servo Position in percent for 25 percent collective. This is on a scale from 0 to 100, where 100 is full throttle and 0 is zero throttle. Actual PWM values are controlled by SERVOX_MIN and SERVOX_MAX. The 0 percent collective is defined by H_COL_MIN and 100 percent collective is defined by H_COL_MAX.
// @Range: 0 100
// @Increment: 1
// @User: Standard
AP_GROUPINFO("25", 3, RSCThrCrvInt16Param, thrcrv[1], AP_MOTORS_HELI_RSC_THRCRV_25_DEFAULT),
AP_GROUPINFO("025", 3, RSCThrCrvInt16Param, thrcrv[1], AP_MOTORS_HELI_RSC_THRCRV_25_DEFAULT),
// @Param: 50
// @DisplayName: Throttle Servo Position for 50 percent collective
// @Description: Throttle Servo Position for 50 percent collective. This is on a scale from 0 to 1000, where 1000 is full throttle and 0 is zero throttle. Actual PWM values are controlled by SERVOX_MIN and SERVOX_MAX. The 0 percent collective is defined by H_COL_MIN and 100 percent collective is defined by H_COL_MAX.
// @Param: 050
// @DisplayName: Throttle Servo Position in percent for 50 percent collective
// @Description: Throttle Servo Position in percent for 50 percent collective. This is on a scale from 0 to 100, where 100 is full throttle and 0 is zero throttle. Actual PWM values are controlled by SERVOX_MIN and SERVOX_MAX. The 0 percent collective is defined by H_COL_MIN and 100 percent collective is defined by H_COL_MAX.
// @Range: 0 100
// @Increment: 1
// @User: Standard
AP_GROUPINFO("50", 4, RSCThrCrvInt16Param, thrcrv[2], AP_MOTORS_HELI_RSC_THRCRV_50_DEFAULT),
AP_GROUPINFO("050", 4, RSCThrCrvInt16Param, thrcrv[2], AP_MOTORS_HELI_RSC_THRCRV_50_DEFAULT),
// @Param: 75
// @DisplayName: Throttle Servo Position for 75 percent collective
// @Description: Throttle Servo Position for 75 percent collective. This is on a scale from 0 to 1000, where 1000 is full throttle and 0 is zero throttle. Actual PWM values are controlled by SERVOX_MIN and SERVOX_MAX. The 0 percent collective is defined by H_COL_MIN and 100 percent collective is defined by H_COL_MAX.
// @Param: 075
// @DisplayName: Throttle Servo Position in percent for 75 percent collective
// @Description: Throttle Servo Position in percent for 75 percent collective. This is on a scale from 0 to 100, where 100 is full throttle and 0 is zero throttle. Actual PWM values are controlled by SERVOX_MIN and SERVOX_MAX. The 0 percent collective is defined by H_COL_MIN and 100 percent collective is defined by H_COL_MAX.
// @Range: 0 100
// @Increment: 1
// @User: Standard
AP_GROUPINFO("75", 5, RSCThrCrvInt16Param, thrcrv[3], AP_MOTORS_HELI_RSC_THRCRV_75_DEFAULT),
AP_GROUPINFO("075", 5, RSCThrCrvInt16Param, thrcrv[3], AP_MOTORS_HELI_RSC_THRCRV_75_DEFAULT),
// @Param: 100
// @DisplayName: Throttle Servo Position for 100 percent collective
// @Description: Throttle Servo Position for 100 percent collective. This is on a scale from 0 to 1000, where 1000 is full throttle and 0 is zero throttle. Actual PWM values are controlled by SERVOX_MIN and SERVOX_MAX. The 0 percent collective is defined by H_COL_MIN and 100 percent collective is defined by H_COL_MAX.
// @DisplayName: Throttle Servo Position in percent for 100 percent collective
// @Description: Throttle Servo Position in percent for 100 percent collective. This is on a scale from 0 to 100, where 100 is full throttle and 0 is zero throttle. Actual PWM values are controlled by SERVOX_MIN and SERVOX_MAX. The 0 percent collective is defined by H_COL_MIN and 100 percent collective is defined by H_COL_MAX.
// @Range: 0 100
// @Increment: 1
// @User: Standard

View File

@ -5,15 +5,12 @@
#include <RC_Channel/RC_Channel.h>
#include <SRV_Channel/SRV_Channel.h>
// default main rotor speed (ch8 out) as a number from 0 ~ 1000
#define AP_MOTORS_HELI_RSC_SETPOINT 700
// Throttle Curve Defaults
#define AP_MOTORS_HELI_RSC_THRCRV_0_DEFAULT 250
#define AP_MOTORS_HELI_RSC_THRCRV_25_DEFAULT 320
#define AP_MOTORS_HELI_RSC_THRCRV_50_DEFAULT 380
#define AP_MOTORS_HELI_RSC_THRCRV_75_DEFAULT 500
#define AP_MOTORS_HELI_RSC_THRCRV_100_DEFAULT 1000
#define AP_MOTORS_HELI_RSC_THRCRV_0_DEFAULT 25
#define AP_MOTORS_HELI_RSC_THRCRV_25_DEFAULT 32
#define AP_MOTORS_HELI_RSC_THRCRV_50_DEFAULT 38
#define AP_MOTORS_HELI_RSC_THRCRV_75_DEFAULT 50
#define AP_MOTORS_HELI_RSC_THRCRV_100_DEFAULT 100
// RSC governor defaults
#define AP_MOTORS_HELI_RSC_GOVERNOR_SET_DEFAULT 1500
@ -163,7 +160,7 @@ public:
float * get_thrcrv() {
static float throttlecurve[5];
for (uint8_t i = 0; i < 5; i++) {
throttlecurve[i] = (float)thrcrv[i] * 0.001f;
throttlecurve[i] = (float)thrcrv[i] * 0.01f;
}
return throttlecurve;
}