mirror of https://github.com/ArduPilot/ardupilot
Copter: tradheli-convert swash parameters on firmware upgrade
This commit is contained in:
parent
8c46fe1c61
commit
2fc942ac24
|
@ -768,6 +768,7 @@ private:
|
|||
void load_parameters(void);
|
||||
void convert_pid_parameters(void);
|
||||
void convert_lgr_parameters(void);
|
||||
void convert_tradheli_parameters(void);
|
||||
|
||||
// precision_landing.cpp
|
||||
void init_precland();
|
||||
|
|
|
@ -1300,3 +1300,43 @@ void Copter::convert_lgr_parameters(void)
|
|||
servo_reversed->set_and_save_ifchanged(1);
|
||||
}
|
||||
}
|
||||
|
||||
#if FRAME_CONFIG == HELI_FRAME
|
||||
// handle conversion of tradheli parameters from Copter-3.6 to Copter-3.7
|
||||
void Copter::convert_tradheli_parameters(void)
|
||||
{
|
||||
if (g2.frame_class.get() == AP_Motors::MOTOR_FRAME_HELI) {
|
||||
// single heli conversion info
|
||||
const AP_Param::ConversionInfo singleheli_conversion_info[] = {
|
||||
{ Parameters::k_param_motors, 1, AP_PARAM_INT16, "H_SW_H3_SV1_POS" },
|
||||
{ Parameters::k_param_motors, 2, AP_PARAM_INT16, "H_SW_H3_SV2_POS" },
|
||||
{ Parameters::k_param_motors, 3, AP_PARAM_INT16, "H_SW_H3_SV3_POS" },
|
||||
{ Parameters::k_param_motors, 7, AP_PARAM_INT16, "H_SW_H3_PHANG" },
|
||||
};
|
||||
|
||||
// convert single heli paramters without scaling
|
||||
uint8_t table_size = ARRAY_SIZE(singleheli_conversion_info);
|
||||
for (uint8_t i=0; i<table_size; i++) {
|
||||
AP_Param::convert_old_parameter(&singleheli_conversion_info[i], 1.0f);
|
||||
}
|
||||
} else if (g2.frame_class.get() == AP_Motors::MOTOR_FRAME_HELI_DUAL) {
|
||||
// dual heli conversion info
|
||||
const AP_Param::ConversionInfo dualheli_conversion_info[] = {
|
||||
{ Parameters::k_param_motors, 1, AP_PARAM_INT16, "H_SW1_H3_SV1_POS" },
|
||||
{ Parameters::k_param_motors, 2, AP_PARAM_INT16, "H_SW1_H3_SV2_POS" },
|
||||
{ Parameters::k_param_motors, 3, AP_PARAM_INT16, "H_SW1_H3_SV3_POS" },
|
||||
{ Parameters::k_param_motors, 4, AP_PARAM_INT16, "H_SW2_H3_SV1_POS" },
|
||||
{ Parameters::k_param_motors, 5, AP_PARAM_INT16, "H_SW2_H3_SV2_POS" },
|
||||
{ Parameters::k_param_motors, 6, AP_PARAM_INT16, "H_SW2_H3_SV3_POS" },
|
||||
{ Parameters::k_param_motors, 7, AP_PARAM_INT16, "H_SW1_H3_PHANG" },
|
||||
{ Parameters::k_param_motors, 8, AP_PARAM_INT16, "H_SW2_H3_PHANG" },
|
||||
};
|
||||
|
||||
// convert dual heli parameters without scaling
|
||||
uint8_t table_size = ARRAY_SIZE(dualheli_conversion_info);
|
||||
for (uint8_t i=0; i<table_size; i++) {
|
||||
AP_Param::convert_old_parameter(&dualheli_conversion_info[i], 1.0f);
|
||||
}
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
|
|
@ -639,6 +639,9 @@ void Copter::allocate_motors(void)
|
|||
|
||||
// upgrade parameters. This must be done after allocating the objects
|
||||
convert_pid_parameters();
|
||||
#if FRAME_CONFIG == HELI_FRAME
|
||||
convert_tradheli_parameters();
|
||||
#endif
|
||||
}
|
||||
|
||||
bool Copter::is_tradheli() const
|
||||
|
|
Loading…
Reference in New Issue