From 88194c597132b5590be8ea02711f28e4ca4c6598 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 9 May 2014 09:20:08 +0200 Subject: Remove noreturn attribute from all apps that actually can return --- src/modules/fw_att_control/fw_att_control_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules/fw_att_control/fw_att_control_main.cpp') diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 5276b1c13..a40c402d6 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -273,7 +273,7 @@ private: /** * Main sensor collection task. */ - void task_main() __attribute__((noreturn)); + void task_main(); }; namespace att_control -- cgit v1.2.3 From 3779e216be53540eb9d2e9470ba4077d5d33d534 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 12 May 2014 09:22:07 +0200 Subject: fw att control: use new manual control setpoint variable names --- src/modules/fw_att_control/fw_att_control_main.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) (limited to 'src/modules/fw_att_control/fw_att_control_main.cpp') diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 5276b1c13..5c83f85a1 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -717,9 +717,9 @@ FixedwingAttitudeControl::task_main() * the intended attitude setpoint. Later, after the rate control step the * trim is added again to the control signal. */ - roll_sp = (_manual.roll * _parameters.man_roll_max - _parameters.trim_roll) + _parameters.rollsp_offset_rad; - pitch_sp = (-_manual.pitch * _parameters.man_pitch_max - _parameters.trim_pitch) + _parameters.pitchsp_offset_rad; - throttle_sp = _manual.throttle; + roll_sp = (_manual.y * _parameters.man_roll_max - _parameters.trim_roll) + _parameters.rollsp_offset_rad; + pitch_sp = (-_manual.x * _parameters.man_pitch_max - _parameters.trim_pitch) + _parameters.pitchsp_offset_rad; + throttle_sp = _manual.z; _actuators.control[4] = _manual.flaps; /* @@ -825,10 +825,10 @@ FixedwingAttitudeControl::task_main() } else { /* manual/direct control */ - _actuators.control[0] = _manual.roll; - _actuators.control[1] = -_manual.pitch; - _actuators.control[2] = _manual.yaw; - _actuators.control[3] = _manual.throttle; + _actuators.control[0] = _manual.y; + _actuators.control[1] = -_manual.x; + _actuators.control[2] = _manual.r; + _actuators.control[3] = _manual.z; _actuators.control[4] = _manual.flaps; } -- cgit v1.2.3 From bafa344dcb2e7b7612f6db4198d87f2fc37c4366 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 13 May 2014 08:58:40 +0200 Subject: fw att control: manual setpoint: fix comment and trim sign --- src/modules/fw_att_control/fw_att_control_main.cpp | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) (limited to 'src/modules/fw_att_control/fw_att_control_main.cpp') diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index a40c402d6..654b7d56b 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -706,19 +706,20 @@ FixedwingAttitudeControl::task_main() } else { /* * Scale down roll and pitch as the setpoints are radians - * and a typical remote can only do 45 degrees, the mapping is - * -1..+1 to -45..+45 degrees or -0.75..+0.75 radians. + * and a typical remote can only do around 45 degrees, the mapping is + * -1..+1 to -man_roll_max rad..+man_roll_max rad (equivalent for pitch) * * With this mapping the stick angle is a 1:1 representation of - * the commanded attitude. If more than 45 degrees are desired, - * a scaling parameter can be applied to the remote. + * the commanded attitude. * * The trim gets subtracted here from the manual setpoint to get * the intended attitude setpoint. Later, after the rate control step the * trim is added again to the control signal. */ - roll_sp = (_manual.roll * _parameters.man_roll_max - _parameters.trim_roll) + _parameters.rollsp_offset_rad; - pitch_sp = (-_manual.pitch * _parameters.man_pitch_max - _parameters.trim_pitch) + _parameters.pitchsp_offset_rad; + roll_sp = (_manual.roll * _parameters.man_roll_max - _parameters.trim_roll) + + _parameters.rollsp_offset_rad; + pitch_sp = -(_manual.pitch * _parameters.man_pitch_max - _parameters.trim_pitch) + + _parameters.pitchsp_offset_rad; throttle_sp = _manual.throttle; _actuators.control[4] = _manual.flaps; -- cgit v1.2.3 From e72a7a7dd78cde4a93d42a53328553e71560fc27 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 18 May 2014 00:15:14 +0200 Subject: fw att: robustify main loop against non finite numbers and limit error output rate --- src/modules/fw_att_control/fw_att_control_main.cpp | 42 ++++++++++++++++++---- 1 file changed, 35 insertions(+), 7 deletions(-) (limited to 'src/modules/fw_att_control/fw_att_control_main.cpp') diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 1f3e9f098..cafce4417 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -592,6 +592,8 @@ FixedwingAttitudeControl::task_main() while (!_task_should_exit) { + static int loop_counter = 0; + /* wait for up to 500ms for data */ int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); @@ -755,7 +757,9 @@ FixedwingAttitudeControl::task_main() speed_body_v = _att.R[0][1] * _global_pos.vel_n + _att.R[1][1] * _global_pos.vel_e + _att.R[2][1] * _global_pos.vel_d; speed_body_w = _att.R[0][2] * _global_pos.vel_n + _att.R[1][2] * _global_pos.vel_e + _att.R[2][2] * _global_pos.vel_d; } else { - warnx("Did not get a valid R\n"); + if (loop_counter % 10 == 0) { + warnx("Did not get a valid R\n"); + } } /* Run attitude controllers */ @@ -773,7 +777,10 @@ FixedwingAttitudeControl::task_main() _parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator); _actuators.control[0] = (isfinite(roll_u)) ? roll_u + _parameters.trim_roll : _parameters.trim_roll; if (!isfinite(roll_u)) { - warnx("roll_u %.4f", roll_u); + _roll_ctrl.reset_integrator(); + if (loop_counter % 10 == 0) { + warnx("roll_u %.4f", roll_u); + } } float pitch_u = _pitch_ctrl.control_bodyrate(_att.roll, _att.pitch, @@ -782,8 +789,21 @@ FixedwingAttitudeControl::task_main() _parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator); _actuators.control[1] = (isfinite(pitch_u)) ? pitch_u + _parameters.trim_pitch : _parameters.trim_pitch; if (!isfinite(pitch_u)) { - warnx("pitch_u %.4f, _yaw_ctrl.get_desired_rate() %.4f, airspeed %.4f, airspeed_scaling %.4f, roll_sp %.4f, pitch_sp %.4f, _roll_ctrl.get_desired_rate() %.4f, _pitch_ctrl.get_desired_rate() %.4f att_sp.roll_body %.4f", - (double)pitch_u, (double)_yaw_ctrl.get_desired_rate(), (double)airspeed, (double)airspeed_scaling, (double)roll_sp, (double)pitch_sp, (double)_roll_ctrl.get_desired_rate(), (double)_pitch_ctrl.get_desired_rate(), (double)_att_sp.roll_body); + _pitch_ctrl.reset_integrator(); + if (loop_counter % 10 == 0) { + warnx("pitch_u %.4f, _yaw_ctrl.get_desired_rate() %.4f," + " airspeed %.4f, airspeed_scaling %.4f," + " roll_sp %.4f, pitch_sp %.4f," + " _roll_ctrl.get_desired_rate() %.4f," + " _pitch_ctrl.get_desired_rate() %.4f" + " att_sp.roll_body %.4f", + (double)pitch_u, (double)_yaw_ctrl.get_desired_rate(), + (double)airspeed, (double)airspeed_scaling, + (double)roll_sp, (double)pitch_sp, + (double)_roll_ctrl.get_desired_rate(), + (double)_pitch_ctrl.get_desired_rate(), + (double)_att_sp.roll_body); + } } float yaw_u = _yaw_ctrl.control_bodyrate(_att.roll, _att.pitch, @@ -792,16 +812,23 @@ FixedwingAttitudeControl::task_main() _parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator); _actuators.control[2] = (isfinite(yaw_u)) ? yaw_u + _parameters.trim_yaw : _parameters.trim_yaw; if (!isfinite(yaw_u)) { - warnx("yaw_u %.4f", (double)yaw_u); + _yaw_ctrl.reset_integrator(); + if (loop_counter % 10 == 0) { + warnx("yaw_u %.4f", (double)yaw_u); + } } /* throttle passed through */ _actuators.control[3] = (isfinite(throttle_sp)) ? throttle_sp : 0.0f; if (!isfinite(throttle_sp)) { - warnx("throttle_sp %.4f", (double)throttle_sp); + if (loop_counter % 10 == 0) { + warnx("throttle_sp %.4f", (double)throttle_sp); + } } } else { - warnx("Non-finite setpoint roll_sp: %.4f, pitch_sp %.4f", (double)roll_sp, (double)pitch_sp); + if (loop_counter % 10 == 0) { + warnx("Non-finite setpoint roll_sp: %.4f, pitch_sp %.4f", (double)roll_sp, (double)pitch_sp); + } } /* @@ -865,6 +892,7 @@ FixedwingAttitudeControl::task_main() } + loop_counter++; perf_end(_loop_perf); } -- cgit v1.2.3 From 328fc04d29f055df8cdbd2abc8313fb29eb45148 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 21 May 2014 14:22:01 +0200 Subject: apps: Compile warning fixes --- .../attitude_estimator_ekf/attitude_estimator_ekf_main.cpp | 14 +++----------- .../ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 2 +- src/modules/fw_att_control/fw_att_control_main.cpp | 4 ++-- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 2 +- 4 files changed, 7 insertions(+), 15 deletions(-) (limited to 'src/modules/fw_att_control/fw_att_control_main.cpp') diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index 2ec889efe..66a949af1 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -266,7 +266,6 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att); int loopcounter = 0; - int printcounter = 0; thread_running = true; @@ -274,9 +273,6 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds // struct debug_key_value_s dbg = { .key = "", .value = 0.0f }; // orb_advert_t pub_dbg = -1; - float sensor_update_hz[3] = {0.0f, 0.0f, 0.0f}; - // XXX write this out to perf regs - /* keep track of sensor updates */ uint64_t sensor_last_timestamp[3] = {0, 0, 0}; @@ -287,11 +283,9 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds /* initialize parameter handles */ parameters_init(&ekf_param_handles); - uint64_t start_time = hrt_absolute_time(); bool initialized = false; float gyro_offsets[3] = { 0.0f, 0.0f, 0.0f }; - unsigned offset_count = 0; /* rotation matrix for magnetic declination */ math::Matrix<3, 3> R_decl; @@ -382,7 +376,7 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds /* Fill in gyro measurements */ if (sensor_last_timestamp[0] != raw.timestamp) { update_vect[0] = 1; - sensor_update_hz[0] = 1e6f / (raw.timestamp - sensor_last_timestamp[0]); + // sensor_update_hz[0] = 1e6f / (raw.timestamp - sensor_last_timestamp[0]); sensor_last_timestamp[0] = raw.timestamp; } @@ -393,7 +387,7 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds /* update accelerometer measurements */ if (sensor_last_timestamp[1] != raw.accelerometer_timestamp) { update_vect[1] = 1; - sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]); + // sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]); sensor_last_timestamp[1] = raw.accelerometer_timestamp; } @@ -445,7 +439,7 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds /* update magnetometer measurements */ if (sensor_last_timestamp[2] != raw.magnetometer_timestamp) { update_vect[2] = 1; - sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]); + // sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]); sensor_last_timestamp[2] = raw.magnetometer_timestamp; } @@ -498,8 +492,6 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds continue; } - uint64_t timing_start = hrt_absolute_time(); - attitudeKalmanfilter(update_vect, dt, z_k, x_aposteriori_k, P_aposteriori_k, ekf_params.q, ekf_params.r, euler, Rot_matrix, x_aposteriori, P_aposteriori); diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 7a71894ed..0a6d3fa8f 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -1067,7 +1067,7 @@ FixedwingEstimator::task_main() mavlink_log_info(_mavlink_fd, "[ekf] ref: LA %.4f,LO %.4f,ALT %.2f", lat, lon, (double)gps_alt); warnx("HOME/REF: LA %8.4f,LO %8.4f,ALT %8.2f V: %8.4f %8.4f %8.4f", lat, lon, (double)gps_alt, (double)_ekf->velNED[0], (double)_ekf->velNED[1], (double)_ekf->velNED[2]); - warnx("BARO: %8.4f m / ref: %8.4f m / gps offs: %8.4f m", _ekf->baroHgt, _baro_ref, _baro_gps_offset); + warnx("BARO: %8.4f m / ref: %8.4f m / gps offs: %8.4f m", (double)_ekf->baroHgt, (double)_baro_ref, (double)_baro_gps_offset); warnx("GPS: eph: %8.4f, epv: %8.4f, declination: %8.4f", (double)_gps.eph_m, (double)_gps.epv_m, (double)math::degrees(declination)); _gps_initialized = true; diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 1f3e9f098..6943239e5 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -304,8 +304,8 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : /* publications */ _rate_sp_pub(-1), - _actuators_0_pub(-1), _attitude_sp_pub(-1), + _actuators_0_pub(-1), _actuators_1_pub(-1), /* performance counters */ @@ -773,7 +773,7 @@ FixedwingAttitudeControl::task_main() _parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator); _actuators.control[0] = (isfinite(roll_u)) ? roll_u + _parameters.trim_roll : _parameters.trim_roll; if (!isfinite(roll_u)) { - warnx("roll_u %.4f", roll_u); + warnx("roll_u %.4f", (double)roll_u); } float pitch_u = _pitch_ctrl.control_bodyrate(_att.roll, _att.pitch, diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 9cbc26efe..5b877cd77 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -418,8 +418,8 @@ FixedwingPositionControl::FixedwingPositionControl() : land_motor_lim(false), land_onslope(false), launch_detected(false), - last_manual(false), usePreTakeoffThrust(false), + last_manual(false), flare_curve_alt_rel_last(0.0f), launchDetector(), _airspeed_error(0.0f), -- cgit v1.2.3 From b9b81beb17eb449921f11f46bc419056dce03852 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 21 May 2014 21:49:00 +0200 Subject: fw att: add performance counter --- src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp | 8 ++++++++ src/lib/ecl/attitude_fw/ecl_pitch_controller.h | 4 ++++ src/lib/ecl/attitude_fw/ecl_roll_controller.cpp | 8 ++++++++ src/lib/ecl/attitude_fw/ecl_roll_controller.h | 4 ++++ src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp | 8 ++++++++ src/lib/ecl/attitude_fw/ecl_yaw_controller.h | 4 ++++ src/modules/fw_att_control/fw_att_control_main.cpp | 19 +++++++++++++++++-- 7 files changed, 53 insertions(+), 2 deletions(-) (limited to 'src/modules/fw_att_control/fw_att_control_main.cpp') diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp index a3f5199b1..0a909d02f 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp @@ -63,12 +63,19 @@ ECL_PitchController::ECL_PitchController() : _rate_setpoint(0.0f), _bodyrate_setpoint(0.0f) { + perf_alloc(PC_COUNT, "fw att control pitch nonfinite input"); +} + +ECL_PitchController::~ECL_PitchController() +{ + perf_free(_nonfinite_input_perf); } float ECL_PitchController::control_attitude(float pitch_setpoint, float roll, float pitch, float airspeed) { /* Do not calculate control signal with bad inputs */ if (!(isfinite(pitch_setpoint) && isfinite(roll) && isfinite(pitch) && isfinite(airspeed))) { + perf_count(_nonfinite_input_perf); warnx("not controlling pitch"); return _rate_setpoint; } @@ -131,6 +138,7 @@ float ECL_PitchController::control_bodyrate(float roll, float pitch, if (!(isfinite(roll) && isfinite(pitch) && isfinite(pitch_rate) && isfinite(yaw_rate) && isfinite(yaw_rate_setpoint) && isfinite(airspeed_min) && isfinite(airspeed_max) && isfinite(scaler))) { + perf_count(_nonfinite_input_perf); return math::constrain(_last_output, -1.0f, 1.0f); } diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.h b/src/lib/ecl/attitude_fw/ecl_pitch_controller.h index 30a82a86a..39b9f9d03 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.h @@ -51,12 +51,15 @@ #include #include +#include class __EXPORT ECL_PitchController //XXX: create controller superclass { public: ECL_PitchController(); + ~ECL_PitchController(); + float control_attitude(float pitch_setpoint, float roll, float pitch, float airspeed); @@ -126,6 +129,7 @@ private: float _rate_error; float _rate_setpoint; float _bodyrate_setpoint; + perf_counter_t _nonfinite_input_perf; }; #endif // ECL_PITCH_CONTROLLER_H diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp index 6ad00049d..82903ef5a 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp @@ -61,12 +61,19 @@ ECL_RollController::ECL_RollController() : _rate_setpoint(0.0f), _bodyrate_setpoint(0.0f) { + perf_alloc(PC_COUNT, "fw att control roll nonfinite input"); +} + +ECL_RollController::~ECL_RollController() +{ + perf_free(_nonfinite_input_perf); } float ECL_RollController::control_attitude(float roll_setpoint, float roll) { /* Do not calculate control signal with bad inputs */ if (!(isfinite(roll_setpoint) && isfinite(roll))) { + perf_count(_nonfinite_input_perf); return _rate_setpoint; } @@ -94,6 +101,7 @@ float ECL_RollController::control_bodyrate(float pitch, if (!(isfinite(pitch) && isfinite(roll_rate) && isfinite(yaw_rate) && isfinite(yaw_rate_setpoint) && isfinite(airspeed_min) && isfinite(airspeed_max) && isfinite(scaler))) { + perf_count(_nonfinite_input_perf); return math::constrain(_last_output, -1.0f, 1.0f); } diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.h b/src/lib/ecl/attitude_fw/ecl_roll_controller.h index 92c64b95f..0799dbe03 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.h @@ -51,12 +51,15 @@ #include #include +#include class __EXPORT ECL_RollController //XXX: create controller superclass { public: ECL_RollController(); + ~ECL_RollController(); + float control_attitude(float roll_setpoint, float roll); float control_bodyrate(float pitch, @@ -117,6 +120,7 @@ private: float _rate_error; float _rate_setpoint; float _bodyrate_setpoint; + perf_counter_t _nonfinite_input_perf; }; #endif // ECL_ROLL_CONTROLLER_H diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp index d43e0314e..e53ffc644 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp @@ -60,6 +60,12 @@ ECL_YawController::ECL_YawController() : _bodyrate_setpoint(0.0f), _coordinated_min_speed(1.0f) { + perf_alloc(PC_COUNT, "fw att control yaw nonfinite input"); +} + +ECL_YawController::~ECL_YawController() +{ + perf_free(_nonfinite_input_perf); } float ECL_YawController::control_attitude(float roll, float pitch, @@ -70,6 +76,7 @@ float ECL_YawController::control_attitude(float roll, float pitch, if (!(isfinite(roll) && isfinite(pitch) && isfinite(speed_body_u) && isfinite(speed_body_v) && isfinite(speed_body_w) && isfinite(roll_rate_setpoint) && isfinite(pitch_rate_setpoint))) { + perf_count(_nonfinite_input_perf); return _rate_setpoint; } // static int counter = 0; @@ -113,6 +120,7 @@ float ECL_YawController::control_bodyrate(float roll, float pitch, if (!(isfinite(roll) && isfinite(pitch) && isfinite(pitch_rate) && isfinite(yaw_rate) && isfinite(pitch_rate_setpoint) && isfinite(airspeed_min) && isfinite(airspeed_max) && isfinite(scaler))) { + perf_count(_nonfinite_input_perf); return math::constrain(_last_output, -1.0f, 1.0f); } /* get the usual dt estimate */ diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h index 03f3202d0..a360c14b8 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h @@ -50,12 +50,15 @@ #include #include +#include class __EXPORT ECL_YawController //XXX: create controller superclass { public: ECL_YawController(); + ~ECL_YawController(); + float control_attitude(float roll, float pitch, float speed_body_u, float speed_body_v, float speed_body_w, float roll_rate_setpoint, float pitch_rate_setpoint); @@ -118,6 +121,7 @@ private: float _rate_setpoint; float _bodyrate_setpoint; float _coordinated_min_speed; + perf_counter_t _nonfinite_input_perf; }; diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 59854e734..178b590ae 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -134,6 +134,8 @@ private: struct vehicle_global_position_s _global_pos; /**< global position */ perf_counter_t _loop_perf; /**< loop performance counter */ + perf_counter_t _nonfinite_input_perf; /**< performance counter for non finite input */ + perf_counter_t _nonfinite_output_perf; /**< performance counter for non finite output */ bool _setpoint_valid; /**< flag if the position control setpoint is valid */ @@ -310,6 +312,8 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "fw att control")), + _nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control nonfinite input")), + _nonfinite_output_perf(perf_alloc(PC_COUNT, "fw att control nonfinite output")), /* states */ _setpoint_valid(false) { @@ -387,6 +391,10 @@ FixedwingAttitudeControl::~FixedwingAttitudeControl() } while (_control_task != -1); } + perf_free(_loop_perf); + perf_free(_nonfinite_input_perf); + perf_free(_nonfinite_output_perf); + att_control::g_control = nullptr; } @@ -674,10 +682,12 @@ FixedwingAttitudeControl::task_main() float airspeed; /* if airspeed is not updating, we assume the normal average speed */ - if (!isfinite(_airspeed.true_airspeed_m_s) || + if (bool nonfinite = !isfinite(_airspeed.true_airspeed_m_s) || hrt_elapsed_time(&_airspeed.timestamp) > 1e6) { airspeed = _parameters.airspeed_trim; - + if (nonfinite) { + perf_count(_nonfinite_input_perf); + } } else { airspeed = _airspeed.true_airspeed_m_s; } @@ -778,6 +788,8 @@ FixedwingAttitudeControl::task_main() _actuators.control[0] = (isfinite(roll_u)) ? roll_u + _parameters.trim_roll : _parameters.trim_roll; if (!isfinite(roll_u)) { _roll_ctrl.reset_integrator(); + perf_count(_nonfinite_output_perf); + if (loop_counter % 10 == 0) { warnx("roll_u %.4f", (double)roll_u); } @@ -790,6 +802,7 @@ FixedwingAttitudeControl::task_main() _actuators.control[1] = (isfinite(pitch_u)) ? pitch_u + _parameters.trim_pitch : _parameters.trim_pitch; if (!isfinite(pitch_u)) { _pitch_ctrl.reset_integrator(); + perf_count(_nonfinite_output_perf); if (loop_counter % 10 == 0) { warnx("pitch_u %.4f, _yaw_ctrl.get_desired_rate() %.4f," " airspeed %.4f, airspeed_scaling %.4f," @@ -813,6 +826,7 @@ FixedwingAttitudeControl::task_main() _actuators.control[2] = (isfinite(yaw_u)) ? yaw_u + _parameters.trim_yaw : _parameters.trim_yaw; if (!isfinite(yaw_u)) { _yaw_ctrl.reset_integrator(); + perf_count(_nonfinite_output_perf); if (loop_counter % 10 == 0) { warnx("yaw_u %.4f", (double)yaw_u); } @@ -826,6 +840,7 @@ FixedwingAttitudeControl::task_main() } } } else { + perf_count(_nonfinite_input_perf); if (loop_counter % 10 == 0) { warnx("Non-finite setpoint roll_sp: %.4f, pitch_sp %.4f", (double)roll_sp, (double)pitch_sp); } -- cgit v1.2.3