From 1a23f2d23532d71ddd52213367aee4c79cd83d5f Mon Sep 17 00:00:00 2001 From: bnsgeyer Date: Tue, 28 Jan 2025 22:26:49 -0500 Subject: [PATCH] Plane: add system ID to quadplane enable as an AUX switch --- ArduPlane/Log.cpp | 100 ++++++++++++++ ArduPlane/Parameters.cpp | 8 +- ArduPlane/Parameters.h | 4 + ArduPlane/Plane.h | 6 + ArduPlane/RC_Channel_Plane.cpp | 13 ++ ArduPlane/defines.h | 2 + ArduPlane/mode.cpp | 4 + ArduPlane/mode.h | 21 +++ ArduPlane/mode_qloiter.cpp | 14 +- ArduPlane/quadplane.cpp | 24 +++- ArduPlane/quadplane.h | 1 + ArduPlane/systemid.cpp | 243 +++++++++++++++++++++++++++++++++ ArduPlane/systemid.h | 85 ++++++++++++ 13 files changed, 515 insertions(+), 10 deletions(-) create mode 100644 ArduPlane/systemid.cpp create mode 100644 ArduPlane/systemid.h diff --git a/ArduPlane/Log.cpp b/ArduPlane/Log.cpp index 184dfc611a66ec..31971e6b53db9c 100644 --- a/ArduPlane/Log.cpp +++ b/ArduPlane/Log.cpp @@ -72,6 +72,71 @@ void Plane::Log_Write_FullRate(void) #endif } +#if HAL_QUADPLANE_ENABLED +struct PACKED log_SysIdD { + LOG_PACKET_HEADER; + uint64_t time_us; + float waveform_time; + float waveform_sample; + float waveform_freq; + float angle_x; + float angle_y; + float angle_z; + float accel_x; + float accel_y; + float accel_z; +}; + +// Write an rate packet +void Plane::Log_Write_SysID_Data(float waveform_time, float waveform_sample, float waveform_freq, float angle_x, float angle_y, float angle_z, float accel_x, float accel_y, float accel_z) +{ + struct log_SysIdD pkt_sidd = { + LOG_PACKET_HEADER_INIT(LOG_SYSIDD_MSG), + time_us : AP_HAL::micros64(), + waveform_time : waveform_time, + waveform_sample : waveform_sample, + waveform_freq : waveform_freq, + angle_x : angle_x, + angle_y : angle_y, + angle_z : angle_z, + accel_x : accel_x, + accel_y : accel_y, + accel_z : accel_z + }; + logger.WriteBlock(&pkt_sidd, sizeof(pkt_sidd)); +} + +struct PACKED log_SysIdS { + LOG_PACKET_HEADER; + uint64_t time_us; + uint8_t systemID_axis; + float waveform_magnitude; + float frequency_start; + float frequency_stop; + float time_fade_in; + float time_const_freq; + float time_record; + float time_fade_out; +}; + +// Write an rate packet +void Plane::Log_Write_SysID_Setup(uint8_t systemID_axis, float waveform_magnitude, float frequency_start, float frequency_stop, float time_fade_in, float time_const_freq, float time_record, float time_fade_out) +{ + struct log_SysIdS pkt_sids = { + LOG_PACKET_HEADER_INIT(LOG_SYSIDS_MSG), + time_us : AP_HAL::micros64(), + systemID_axis : systemID_axis, + waveform_magnitude : waveform_magnitude, + frequency_start : frequency_start, + frequency_stop : frequency_stop, + time_fade_in : time_fade_in, + time_const_freq : time_const_freq, + time_record : time_record, + time_fade_out : time_fade_out + }; + logger.WriteBlock(&pkt_sids, sizeof(pkt_sids)); +} +#endif struct PACKED log_Control_Tuning { LOG_PACKET_HEADER; @@ -506,6 +571,41 @@ const struct LogStructure Plane::log_structure[] = { { LOG_OFG_MSG, sizeof(log_OFG_Guided), "OFG", "QffffBffB", "TimeUS,Arsp,ArspA,Alt,AltA,AltF,Hdg,HdgA,AltL", "snnmo-d--", "F--------" , true }, #endif + +// @LoggerMessage: SIDD +// @Description: System ID data +// @Field: TimeUS: Time since system startup +// @Field: Time: Time reference for waveform +// @Field: Targ: Current waveform sample +// @Field: F: Instantaneous waveform frequency +// @Field: Gx: Delta angle, X-Axis +// @Field: Gy: Delta angle, Y-Axis +// @Field: Gz: Delta angle, Z-Axis +// @Field: Ax: Delta velocity, X-Axis +// @Field: Ay: Delta velocity, Y-Axis +// @Field: Az: Delta velocity, Z-Axis + +#if HAL_QUADPLANE_ENABLED + { LOG_SYSIDD_MSG, sizeof(log_SysIdD), + "SIDD", "Qfffffffff", "TimeUS,Time,Targ,F,Gx,Gy,Gz,Ax,Ay,Az", "ss-zkkkooo", "F---------" , true }, +#endif + +// @LoggerMessage: SIDS +// @Description: System ID settings +// @Field: TimeUS: Time since system startup +// @Field: Ax: The axis which is being excited +// @Field: Mag: Magnitude of the chirp waveform +// @Field: FSt: Frequency at the start of chirp +// @Field: FSp: Frequency at the end of chirp +// @Field: TFin: Time to reach maximum amplitude of chirp +// @Field: TC: Time at constant frequency before chirp starts +// @Field: TR: Time taken to complete chirp waveform +// @Field: TFout: Time to reach zero amplitude after chirp finishes + +#if HAL_QUADPLANE_ENABLED + { LOG_SYSIDS_MSG, sizeof(log_SysIdS), + "SIDS", "QBfffffff", "TimeUS,Ax,Mag,FSt,FSp,TFin,TC,TR,TFout", "s--ssssss", "F--------" , true }, +#endif }; uint8_t Plane::get_num_log_structures() const diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index fb425e6dd6ffab..3c66fd1223fb01 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -1049,7 +1049,7 @@ const AP_Param::Info Plane::var_info[] = { // @Path: ../libraries/AP_Quicktune/AP_Quicktune.cpp GOBJECT(quicktune, "QWIK_", AP_Quicktune), #endif - + AP_VAREND }; @@ -1309,6 +1309,12 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { // @User: Standard AP_GROUPINFO("RNGFND_LND_ORNT", 36, ParametersG2, rangefinder_land_orient, ROTATION_PITCH_270), #endif + +#if AP_PLANE_SYSTEMID_ENABLED + // @Group: SID + // @Path: systemid.cpp + AP_SUBGROUPINFO(systemid, "SID", 37, ParametersG2, AP_SystemID), +#endif AP_GROUPEND }; diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index 9945869250b4bd..c0432ecf6f5430 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -590,6 +590,10 @@ class ParametersG2 { // orientation of rangefinder to use for landing AP_Int8 rangefinder_land_orient; #endif + +#if AP_PLANE_SYSTEMID_ENABLED + AP_SystemID systemid; +#endif }; extern const AP_Param::Info var_info[]; diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 86783bed8c1517..ee162e5c58ecdc 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -122,6 +122,7 @@ #endif #include "AP_Arming_Plane.h" #include "pullup.h" +#include "systemid.h" /* main APM:Plane class @@ -181,6 +182,9 @@ class Plane : public AP_Vehicle { #if AP_PLANE_GLIDER_PULLUP_ENABLED friend class GliderPullup; #endif +#if AP_PLANE_SYSTEMID_ENABLED + friend class AP_SystemID; +#endif Plane(void); @@ -946,6 +950,8 @@ class Plane : public AP_Vehicle { // Log.cpp void Log_Write_FullRate(void); void Log_Write_Attitude(void); + void Log_Write_SysID_Setup(uint8_t systemID_axis, float waveform_magnitude, float frequency_start, float frequency_stop, float time_fade_in, float time_const_freq, float time_record, float time_fade_out); + void Log_Write_SysID_Data(float waveform_time, float waveform_sample, float waveform_freq, float angle_x, float angle_y, float angle_z, float accel_x, float accel_y, float accel_z); void Log_Write_Control_Tuning(); void Log_Write_OFG_Guided(); void Log_Write_Guided(void); diff --git a/ArduPlane/RC_Channel_Plane.cpp b/ArduPlane/RC_Channel_Plane.cpp index 67450cafa1fda4..d1428bcb21dfe3 100644 --- a/ArduPlane/RC_Channel_Plane.cpp +++ b/ArduPlane/RC_Channel_Plane.cpp @@ -177,6 +177,9 @@ void RC_Channel_Plane::init_aux_function(const RC_Channel::AUX_FUNC ch_option, #endif #if AP_QUICKTUNE_ENABLED case AUX_FUNC::QUICKTUNE: +#endif +#if AP_PLANE_SYSTEMID_ENABLED + case AUX_FUNC::SYSTEMID: #endif break; @@ -468,6 +471,16 @@ bool RC_Channel_Plane::do_aux_function(const AuxFuncTrigger &trigger) break; #endif +#if AP_PLANE_SYSTEMID_ENABLED + case AUX_FUNC::SYSTEMID: + if (ch_flag == AuxSwitchPos::HIGH) { + plane.g2.systemid.start(); + } else if (ch_flag == AuxSwitchPos::LOW) { + plane.g2.systemid.stop(); + } + break; +#endif + #if AP_ICENGINE_ENABLED case AUX_FUNC::ICE_START_STOP: plane.g2.ice_control.do_aux_function(trigger); diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h index b0941e85badc57..c89a69ac480903 100644 --- a/ArduPlane/defines.h +++ b/ArduPlane/defines.h @@ -95,6 +95,8 @@ enum log_messages { LOG_OFG_MSG, LOG_TSIT_MSG, LOG_TILT_MSG, + LOG_SYSIDS_MSG, + LOG_SYSIDD_MSG, }; #define MASK_LOG_ATTITUDE_FAST (1<<0) diff --git a/ArduPlane/mode.cpp b/ArduPlane/mode.cpp index 7be469079d7cc2..3b6ad1fe34cc88 100644 --- a/ArduPlane/mode.cpp +++ b/ArduPlane/mode.cpp @@ -101,6 +101,10 @@ bool Mode::enter() plane.target_altitude.terrain_following_pending = false; #endif +#if AP_PLANE_SYSTEMID_ENABLED + plane.g2.systemid.stop(); +#endif + // disable auto mode servo idle during altitude wait command plane.auto_state.idle_mode = false; diff --git a/ArduPlane/mode.h b/ArduPlane/mode.h index c5cf09fb4988a2..c6eca15a88b188 100644 --- a/ArduPlane/mode.h +++ b/ArduPlane/mode.h @@ -10,6 +10,7 @@ #include #include #include "pullup.h" +#include "systemid.h" #ifndef AP_QUICKTUNE_ENABLED #define AP_QUICKTUNE_ENABLED HAL_QUADPLANE_ENABLED @@ -167,6 +168,11 @@ class Mode virtual bool supports_quicktune() const { return false; } #endif +#if AP_PLANE_SYSTEMID_ENABLED + // does this mode support systemid? + virtual bool supports_systemid() const { return false; } +#endif + protected: // subclasses override this to perform checks before entering the mode @@ -697,6 +703,11 @@ class ModeQStabilize : public Mode void run() override; +#if AP_PLANE_SYSTEMID_ENABLED + // does this mode support systemid? + bool supports_systemid() const override { return true; } +#endif + protected: private: @@ -721,6 +732,11 @@ class ModeQHover : public Mode void run() override; +#if AP_PLANE_SYSTEMID_ENABLED + // does this mode support systemid? + bool supports_systemid() const override { return true; } +#endif + protected: bool _enter() override; @@ -749,6 +765,11 @@ friend class Plane; void run() override; +#if AP_PLANE_SYSTEMID_ENABLED + // does this mode support systemid? + bool supports_systemid() const override { return true; } +#endif + protected: bool _enter() override; diff --git a/ArduPlane/mode_qloiter.cpp b/ArduPlane/mode_qloiter.cpp index 6ab2c1bd3d81db..9a8c79cd449f2b 100644 --- a/ArduPlane/mode_qloiter.cpp +++ b/ArduPlane/mode_qloiter.cpp @@ -126,10 +126,18 @@ void ModeQLoiter::run() // Pilot input, use yaw rate time constant quadplane.set_pilot_yaw_rate_time_constant(); + Vector3f target { plane.nav_roll_cd*0.01, plane.nav_pitch_cd*0.01, quadplane.get_desired_yaw_rate_cds() * 0.01 }; + +#if AP_PLANE_SYSTEMID_ENABLED + auto &systemid = plane.g2.systemid; + systemid.update(); + target += systemid.get_attitude_offset_deg(); +#endif + // call attitude controller with conservative smoothing gain of 4.0f - attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(plane.nav_roll_cd, - plane.nav_pitch_cd, - quadplane.get_desired_yaw_rate_cds()); + attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target.x*100, + target.y*100, + target.z*100); if (plane.control_mode == &plane.mode_qland) { if (poscontrol.get_state() < QuadPlane::QPOS_LAND_FINAL && quadplane.check_land_final()) { diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index eaf767fdfb1c5b..7cb45d8ee26143 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -936,16 +936,25 @@ void QuadPlane::multicopter_attitude_rate_update(float yaw_rate_cds) } } + // note this is actually in deg/s for some SID_AXIS values for yaw + Vector3f offset_deg; + +#if AP_PLANE_SYSTEMID_ENABLED + auto &systemid = plane.g2.systemid; + systemid.update(); + offset_deg = systemid.get_attitude_offset_deg(); +#endif + if (use_yaw_target) { - attitude_control->input_euler_angle_roll_pitch_yaw(plane.nav_roll_cd, - plane.nav_pitch_cd, - yaw_target_cd, + attitude_control->input_euler_angle_roll_pitch_yaw(plane.nav_roll_cd + offset_deg.x*100, + plane.nav_pitch_cd + offset_deg.y*100, + yaw_target_cd + offset_deg.z*100, true); } else { // use euler angle attitude control - attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(plane.nav_roll_cd, - plane.nav_pitch_cd, - yaw_rate_cds); + attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(plane.nav_roll_cd + offset_deg.x*100, + plane.nav_pitch_cd + offset_deg.y*100, + yaw_rate_cds + offset_deg.z*100); } } else { // use the fixed wing desired rates @@ -980,6 +989,9 @@ void QuadPlane::hold_stabilize(float throttle_in) // tailsitters in forward flight should not use angle boost should_boost = false; } +#if AP_PLANE_SYSTEMID_ENABLED + throttle_in += plane.g2.systemid.get_throttle_offset(); +#endif attitude_control->set_throttle_out(throttle_in, should_boost, 0); } } diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index 47d9154f938e00..bff76753ca8461 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -63,6 +63,7 @@ class QuadPlane friend class ModeQAcro; friend class ModeLoiterAltQLand; friend class ModeAutoLand; + friend class AP_SystemID; QuadPlane(AP_AHRS &_ahrs); diff --git a/ArduPlane/systemid.cpp b/ArduPlane/systemid.cpp new file mode 100644 index 00000000000000..5f953baa912061 --- /dev/null +++ b/ArduPlane/systemid.cpp @@ -0,0 +1,243 @@ +#include "systemid.h" + +#if AP_PLANE_SYSTEMID_ENABLED + +#include +#include "Plane.h" + +/* + handle systemid via an auxillary switch + */ + +const AP_Param::GroupInfo AP_SystemID::var_info[] = { + + // @Param: _AXIS + // @DisplayName: System identification axis + // @Description: Controls which axis are being excited. Set to non-zero to see more parameters + // @User: Standard + // @Values: 0:None, 1:Input Roll Angle, 2:Input Pitch Angle, 3:Input Yaw Angle, 4:Recovery Roll Angle, 5:Recovery Pitch Angle, 6:Recovery Yaw Angle, 7:Rate Roll, 8:Rate Pitch, 9:Rate Yaw, 10:Mixer Roll, 11:Mixer Pitch, 12:Mixer Yaw, 13:Mixer Thrust, 14:Measured Lateral Position, 15:Measured Longitudinal Position, 16:Measured Lateral Velocity, 17:Measured Longitudinal Velocity, 18:Input Lateral Velocity, 19:Input Longitudinal Velocity + AP_GROUPINFO_FLAGS("_AXIS", 1, AP_SystemID, axis, 0, AP_PARAM_FLAG_ENABLE), + + // @Param: _MAGNITUDE + // @DisplayName: System identification Chirp Magnitude + // @Description: Magnitude of sweep in deg, deg/s and 0-1 for mixer outputs. + // @User: Standard + AP_GROUPINFO("_MAGNITUDE", 2, AP_SystemID, waveform_magnitude, 5), + + // @Param: _F_START_HZ + // @DisplayName: System identification Start Frequency + // @Description: Frequency at the start of the sweep + // @Range: 0.01 100 + // @Units: Hz + // @User: Standard + AP_GROUPINFO("_F_START_HZ", 3, AP_SystemID, frequency_start, 0.5f), + + // @Param: _F_STOP_HZ + // @DisplayName: System identification Stop Frequency + // @Description: Frequency at the end of the sweep + // @Range: 0.01 100 + // @Units: Hz + // @User: Standard + AP_GROUPINFO("_F_STOP_HZ", 4, AP_SystemID, frequency_stop, 15), + + // @Param: _T_FADE_IN + // @DisplayName: System identification Fade in time + // @Description: Time to reach maximum amplitude of sweep + // @Range: 0 20 + // @Units: s + // @User: Standard + AP_GROUPINFO("_T_FADE_IN", 5, AP_SystemID, time_fade_in, 5), + + // @Param: _T_REC + // @DisplayName: System identification Total Sweep length + // @Description: Time taken to complete the sweep + // @Range: 0 255 + // @Units: s + // @User: Standard + AP_GROUPINFO("_T_REC", 6, AP_SystemID, time_record, 70), + + // @Param: _T_FADE_OUT + // @DisplayName: System identification Fade out time + // @Description: Time to reach zero amplitude at the end of the sweep + // @Range: 0 5 + // @Units: s + // @User: Standard + AP_GROUPINFO("_T_FADE_OUT", 7, AP_SystemID, time_fade_out, 1), + + // @Param: _XY_CTRL_MUL + // @DisplayName: System identification XY control multiplier + // @Description: A multiplier for the XY velocity and position controller when using systemID in VTOL modes that do horizontal position and velocity control + // @Range: 0.1 1.0 + // @User: Standard + AP_GROUPINFO("_XY_CTRL_MUL", 8, AP_SystemID, xy_control_mul, 0.25), + + AP_GROUPEND +}; + +AP_SystemID::AP_SystemID(void) +{ + AP_Param::setup_object_defaults(this, var_info); +} + +/* + start systemid + */ +void AP_SystemID::start() +{ + start_axis = axis; + + // check if enabled + if (start_axis == AxisType::NONE) { + gcs().send_text(MAV_SEVERITY_WARNING, "SystemID: No axis selected"); + return; + } + if (!plane.control_mode->supports_systemid()) { + gcs().send_text(MAV_SEVERITY_WARNING, "SystemID: Not supported in mode %s", plane.control_mode->name()); + return; + } + if (!hal.util->get_soft_armed()) { + gcs().send_text(MAV_SEVERITY_WARNING, "SystemID: must be armed"); + return; + } + if (!plane.quadplane.enabled()) { + gcs().send_text(MAV_SEVERITY_WARNING, "SystemID: only for quadplane"); + return; + } + + attitude_offset_deg.zero(); + throttle_offset = 0; + + restore.att_bf_feedforward = plane.quadplane.attitude_control->get_bf_feedforward(); + + waveform_time = 0; + time_const_freq = 2.0 / frequency_start; // Two full cycles at the starting frequency + + chirp_input.init(time_record, frequency_start, frequency_stop, time_fade_in, time_fade_out, time_const_freq); + + gcs().send_text(MAV_SEVERITY_INFO, "SystemID Starting: axis=%d", (unsigned)axis); + + plane.Log_Write_SysID_Setup(axis, waveform_magnitude, frequency_start, frequency_stop, time_fade_in, time_const_freq, time_record, time_fade_out); + + running = true; +} + +/* + stop systemid + */ +void AP_SystemID::stop() +{ + if (running) { + running = false; + attitude_offset_deg.zero(); + throttle_offset = 0; + + auto *attitude_control = plane.quadplane.attitude_control; + attitude_control->bf_feedforward(restore.att_bf_feedforward); + attitude_control->rate_bf_roll_sysid(0); + attitude_control->rate_bf_pitch_sysid(0); + attitude_control->rate_bf_yaw_sysid(0); + plane.quadplane.pos_control->set_xy_control_scale_factor(0); + + gcs().send_text(MAV_SEVERITY_INFO, "SystemID stopped"); + } +} + +/* + update systemid - needs to be called at main loop rate + */ +void AP_SystemID::update() +{ + if (!running) { + return; + } + if (chirp_input.completed()) { + stop(); + return; + } + + float const last_loop_time_s = AP::scheduler().get_last_loop_time_s(); + + waveform_time += last_loop_time_s; + waveform_sample = chirp_input.update(waveform_time, waveform_magnitude); + waveform_freq_rads = chirp_input.get_frequency_rads(); + + auto *attitude_control = plane.quadplane.attitude_control; + + switch (start_axis) { + case AxisType::NONE: + // not possible, see start() + break; + case AxisType::INPUT_ROLL: + attitude_offset_deg.x = waveform_sample; + break; + case AxisType::INPUT_PITCH: + attitude_offset_deg.y = waveform_sample; + break; + case AxisType::INPUT_YAW: + attitude_offset_deg.z = waveform_sample; + break; + case AxisType::RECOVER_ROLL: + attitude_offset_deg.x = waveform_sample; + attitude_control->bf_feedforward(false); + break; + case AxisType::RECOVER_PITCH: + attitude_offset_deg.y = waveform_sample; + attitude_control->bf_feedforward(false); + break; + case AxisType::RECOVER_YAW: + attitude_offset_deg.z = waveform_sample; + attitude_control->bf_feedforward(false); + break; + case AxisType::RATE_ROLL: + attitude_control->rate_bf_roll_sysid(radians(waveform_sample)); + break; + case AxisType::RATE_PITCH: + attitude_control->rate_bf_pitch_sysid(radians(waveform_sample)); + break; + case AxisType::RATE_YAW: + attitude_control->rate_bf_yaw_sysid(radians(waveform_sample)); + break; + case AxisType::MIX_ROLL: + attitude_control->actuator_roll_sysid(waveform_sample); + break; + case AxisType::MIX_PITCH: + attitude_control->actuator_pitch_sysid(waveform_sample); + break; + case AxisType::MIX_YAW: + attitude_control->actuator_yaw_sysid(waveform_sample); + break; + case AxisType::MIX_THROTTLE: + throttle_offset += waveform_sample; + break; + } + + // reduce control in XY axis when in position controlled modes + plane.quadplane.pos_control->set_xy_control_scale_factor(xy_control_mul); + + log_data(); +} + +// log system id +void AP_SystemID::log_data() const +{ + Vector3f delta_angle; + float delta_angle_dt; + plane.ins.get_delta_angle(delta_angle, delta_angle_dt); + + Vector3f delta_velocity; + float delta_velocity_dt; + plane.ins.get_delta_velocity(delta_velocity, delta_velocity_dt); + + if (is_positive(delta_angle_dt) && is_positive(delta_velocity_dt)) { + plane.Log_Write_SysID_Data(waveform_time, waveform_sample, waveform_freq_rads / (2 * M_PI), + degrees(delta_angle.x / delta_angle_dt), + degrees(delta_angle.y / delta_angle_dt), + degrees(delta_angle.z / delta_angle_dt), + delta_velocity.x / delta_velocity_dt, + delta_velocity.y / delta_velocity_dt, + delta_velocity.z / delta_velocity_dt); + } +} + +#endif // AP_PLANE_SYSTEMID_ENABLED + diff --git a/ArduPlane/systemid.h b/ArduPlane/systemid.h new file mode 100644 index 00000000000000..1204cf1ccb0d48 --- /dev/null +++ b/ArduPlane/systemid.h @@ -0,0 +1,85 @@ +#pragma once + +#include "quadplane.h" + +#ifndef AP_PLANE_SYSTEMID_ENABLED +#define AP_PLANE_SYSTEMID_ENABLED CONFIG_HAL_BOARD == HAL_BOARD_SITL && HAL_QUADPLANE_ENABLED +#endif + +#if AP_PLANE_SYSTEMID_ENABLED + +#include + +class AP_SystemID { + +public: + AP_SystemID(void); + void start(void); + void stop(void); + void update(); + + static const struct AP_Param::GroupInfo var_info[]; + + const Vector3f &get_attitude_offset_deg(void) const { + return attitude_offset_deg; + } + float get_throttle_offset(void) const { + return throttle_offset; + } + +private: + Chirp chirp_input; + bool running; + + enum class AxisType { + NONE = 0, // none + INPUT_ROLL = 1, // angle input roll axis is being excited + INPUT_PITCH = 2, // angle pitch axis is being excited + INPUT_YAW = 3, // angle yaw axis is being excited + RECOVER_ROLL = 4, // angle roll axis is being excited + RECOVER_PITCH = 5, // angle pitch axis is being excited + RECOVER_YAW = 6, // angle yaw axis is being excited + RATE_ROLL = 7, // rate roll axis is being excited + RATE_PITCH = 8, // rate pitch axis is being excited + RATE_YAW = 9, // rate yaw axis is being excited + MIX_ROLL = 10, // mixer roll axis is being excited + MIX_PITCH = 11, // mixer pitch axis is being excited + MIX_YAW = 12, // mixer pitch axis is being excited + MIX_THROTTLE = 13, // mixer throttle axis is being excited + }; + + void set_bf_feedforward(bool value); + void log_data() const; + + AP_Enum axis; // Controls which axis are being excited. Set to non-zero to display other parameters + AP_Float waveform_magnitude;// Magnitude of chirp waveform + AP_Float frequency_start; // Frequency at the start of the chirp + AP_Float frequency_stop; // Frequency at the end of the chirp + AP_Float time_fade_in; // Time to reach maximum amplitude of chirp + AP_Float time_record; // Time taken to complete the chirp waveform + AP_Float time_fade_out; // Time to reach zero amplitude after chirp finishes + AP_Float xy_control_mul; // multipler for VTOL XY control + + struct { + bool att_bf_feedforward; // Setting of attitude_control->get_bf_feedforward + } restore; + + float waveform_time; // Time reference for waveform + float waveform_sample; // Current waveform sample + float waveform_freq_rads; // Instantaneous waveform frequency + float time_const_freq; // Time at constant frequency before chirp starts + uint32_t last_loop_time_ms; // time in milliseconds of last loop + + Vector2f target_vel; // target velocity for position controller modes + Vector2f target_pos; // target positon + Vector2f input_vel_last; // last cycle input velocity + + // current attitude offset + Vector3f attitude_offset_deg; + float throttle_offset; + + AxisType start_axis; + +}; + +#endif // AP_PLANE_SYSTEMID_ENABLED