diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index dc121e07ae..febde6db11 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -963,6 +963,10 @@ void QuadPlane::motors_output(void) if (motors->armed()) { plane.DataFlash.Log_Write_Rate(plane.ahrs, *motors, *attitude_control, *pos_control); Log_Write_QControl_Tuning(); + uint32_t now = AP_HAL::millis(); + if (now - last_ctrl_log_ms > 100) { + attitude_control->control_monitor_log(); + } } } diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index 37b33067d8..5afa3bcb42 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -299,6 +299,9 @@ private: uint8_t motor_count; // number of motors to cycle } motor_test; + // time of last control log message + uint32_t last_ctrl_log_ms; + // tiltrotor control variables struct { AP_Int16 tilt_mask; diff --git a/ArduPlane/tuning.cpp b/ArduPlane/tuning.cpp index 80d8ea0f27..a53c845572 100644 --- a/ArduPlane/tuning.cpp +++ b/ArduPlane/tuning.cpp @@ -264,3 +264,33 @@ void AP_Tuning_Plane::reload_value(uint8_t parm) break; } } + +/* + return current controller error + */ +float AP_Tuning_Plane::controller_error(uint8_t parm) +{ + switch(parm) { + // special handling of dual-parameters + case TUNING_RATE_ROLL_PI: + case TUNING_RATE_ROLL_P: + case TUNING_RATE_ROLL_I: + case TUNING_RATE_ROLL_D: + case TUNING_RATE_PITCH_PI: + case TUNING_RATE_PITCH_P: + case TUNING_RATE_PITCH_I: + case TUNING_RATE_PITCH_D: + case TUNING_RATE_YAW_PI: + case TUNING_RATE_YAW_P: + case TUNING_RATE_YAW_I: + case TUNING_RATE_YAW_D: + if (!plane.quadplane.available()) { + return 0; + } + return plane.quadplane.attitude_control->control_monitor_rms_output(); + + default: + // no special handler + return 0; + } +} diff --git a/ArduPlane/tuning.h b/ArduPlane/tuning.h index e1178550da..dd457d977a 100644 --- a/ArduPlane/tuning.h +++ b/ArduPlane/tuning.h @@ -83,10 +83,11 @@ private: TUNING_SET_AZ = 7, }; - AP_Float *get_param_pointer(uint8_t parm); - void save_value(uint8_t parm); - void reload_value(uint8_t parm); - void set_value(uint8_t parm, float value); + AP_Float *get_param_pointer(uint8_t parm) override; + void save_value(uint8_t parm) override; + void reload_value(uint8_t parm) override; + void set_value(uint8_t parm, float value) override; + float controller_error(uint8_t parm) override; // tuning set arrays static const uint8_t tuning_set_rate_roll_pitch[];