diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp index 5a9fedd7a4..638cc8b159 100644 --- a/ArduCopter/Copter.cpp +++ b/ArduCopter/Copter.cpp @@ -330,7 +330,7 @@ void Copter::update_batt_compass(void) // should be run at 400hz void Copter::fourhundred_hz_logging() { - if (should_log(MASK_LOG_ATTITUDE_FAST) && !copter.flightmode->stop_attitude_logging()) { + if (should_log(MASK_LOG_ATTITUDE_FAST) && !copter.flightmode->logs_attitude()) { Log_Write_Attitude(); } } @@ -340,7 +340,7 @@ void Copter::fourhundred_hz_logging() void Copter::ten_hz_logging_loop() { // log attitude data if we're not already logging at the higher rate - if (should_log(MASK_LOG_ATTITUDE_MED) && !should_log(MASK_LOG_ATTITUDE_FAST) && !copter.flightmode->stop_attitude_logging()) { + if (should_log(MASK_LOG_ATTITUDE_MED) && !should_log(MASK_LOG_ATTITUDE_FAST) && !copter.flightmode->logs_attitude()) { Log_Write_Attitude(); } // log EKF attitude data diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index e4a957a368..614931a2c5 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -95,7 +95,7 @@ enum tuning_func { TUNING_RATE_MOT_YAW_HEADROOM = 55, // motors yaw headroom minimum TUNING_RATE_YAW_FILT = 56, // yaw rate input filter TUNING_WINCH = 57, // winch control (not actually a value to be tuned) - SYSTEM_ID_MAGNITUDE = 58 // magnitude of the system ID signal + TUNING_SYSTEM_ID_MAGNITUDE = 58 // magnitude of the system ID signal }; // Acro Trainer types diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index b36579384e..8427ca0b31 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -56,7 +56,7 @@ public: virtual bool is_autopilot() const { return false; } virtual bool has_user_takeoff(bool must_navigate) const { return false; } virtual bool in_guided_mode() const { return false; } - virtual bool stop_attitude_logging() const { return false; } + virtual bool logs_attitude() const { return false; } // return a string for this flightmode virtual const char *name() const = 0; @@ -1185,8 +1185,8 @@ public: bool has_manual_throttle() const override { return false; } bool allows_arming(bool from_gcs) const override { return true; }; bool is_autopilot() const override { return false; } - bool stop_attitude_logging() const override { return true; } - void set_magnitude(float input) {waveform_magnitude = input;} + bool logs_attitude() const override { return true; } + void set_magnitude(float input) { waveform_magnitude = input; } static const struct AP_Param::GroupInfo var_info[]; @@ -1196,48 +1196,47 @@ protected: const char *name4() const override { return "SYSI"; } private: - void log_data(); - float waveform(float time); - enum 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 log_data(); + float waveform(float time); + + 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 }; - AP_Int8 systemID_axis; // Controls which axis are being excited - 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_Int8 axis; // Controls which axis are being excited + 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 - bool att_bf_feedforward; // Setting of attitude_control->get_bf_feedforward - 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 - int8_t log_subsample; // Subsample multiple for logging. + bool att_bf_feedforward; // Setting of attitude_control->get_bf_feedforward + 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 + int8_t log_subsample; // Subsample multiple for logging. // System ID states - enum SystemIDModeState { - SystemID_Stopped, - SystemID_Testing - }; - - SystemIDModeState systemIDState; + enum class SystemIDModeState { + SYSTEMID_STATE_STOPPED, + SYSTEMID_STATE_TESTING + } systemid_state; }; class ModeThrow : public Mode { diff --git a/ArduCopter/mode_systemid.cpp b/ArduCopter/mode_systemid.cpp index 5a089c06c7..47daff5136 100644 --- a/ArduCopter/mode_systemid.cpp +++ b/ArduCopter/mode_systemid.cpp @@ -13,16 +13,16 @@ const AP_Param::GroupInfo ModeSystemId::var_info[] = { // @Description: Controls which axis are being excited // @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 - AP_GROUPINFO("_AXIS", 1, ModeSystemId, systemID_axis, 0), + AP_GROUPINFO("_AXIS", 1, ModeSystemId, axis, 0), - // @Param: _MAG - // @DisplayName: Chirp Magnitude + // @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("_MAG", 2, ModeSystemId, waveform_magnitude, 15), + AP_GROUPINFO("_MAGNITUDE", 2, ModeSystemId, waveform_magnitude, 15), // @Param: _F_START_HZ - // @DisplayName: Start Frequency + // @DisplayName: System identification Start Frequency // @Description: Frequency at the start of the sweep // @Range: 0.01 100 // @Units: Hz @@ -30,7 +30,7 @@ const AP_Param::GroupInfo ModeSystemId::var_info[] = { AP_GROUPINFO("_F_START_HZ", 3, ModeSystemId, frequency_start, 0.5f), // @Param: _F_STOP_HZ - // @DisplayName: Stop Frequency + // @DisplayName: System identification Stop Frequency // @Description: Frequency at the end of the sweep // @Range: 0.01 100 // @Units: Hz @@ -38,7 +38,7 @@ const AP_Param::GroupInfo ModeSystemId::var_info[] = { AP_GROUPINFO("_F_STOP_HZ", 4, ModeSystemId, frequency_stop, 40), // @Param: _T_FADE_IN - // @DisplayName: Fade in time + // @DisplayName: System identification Fade in time // @Description: Time to reach maximum amplitude of sweep // @Range: 0 20 // @Units: s @@ -46,7 +46,7 @@ const AP_Param::GroupInfo ModeSystemId::var_info[] = { AP_GROUPINFO("_T_FADE_IN", 5, ModeSystemId, time_fade_in, 15), // @Param: _T_REC - // @DisplayName: Total Sweep length + // @DisplayName: System identification Total Sweep length // @Description: Time taken to complete the sweep // @Range: 0 255 // @Units: s @@ -54,7 +54,7 @@ const AP_Param::GroupInfo ModeSystemId::var_info[] = { AP_GROUPINFO("_T_REC", 6, ModeSystemId, time_record, 70), // @Param: _T_FADE_OUT - // @DisplayName: Fade out time + // @DisplayName: System identification Fade out time // @Description: Time to reach zero amplitude at the end of the sweep // @Range: 0 5 // @Units: s @@ -86,12 +86,12 @@ bool ModeSystemId::init(bool ignore_checks) att_bf_feedforward = attitude_control->get_bf_feedforward(); waveform_time = 0.0f; time_const_freq = 2.0f / frequency_start; // Two full cycles at the starting frequency - systemIDState = SystemID_Testing; + systemid_state = SystemIDModeState::SYSTEMID_STATE_TESTING; log_subsample = 0; - gcs().send_text(MAV_SEVERITY_INFO, "SystemID Starting: axis=%d", (unsigned)systemID_axis); + gcs().send_text(MAV_SEVERITY_INFO, "SystemID Starting: axis=%d", (unsigned)axis); - copter.Log_Write_SysID_Setup(systemID_axis, waveform_magnitude, frequency_start, frequency_stop, time_fade_in, time_const_freq, time_record, time_fade_out); + copter.Log_Write_SysID_Setup(axis, waveform_magnitude, frequency_start, frequency_stop, time_fade_in, time_const_freq, time_record, time_fade_out); return true; } @@ -156,81 +156,82 @@ void ModeSystemId::run() float pilot_throttle_scaled = get_pilot_desired_throttle(); #endif - if((systemIDState == SystemID_Testing) && (!is_positive(frequency_start) || !is_positive(frequency_stop) || is_negative(time_fade_in) || !is_positive(time_record) || is_negative(time_fade_out) || (time_record <= time_const_freq))) { - systemIDState = SystemID_Stopped; + if ((systemid_state == SystemIDModeState::SYSTEMID_STATE_TESTING) && + (!is_positive(frequency_start) || !is_positive(frequency_stop) || is_negative(time_fade_in) || !is_positive(time_record) || is_negative(time_fade_out) || (time_record <= time_const_freq))) { + systemid_state = SystemIDModeState::SYSTEMID_STATE_STOPPED; gcs().send_text(MAV_SEVERITY_INFO, "SystemID Parameter Error"); } waveform_time += G_Dt; waveform_sample = waveform(waveform_time - SYSTEM_ID_DELAY); - switch (systemIDState) { - case SystemID_Stopped: + switch (systemid_state) { + case SystemIDModeState::SYSTEMID_STATE_STOPPED: break; - case SystemID_Testing: + case SystemIDModeState::SYSTEMID_STATE_TESTING: attitude_control->bf_feedforward(att_bf_feedforward); - if(copter.ap.land_complete) { - systemIDState = SystemID_Stopped; + if (copter.ap.land_complete) { + systemid_state = SystemIDModeState::SYSTEMID_STATE_STOPPED; gcs().send_text(MAV_SEVERITY_INFO, "SystemID Stopped: Landed"); break; } - if(attitude_control->lean_angle()*100 > attitude_control->lean_angle_max()) { - systemIDState = SystemID_Stopped; + if (attitude_control->lean_angle()*100 > attitude_control->lean_angle_max()) { + systemid_state = SystemIDModeState::SYSTEMID_STATE_STOPPED; gcs().send_text(MAV_SEVERITY_INFO, "SystemID Stopped: lean=%f max=%f", (double)attitude_control->lean_angle(), (double)attitude_control->lean_angle_max()); break; } - if(waveform_time > SYSTEM_ID_DELAY + time_fade_in + time_const_freq + time_record + time_fade_out) { - systemIDState = SystemID_Stopped; + if (waveform_time > SYSTEM_ID_DELAY + time_fade_in + time_const_freq + time_record + time_fade_out) { + systemid_state = SystemIDModeState::SYSTEMID_STATE_STOPPED; gcs().send_text(MAV_SEVERITY_INFO, "SystemID Finished"); break; } - switch (systemID_axis) { - case NONE: - systemIDState = SystemID_Stopped; + switch ((AxisType)axis.get()) { + case AxisType::NONE: + systemid_state = SystemIDModeState::SYSTEMID_STATE_STOPPED; gcs().send_text(MAV_SEVERITY_INFO, "SystemID Stopped: axis = 0"); break; - case INPUT_ROLL: + case AxisType::INPUT_ROLL: target_roll += waveform_sample*100.0f; break; - case INPUT_PITCH: + case AxisType::INPUT_PITCH: target_pitch += waveform_sample*100.0f; break; - case INPUT_YAW: + case AxisType::INPUT_YAW: target_yaw_rate += waveform_sample*100.0f; break; - case RECOVER_ROLL: + case AxisType::RECOVER_ROLL: target_roll += waveform_sample*100.0f; attitude_control->bf_feedforward(false); break; - case RECOVER_PITCH: + case AxisType::RECOVER_PITCH: target_pitch += waveform_sample*100.0f; attitude_control->bf_feedforward(false); break; - case RECOVER_YAW: + case AxisType::RECOVER_YAW: target_yaw_rate += waveform_sample*100.0f; attitude_control->bf_feedforward(false); break; - case RATE_ROLL: + case AxisType::RATE_ROLL: attitude_control->rate_bf_roll_sysid(radians(waveform_sample)); break; - case RATE_PITCH: + case AxisType::RATE_PITCH: attitude_control->rate_bf_pitch_sysid(radians(waveform_sample)); break; - case RATE_YAW: + case AxisType::RATE_YAW: attitude_control->rate_bf_yaw_sysid(radians(waveform_sample)); break; - case MIX_ROLL: + case AxisType::MIX_ROLL: attitude_control->actuator_roll_sysid(waveform_sample); break; - case MIX_PITCH: + case AxisType::MIX_PITCH: attitude_control->actuator_pitch_sysid(waveform_sample); break; - case MIX_YAW: + case AxisType::MIX_YAW: attitude_control->actuator_yaw_sysid(waveform_sample); break; - case MIX_THROTTLE: + case AxisType::MIX_THROTTLE: pilot_throttle_scaled += waveform_sample; break; } @@ -240,8 +241,6 @@ void ModeSystemId::run() // call attitude controller attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate); - // body-frame rate controller is run directly from 100hz loop - // output pilot's throttle if (copter.is_tradheli()) { attitude_control->set_throttle_out(pilot_throttle_scaled, false, g.throttle_filt); @@ -249,13 +248,13 @@ void ModeSystemId::run() attitude_control->set_throttle_out(pilot_throttle_scaled, true, g.throttle_filt); } - if(log_subsample <= 0){ + if (log_subsample <= 0) { log_data(); - if(copter.should_log(MASK_LOG_ATTITUDE_FAST) && copter.should_log(MASK_LOG_ATTITUDE_MED)) { + if (copter.should_log(MASK_LOG_ATTITUDE_FAST) && copter.should_log(MASK_LOG_ATTITUDE_MED)) { log_subsample = 1; - } else if(copter.should_log(MASK_LOG_ATTITUDE_FAST)){ + } else if (copter.should_log(MASK_LOG_ATTITUDE_FAST)) { log_subsample = 2; - } else if(copter.should_log(MASK_LOG_ATTITUDE_MED)){ + } else if (copter.should_log(MASK_LOG_ATTITUDE_MED)) { log_subsample = 4; } else { log_subsample = 8; @@ -264,26 +263,20 @@ void ModeSystemId::run() log_subsample -= 1; } -// init_test - initialises the test +// log system id and attitude void ModeSystemId::log_data() { - int8_t index = copter.ahrs.get_primary_gyro_index(); - if(index < 0){ - index = 0; - } + uint8_t index = copter.ahrs.get_primary_gyro_index(); Vector3f delta_angle; copter.ins.get_delta_angle(index, delta_angle); float delta_angle_dt = copter.ins.get_delta_angle_dt(index); index = copter.ahrs.get_primary_accel_index(); - if(index < 0){ - index = 0; - } Vector3f delta_velocity; copter.ins.get_delta_velocity(index, delta_velocity); float delta_velocity_dt = copter.ins.get_delta_velocity_dt(index); - if(is_positive(delta_angle_dt) && is_positive(delta_velocity_dt)){ + if (is_positive(delta_angle_dt) && is_positive(delta_velocity_dt)) { copter.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); } @@ -302,7 +295,7 @@ float ModeSystemId::waveform(float time) float B = logf(wMax / wMin); - if(time <= 0.0f) { + if (time <= 0.0f) { window = 0.0f; } else if (time <= time_fade_in) { window = 0.5 - 0.5 * cosf(M_PI * time / time_fade_in); @@ -314,7 +307,7 @@ float ModeSystemId::waveform(float time) window = 0.0; } - if(time <= 0.0f) { + if (time <= 0.0f) { waveform_freq_rads = wMin; output = 0.0f; } else if (time <= time_const_freq) { diff --git a/ArduCopter/tuning.cpp b/ArduCopter/tuning.cpp index cbdc334e0b..0744f3f4ac 100644 --- a/ArduCopter/tuning.cpp +++ b/ArduCopter/tuning.cpp @@ -229,7 +229,7 @@ void Copter::tuning() #endif #if MODE_SYSTEMID_ENABLED == ENABLED - case SYSTEM_ID_MAGNITUDE: + case TUNING_SYSTEM_ID_MAGNITUDE: copter.mode_systemid.set_magnitude(tuning_value); break; }