Copter: move get_frame_mav_type to motors

This commit is contained in:
Iampete1 2021-01-19 17:00:53 +00:00 committed by Randy Mackay
parent 5f4b5c4b94
commit 0a23084d43
3 changed files with 4 additions and 34 deletions

View File

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

View File

@ -14,7 +14,10 @@
MAV_TYPE GCS_Copter::frame_type() const
{
return copter.get_frame_mav_type();
if (copter.motors == nullptr) {
return MAV_TYPE_GENERIC;
}
return copter.motors->get_frame_mav_type();
}
MAV_MODE GCS_MAVLINK_Copter::base_mode() const

View File

@ -448,38 +448,6 @@ bool Copter::should_log(uint32_t mask)
#endif
}
// return MAV_TYPE corresponding to frame class
MAV_TYPE Copter::get_frame_mav_type()
{
switch ((AP_Motors::motor_frame_class)g2.frame_class.get()) {
case AP_Motors::MOTOR_FRAME_QUAD:
case AP_Motors::MOTOR_FRAME_UNDEFINED:
return MAV_TYPE_QUADROTOR;
case AP_Motors::MOTOR_FRAME_HEXA:
case AP_Motors::MOTOR_FRAME_Y6:
return MAV_TYPE_HEXAROTOR;
case AP_Motors::MOTOR_FRAME_OCTA:
case AP_Motors::MOTOR_FRAME_OCTAQUAD:
return MAV_TYPE_OCTOROTOR;
case AP_Motors::MOTOR_FRAME_HELI:
case AP_Motors::MOTOR_FRAME_HELI_DUAL:
case AP_Motors::MOTOR_FRAME_HELI_QUAD:
return MAV_TYPE_HELICOPTER;
case AP_Motors::MOTOR_FRAME_TRI:
return MAV_TYPE_TRICOPTER;
case AP_Motors::MOTOR_FRAME_SINGLE:
case AP_Motors::MOTOR_FRAME_COAX:
case AP_Motors::MOTOR_FRAME_TAILSITTER:
return MAV_TYPE_COAXIAL;
case AP_Motors::MOTOR_FRAME_DODECAHEXA:
return MAV_TYPE_DODECAROTOR;
case AP_Motors::MOTOR_FRAME_DECA:
return MAV_TYPE_DECAROTOR;
}
// unknown frame so return generic
return MAV_TYPE_GENERIC;
}
// return string corresponding to frame_class
const char* Copter::get_frame_string()
{