Plane: use get_frame_class/type string methods

use AP_Enum<> for frame_class/type
remove invalid copterTS check
improve quadplane setup failure message

Co-authored-by: Peter Barker <pb-gh@barker.dropbear.id.au>
This commit is contained in:
Mark Whitehorn 2020-11-06 07:09:23 -07:00 committed by Peter Barker
parent f405454aba
commit 236961f17f
3 changed files with 16 additions and 17 deletions

View File

@ -506,6 +506,10 @@ void Plane::Log_Write_Vehicle_Startup_Messages()
{
// only 200(?) bytes are guaranteed by AP_Logger
Log_Write_Startup(TYPE_GROUNDSTART_MSG);
if (quadplane.initialised) {
logger.Write_MessageF("QuadPlane Frame: %s/%s", quadplane.motors->get_frame_string(),
quadplane.motors->get_type_string());
}
logger.Write_Mode(control_mode->mode_number(), control_mode_reason);
ahrs.Log_Write_Home_And_Origin();
gps.Write_AP_Logger_Log_Startup_messages();

View File

@ -624,7 +624,6 @@ bool QuadPlane::setup(void)
}
float loop_delta_t = 1.0 / plane.scheduler.get_loop_rate_hz();
enum AP_Motors::motor_frame_class motor_class;
enum Rotation rotation = ROTATION_NONE;
/*
@ -654,8 +653,6 @@ bool QuadPlane::setup(void)
}
frame_class.set_and_save(new_value);
}
gcs().send_text(MAV_SEVERITY_INFO, "QuadPlane initialise, class: %u, type: %u",
(unsigned)frame_class.get(), (unsigned)frame_type.get());
if (hal.util->available_memory() <
4096 + sizeof(*motors) + sizeof(*attitude_control) + sizeof(*pos_control) + sizeof(*wp_nav) + sizeof(*ahrs_view) + sizeof(*loiter_nav)) {
@ -667,8 +664,7 @@ bool QuadPlane::setup(void)
that the objects don't affect the vehicle unless enabled and
also saves memory when not in use
*/
motor_class = (enum AP_Motors::motor_frame_class)frame_class.get();
switch (motor_class) {
switch ((AP_Motors::motor_frame_class)frame_class) {
case AP_Motors::MOTOR_FRAME_QUAD:
setup_default_channels(4);
break;
@ -692,12 +688,13 @@ bool QuadPlane::setup(void)
case AP_Motors::MOTOR_FRAME_TAILSITTER:
break;
default:
AP_BoardConfig::config_error("Unknown Q_FRAME_CLASS %u", (unsigned)frame_class.get());
AP_BoardConfig::config_error("Unsupported Q_FRAME_CLASS %u", frame_class);
}
::printf("QuadPlane initialise, class: %u, type: %u\n", frame_class.get(), frame_type.get());
if (tailsitter.motor_mask == 0) {
// this is a normal quadplane
switch (motor_class) {
switch ((AP_Motors::motor_frame_class)frame_class) {
case AP_Motors::MOTOR_FRAME_TRI:
motors = new AP_MotorsTri(plane.scheduler.get_loop_rate_hz(), rc_speed);
motors_var_info = AP_MotorsTri::var_info;
@ -719,7 +716,7 @@ bool QuadPlane::setup(void)
// this is a copter tailsitter with motor layout specified by frame_class and frame_type
// tilting motors are not supported (tiltrotor control variables are ignored)
if (tilt.tilt_mask != 0) {
gcs().send_text(MAV_SEVERITY_INFO, "Warning: Motor tilt not supported");
::printf("Warning: Motor tilt not supported\n");
}
rotation = ROTATION_PITCH_90;
motors = new AP_MotorsMatrix(plane.scheduler.get_loop_rate_hz(), rc_speed);
@ -761,10 +758,10 @@ bool QuadPlane::setup(void)
}
AP_Param::load_object_from_eeprom(loiter_nav, loiter_nav->var_info);
motors->init((AP_Motors::motor_frame_class)frame_class.get(), (AP_Motors::motor_frame_type)frame_type.get());
motors->init(frame_class, frame_type);
if (!motors->initialised_ok()) {
AP_BoardConfig::config_error("unknown Q_FRAME_TYPE %u", (unsigned)frame_type.get());
AP_BoardConfig::config_error("init failed: Q_FRAME_CLASS=%u Q_FRAME_TYPE=%u", frame_class, frame_type);
}
tilt.is_vectored = tilt.tilt_mask != 0 && tilt.tilt_type == TILT_TYPE_VECTORED_YAW;
@ -783,7 +780,7 @@ bool QuadPlane::setup(void)
// TODO: update this if servo function assignments change
// used by relax_attitude_control() to control special behavior for vectored tailsitters
_is_vectored = (motor_class == AP_Motors::MOTOR_FRAME_TAILSITTER) &&
_is_vectored = (frame_class == AP_Motors::MOTOR_FRAME_TAILSITTER) &&
(!is_zero(tailsitter.vectored_hover_gain) &&
(SRV_Channels::function_assigned(SRV_Channel::k_tiltMotorLeft) ||
SRV_Channels::function_assigned(SRV_Channel::k_tiltMotorRight)));
@ -822,7 +819,7 @@ bool QuadPlane::setup(void)
// param count will have changed
AP_Param::invalidate_count();
gcs().send_text(MAV_SEVERITY_INFO, "QuadPlane initialised");
::printf("QuadPlane initialised, class: %s, type: %s\n", motors->get_frame_string(), motors->get_type_string());
initialised = true;
return true;
}
@ -834,9 +831,7 @@ void QuadPlane::setup_defaults(void)
{
AP_Param::set_defaults_from_table(defaults_table, ARRAY_SIZE(defaults_table));
enum AP_Motors::motor_frame_class motor_class;
motor_class = (enum AP_Motors::motor_frame_class)frame_class.get();
if (motor_class == AP_Motors::MOTOR_FRAME_TAILSITTER) {
if (frame_class == AP_Motors::MOTOR_FRAME_TAILSITTER) {
AP_Param::set_defaults_from_table(defaults_table_tailsitter, ARRAY_SIZE(defaults_table_tailsitter));
}

View File

@ -188,8 +188,8 @@ private:
AP_InertialNav_NavEKF inertial_nav{ahrs};
AP_Int8 frame_class;
AP_Int8 frame_type;
AP_Enum<AP_Motors::motor_frame_class> frame_class;
AP_Enum<AP_Motors::motor_frame_type> frame_type;
AP_MotorsMulticopter *motors;
const struct AP_Param::GroupInfo *motors_var_info;