Browse Source

Copter: tradheli-properly upgrade rsc parameters

master
bnsgeyer 6 years ago committed by Randy Mackay
parent
commit
23d4473f88
  1. 45
      ArduCopter/Parameters.cpp

45
ArduCopter/Parameters.cpp

@ -1463,17 +1463,44 @@ void Copter::convert_tradheli_parameters(void) @@ -1463,17 +1463,44 @@ void Copter::convert_tradheli_parameters(void)
}
}
}
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" },
// table of rsc parameters to be converted with scaling
const AP_Param::ConversionInfo rschelipct_conversion_info[] = {
{ Parameters::k_param_motors, 1280, AP_PARAM_INT16, "H_RSC_THRCRV_0" },
{ Parameters::k_param_motors, 1344, AP_PARAM_INT16, "H_RSC_THRCRV_25" },
{ Parameters::k_param_motors, 1408, AP_PARAM_INT16, "H_RSC_THRCRV_50" },
{ Parameters::k_param_motors, 1472, AP_PARAM_INT16, "H_RSC_THRCRV_75" },
{ Parameters::k_param_motors, 1536, AP_PARAM_INT16, "H_RSC_THRCRV_100" },
{ Parameters::k_param_motors, 448, AP_PARAM_INT16, "H_RSC_SETPOINT" },
{ Parameters::k_param_motors, 768, AP_PARAM_INT16, "H_RSC_CRITICAL" },
{ Parameters::k_param_motors, 832, AP_PARAM_INT16, "H_RSC_IDLE" },
};
// convert heli rsc parameters with scaling
uint8_t table_size = ARRAY_SIZE(rschelipct_conversion_info);
for (uint8_t i=0; i<table_size; i++) {
AP_Param::convert_old_parameter(&rschelipct_conversion_info[i], 0.1f);
}
// table of rsc parameters to be converted without scaling
const AP_Param::ConversionInfo rscheli_conversion_info[] = {
{ Parameters::k_param_motors, 512, AP_PARAM_INT8, "H_RSC_MODE" },
{ Parameters::k_param_motors, 640, AP_PARAM_INT8, "H_RSC_RAMP_TIME" },
{ Parameters::k_param_motors, 704, AP_PARAM_INT8, "H_RSC_RUNUP_TIME" },
{ Parameters::k_param_motors, 1216, AP_PARAM_INT16,"H_RSC_SLEWRATE" },
};
// convert dual heli parameters without scaling
uint8_t table_size = ARRAY_SIZE(allheli_conversion_info);
// convert heli rsc parameters without scaling
table_size = ARRAY_SIZE(rscheli_conversion_info);
for (uint8_t i=0; i<table_size; i++) {
AP_Param::convert_old_parameter(&allheli_conversion_info[i], 0.1f);
AP_Param::convert_old_parameter(&rscheli_conversion_info[i], 1.0f);
}
// update tail speed parameter with scaling
AP_Int16 *tailspeed;
enum ap_var_type ptype;
tailspeed = (AP_Int16 *)AP_Param::find("H_TAIL_SPEED", &ptype);
if (tailspeed != nullptr && tailspeed->get() > 100 ) {
uint16_t tailspeed_pct = (uint16_t)(0.1f * tailspeed->get());
AP_Param::set_and_save_by_name("H_TAIL_SPEED", tailspeed_pct );
}
}

Loading…
Cancel
Save