zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Copter: systemid mode formatting and name changes

master
Randy Mackay 5 years ago
parent
c11f2247eb
commit
2cb3e446ab
5 changed files with 93 additions and 101 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 4
      ArduCopter/Copter.cpp
  2. 2
      ArduCopter/defines.h
  3. 81
      ArduCopter/mode.h
  4. 105
      ArduCopter/mode_systemid.cpp
  5. 2
      ArduCopter/tuning.cpp

4
ArduCopter/Copter.cpp
Unescape View File

@ -330,7 +330,7 @@ void Copter::update_batt_compass(void) @@ -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() @@ -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

2
ArduCopter/defines.h
Unescape View File

@ -95,7 +95,7 @@ enum tuning_func { @@ -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

81
ArduCopter/mode.h
Unescape View File

@ -56,7 +56,7 @@ public: @@ -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: @@ -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: @@ -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
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.
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.
// 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 {

105
ArduCopter/mode_systemid.cpp
Unescape View File

@ -13,16 +13,16 @@ const AP_Param::GroupInfo ModeSystemId::var_info[] = { @@ -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[] = { @@ -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[] = { @@ -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[] = { @@ -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[] = { @@ -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) @@ -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() @@ -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() @@ -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() @@ -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() @@ -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) @@ -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) @@ -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) {

2
ArduCopter/tuning.cpp
Unescape View File

@ -229,7 +229,7 @@ void Copter::tuning() @@ -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;
}

Write Preview
Loading…
Cancel
Save