diff --git a/ArduPlane/Log.cpp b/ArduPlane/Log.cpp index bcca30e38a..12067036e5 100644 --- a/ArduPlane/Log.cpp +++ b/ArduPlane/Log.cpp @@ -439,6 +439,17 @@ const struct LogStructure Plane::log_structure[] = { "TSIT", "Qfff", "TimeUS,Ts,Ss,Tmin", "s---", "F---" , true }, #endif +// @LoggerMessage: TILT +// @Description: Tiltrotor tilt values +// @Field: TimeUS: Time since system startup +// @Field: Tilt: Current tilt angle, 0 deg vertical, 90 deg horizontal +// @Field: FL: Front left tilt angle, 0 deg vertical, 90 deg horizontal +// @Field: FR: Front right tilt angle, 0 deg vertical, 90 deg horizontal +#if HAL_QUADPLANE_ENABLED + { LOG_TILT_MSG, sizeof(Tiltrotor::log_tiltrotor), + "TILT", "Qfff", "TimeUS,Tilt,FL,FR", "sddd", "F---" , true }, +#endif + // @LoggerMessage: PIDG // @Description: Plane Proportional/Integral/Derivative gain values for Heading when using COMMAND_INT control. // @Field: TimeUS: Time since system startup diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h index fac041ad66..f4a8f9fc45 100644 --- a/ArduPlane/defines.h +++ b/ArduPlane/defines.h @@ -103,6 +103,7 @@ enum log_messages { LOG_AETR_MSG, LOG_OFG_MSG, LOG_TSIT_MSG, + LOG_TILT_MSG, }; #define MASK_LOG_ATTITUDE_FAST (1<<0) diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 35a4c39d15..36e0b21619 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -3658,6 +3658,9 @@ void QuadPlane::Log_Write_QControl_Tuning() // write multicopter position control message pos_control->write_log(); + + // Write tiltrotor tilt angle log + tiltrotor.write_log(); } #endif diff --git a/ArduPlane/tiltrotor.cpp b/ArduPlane/tiltrotor.cpp index 0be2a07845..4fb17055f9 100644 --- a/ArduPlane/tiltrotor.cpp +++ b/ArduPlane/tiltrotor.cpp @@ -394,6 +394,33 @@ void Tiltrotor::update(void) } } +#if HAL_LOGGING_ENABLED +// Write tiltrotor specific log +void Tiltrotor::write_log() +{ + struct log_tiltrotor pkt { + LOG_PACKET_HEADER_INIT(LOG_TILT_MSG), + time_us : AP_HAL::micros64(), + current_tilt : current_tilt * 90.0, + }; + + if (type != TILT_TYPE_VECTORED_YAW) { + // Left and right tilt are invalid + pkt.front_left_tilt = plane.logger.quiet_nanf(); + pkt.front_right_tilt = plane.logger.quiet_nanf(); + + } else { + // Calculate tilt angle from servo outputs + const float total_angle = 90.0 + tilt_yaw_angle + fixed_angle; + const float scale = total_angle * 0.001; + pkt.front_left_tilt = (SRV_Channels::get_output_scaled(SRV_Channel::k_tiltMotorLeft) * scale) - tilt_yaw_angle; + pkt.front_right_tilt = (SRV_Channels::get_output_scaled(SRV_Channel::k_tiltMotorRight) * scale) - tilt_yaw_angle; + } + + plane.logger.WriteBlock(&pkt, sizeof(pkt)); +} +#endif + /* tilt compensation for angle of tilt. When the rotors are tilted the roll effect of differential thrust on the tilted rotors is decreased diff --git a/ArduPlane/tiltrotor.h b/ArduPlane/tiltrotor.h index 95ddd84181..3add549b71 100644 --- a/ArduPlane/tiltrotor.h +++ b/ArduPlane/tiltrotor.h @@ -16,6 +16,7 @@ #include #include "transition.h" +#include class QuadPlane; class AP_MotorsMulticopter; @@ -69,6 +70,9 @@ public: // always return true if not enabled or not a continuous type bool tilt_angle_achieved() const { return !enabled() || (type != TILT_TYPE_CONTINUOUS) || angle_achieved; } + // Write tiltrotor specific log + void write_log(); + AP_Int8 enable; AP_Int16 tilt_mask; AP_Int16 max_rate_up_dps; @@ -98,6 +102,15 @@ public: private: + // Tiltrotor specific log message + struct PACKED log_tiltrotor { + LOG_PACKET_HEADER; + uint64_t time_us; + float current_tilt; + float front_left_tilt; + float front_right_tilt; + }; + bool setup_complete; // true if a fixed forward motor is setup