diff --git a/ArduPlane/Log.cpp b/ArduPlane/Log.cpp index cb3fd176c2..9804c8d987 100644 --- a/ArduPlane/Log.cpp +++ b/ArduPlane/Log.cpp @@ -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(); diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index dd55f89e07..99b0fcc332 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -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)); } diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index 0a343672fe..29e9c97459 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -188,8 +188,8 @@ private: AP_InertialNav_NavEKF inertial_nav{ahrs}; - AP_Int8 frame_class; - AP_Int8 frame_type; + AP_Enum frame_class; + AP_Enum frame_type; AP_MotorsMulticopter *motors; const struct AP_Param::GroupInfo *motors_var_info;