Copter: set Heli frame default at compile-time

This commit is contained in:
Peter Barker 2019-07-03 15:13:03 +10:00 committed by Randy Mackay
parent 7f3cb09da2
commit cf45108efb
3 changed files with 8 additions and 15 deletions

View File

@ -844,7 +844,6 @@ private:
bool optflow_position_ok();
void update_auto_armed();
bool should_log(uint32_t mask);
void set_default_frame_class();
MAV_TYPE get_frame_mav_type();
const char* get_frame_string();
void allocate_motors(void);

View File

@ -28,6 +28,13 @@
#define GOBJECTVARPTR(v, name, var_info_ptr) { AP_PARAM_GROUP, name, Parameters::k_param_ ## v, (const void *)&copter.v, {group_info_ptr : var_info_ptr}, AP_PARAM_FLAG_POINTER | AP_PARAM_FLAG_INFO_POINTER }
#define GOBJECTN(v, pname, name, class) { AP_PARAM_GROUP, name, Parameters::k_param_ ## pname, (const void *)&copter.v, {group_info : class::var_info} }
#if FRAME_CONFIG == HELI_FRAME
// 6 here is AP_Motors::MOTOR_FRAME_HELI
#define DEFAULT_FRAME_CLASS 6
#else
#define DEFAULT_FRAME_CLASS 0
#endif
const AP_Param::Info Copter::var_info[] = {
// @Param: SYSID_SW_MREV
// @DisplayName: Eeprom format version number
@ -819,7 +826,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
// @Values: 0:Undefined, 1:Quad, 2:Hexa, 3:Octa, 4:OctaQuad, 5:Y6, 6:Heli, 7:Tri, 8:SingleCopter, 9:CoaxCopter, 10:BiCopter, 11:Heli_Dual, 12:DodecaHexa, 13:HeliQuad
// @User: Standard
// @RebootRequired: True
AP_GROUPINFO("FRAME_CLASS", 15, ParametersG2, frame_class, 0),
AP_GROUPINFO("FRAME_CLASS", 15, ParametersG2, frame_class, DEFAULT_FRAME_CLASS),
// @Group: SERVO
// @Path: ../libraries/SRV_Channel/SRV_Channels.cpp

View File

@ -108,9 +108,6 @@ void Copter::init_ardupilot()
init_rc_in(); // sets up rc channels from radio
// default frame class to match firmware if possible
set_default_frame_class();
// allocate the motors class
allocate_motors();
@ -395,16 +392,6 @@ bool Copter::should_log(uint32_t mask)
#endif
}
// default frame_class to match firmware if possible
void Copter::set_default_frame_class()
{
if (FRAME_CONFIG == HELI_FRAME &&
g2.frame_class.get() != AP_Motors::MOTOR_FRAME_HELI_DUAL &&
g2.frame_class.get() != AP_Motors::MOTOR_FRAME_HELI_QUAD) {
g2.frame_class.set(AP_Motors::MOTOR_FRAME_HELI);
}
}
// return MAV_TYPE corresponding to frame class
MAV_TYPE Copter::get_frame_mav_type()
{