Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Copter: set Heli frame default at compile-time #11707

Merged
merged 2 commits into from
Jul 4, 2019
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
14 changes: 14 additions & 0 deletions ArduCopter/AP_Arming.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -172,6 +172,13 @@ bool AP_Arming_Copter::parameter_checks(bool display_failure)
#endif

#if FRAME_CONFIG == HELI_FRAME
if (copter.g2.frame_class.get() != AP_Motors::MOTOR_FRAME_HELI_QUAD &&
copter.g2.frame_class.get() != AP_Motors::MOTOR_FRAME_HELI_DUAL &&
copter.g2.frame_class.get() != AP_Motors::MOTOR_FRAME_HELI) {
check_failed(ARMING_CHECK_PARAMETERS, display_failure, "Invalid Heli FRAME_CLASS");
return false;
}

// check helicopter parameters
if (!copter.motors->parameter_check(display_failure)) {
check_failed(ARMING_CHECK_PARAMETERS, display_failure, "Heli motors checks failed");
Expand All @@ -189,6 +196,13 @@ bool AP_Arming_Copter::parameter_checks(bool display_failure)
return false;
}

#else
if (copter.g2.frame_class.get() == AP_Motors::MOTOR_FRAME_HELI_QUAD ||
copter.g2.frame_class.get() == AP_Motors::MOTOR_FRAME_HELI_DUAL ||
copter.g2.frame_class.get() == AP_Motors::MOTOR_FRAME_HELI) {
check_failed(ARMING_CHECK_PARAMETERS, display_failure, "Invalid MultiCopter FRAME_CLASS");
return false;
}
#endif // HELI_FRAME

// check for missing terrain data
Expand Down
1 change: 0 additions & 1 deletion ArduCopter/Copter.h
Original file line number Diff line number Diff line change
Expand Up @@ -844,7 +844,6 @@ class Copter : public AP_HAL::HAL::Callbacks {
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);
Expand Down
9 changes: 8 additions & 1 deletion ArduCopter/Parameters.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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
Expand Down
13 changes: 0 additions & 13 deletions ArduCopter/system.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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();

Expand Down Expand Up @@ -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()
{
Expand Down