From 8267bdf4a5aa857db0f970da750d782f1d8ec369 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sun, 9 Mar 2014 15:50:41 +0100 Subject: fw_att_control: airspeed is now used correctly --- src/modules/fw_att_control/fw_att_control_main.cpp | 14 +++++--------- 1 file changed, 5 insertions(+), 9 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 17b1028f9..c59a396c2 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -230,7 +230,7 @@ private: /** * Check for airspeed updates. */ - bool vehicle_airspeed_poll(); + void vehicle_airspeed_poll(); /** * Check for accel updates. @@ -452,7 +452,7 @@ FixedwingAttitudeControl::vehicle_manual_poll() } } -bool +void FixedwingAttitudeControl::vehicle_airspeed_poll() { /* check if there is a new position */ @@ -462,10 +462,7 @@ FixedwingAttitudeControl::vehicle_airspeed_poll() if (airspeed_updated) { orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed); // warnx("airspeed poll: ind: %.4f, true: %.4f", _airspeed.indicated_airspeed_m_s, _airspeed.true_airspeed_m_s); - return true; } - - return false; } void @@ -539,7 +536,7 @@ FixedwingAttitudeControl::task_main() parameters_update(); /* get an initial update for all sensor and status data */ - (void)vehicle_airspeed_poll(); + vehicle_airspeed_poll(); vehicle_setpoint_poll(); vehicle_accel_poll(); vehicle_control_mode_poll(); @@ -596,7 +593,7 @@ FixedwingAttitudeControl::task_main() /* load local copies */ orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att); - _airspeed_valid = vehicle_airspeed_poll(); + vehicle_airspeed_poll(); vehicle_setpoint_poll(); @@ -636,8 +633,7 @@ FixedwingAttitudeControl::task_main() float airspeed; /* if airspeed is smaller than min, the sensor is not giving good readings */ - if (!_airspeed_valid || - (_airspeed.indicated_airspeed_m_s < 0.5f * _parameters.airspeed_min) || + if ((_airspeed.indicated_airspeed_m_s < 0.5f * _parameters.airspeed_min) || !isfinite(_airspeed.indicated_airspeed_m_s)) { airspeed = _parameters.airspeed_trim; -- cgit v1.2.3 From 2db140bcabaafcccc1ed2ce070d7a24b4133eb57 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 11 Mar 2014 10:59:11 +0100 Subject: fw_att_control: delete the unused flag --- src/modules/fw_att_control/fw_att_control_main.cpp | 8 +------- 1 file changed, 1 insertion(+), 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 c59a396c2..ec91f4d8c 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -136,7 +136,6 @@ private: perf_counter_t _loop_perf; /**< loop performance counter */ bool _setpoint_valid; /**< flag if the position control setpoint is valid */ - bool _airspeed_valid; /**< flag if the airspeed measurement is valid */ struct { float tconst; @@ -293,8 +292,7 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "fw att control")), /* states */ - _setpoint_valid(false), - _airspeed_valid(false) + _setpoint_valid(false) { /* safely initialize structs */ _att = {0}; @@ -753,10 +751,6 @@ FixedwingAttitudeControl::task_main() warnx("Non-finite setpoint roll_sp: %.4f, pitch_sp %.4f", roll_sp, pitch_sp); } - // warnx("aspd: %s: %6.2f, aspd scaling: %6.2f, controls: %5.2f %5.2f %5.2f %5.2f", (_airspeed_valid) ? "valid" : "unknown", - // airspeed, airspeed_scaling, _actuators.control[0], _actuators.control[1], - // _actuators.control[2], _actuators.control[3]); - /* * Lazily publish the rate setpoint (for analysis, the actuators are published below) * only once available -- cgit v1.2.3 From 2cd29b7c8ce7fc26af6f1a3957e243a18bc09d5b Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 11 Mar 2014 11:07:48 +0100 Subject: fw_att_control: proper struct initialization --- src/modules/fw_att_control/fw_att_control_main.cpp | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 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 ec91f4d8c..fa0f5962f 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -295,15 +295,15 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _setpoint_valid(false) { /* safely initialize structs */ - _att = {0}; - _accel = {0}; - _att_sp = {0}; - _manual = {0}; - _airspeed = {0}; - _vcontrol_mode = {0}; - _actuators = {0}; - _actuators_airframe = {0}; - _global_pos = {0}; + _att = {}; + _accel = {}; + _att_sp = {}; + _manual = {}; + _airspeed = {}; + _vcontrol_mode = {}; + _actuators = {}; + _actuators_airframe = {}; + _global_pos = {}; _parameter_handles.tconst = param_find("FW_ATT_TC"); -- cgit v1.2.3 From 2f06f15240a9d03a282212538283e53e9bcc4b9f Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 11 Mar 2014 11:09:49 +0100 Subject: fw_att_control: whitespace only --- src/modules/fw_att_control/fw_att_control_main.cpp | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 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 fa0f5962f..deafd7f86 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -108,14 +108,14 @@ private: bool _task_should_exit; /**< if true, sensor task should exit */ int _control_task; /**< task handle for sensor task */ - int _att_sub; /**< vehicle attitude subscription */ - int _accel_sub; /**< accelerometer subscription */ + int _att_sub; /**< vehicle attitude subscription */ + int _accel_sub; /**< accelerometer subscription */ int _att_sp_sub; /**< vehicle attitude setpoint */ int _attitude_sub; /**< raw rc channels data subscription */ int _airspeed_sub; /**< airspeed subscription */ int _vcontrol_mode_sub; /**< vehicle status subscription */ - int _params_sub; /**< notification of parameter updates */ - int _manual_sub; /**< notification of manual control updates */ + int _params_sub; /**< notification of parameter updates */ + int _manual_sub; /**< notification of manual control updates */ int _global_pos_sub; /**< global position subscription */ orb_advert_t _rate_sp_pub; /**< rate setpoint publication */ @@ -123,15 +123,15 @@ private: orb_advert_t _actuators_0_pub; /**< actuator control group 0 setpoint */ orb_advert_t _actuators_1_pub; /**< actuator control group 1 setpoint (Airframe) */ - struct vehicle_attitude_s _att; /**< vehicle attitude */ - struct accel_report _accel; /**< body frame accelerations */ + struct vehicle_attitude_s _att; /**< vehicle attitude */ + struct accel_report _accel; /**< body frame accelerations */ struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ struct manual_control_setpoint_s _manual; /**< r/c channel data */ - struct airspeed_s _airspeed; /**< airspeed */ - struct vehicle_control_mode_s _vcontrol_mode; /**< vehicle control mode */ + struct airspeed_s _airspeed; /**< airspeed */ + struct vehicle_control_mode_s _vcontrol_mode; /**< vehicle control mode */ struct actuator_controls_s _actuators; /**< actuator control inputs */ - struct actuator_controls_s _actuators_airframe; /**< actuator control inputs */ - struct vehicle_global_position_s _global_pos; /**< global position */ + struct actuator_controls_s _actuators_airframe; /**< actuator control inputs */ + struct vehicle_global_position_s _global_pos; /**< global position */ perf_counter_t _loop_perf; /**< loop performance counter */ -- cgit v1.2.3 From a977ff2b6295bd4bac614a854ecc35e1b9b2d75c Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 12 Mar 2014 00:20:27 +0100 Subject: fw att control: add timestamp dependency in airspeed check --- src/modules/fw_att_control/fw_att_control_main.cpp | 3 ++- 1 file changed, 2 insertions(+), 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 deafd7f86..497d1d15a 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -632,7 +632,8 @@ FixedwingAttitudeControl::task_main() /* if airspeed is smaller than min, the sensor is not giving good readings */ if ((_airspeed.indicated_airspeed_m_s < 0.5f * _parameters.airspeed_min) || - !isfinite(_airspeed.indicated_airspeed_m_s)) { + !isfinite(_airspeed.indicated_airspeed_m_s) || + (float)hrt_elapsed_time(&_airspeed.timestamp) > 1e6f) { airspeed = _parameters.airspeed_trim; } else { -- cgit v1.2.3 From d9d0e60bd574b135d98bdaa48c4b32056f815217 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 12 Mar 2014 10:02:39 +0100 Subject: fw att ctrl: airspeed check: remove unnecessary cast --- 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 497d1d15a..bb674d6c9 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -633,7 +633,7 @@ FixedwingAttitudeControl::task_main() /* if airspeed is smaller than min, the sensor is not giving good readings */ if ((_airspeed.indicated_airspeed_m_s < 0.5f * _parameters.airspeed_min) || !isfinite(_airspeed.indicated_airspeed_m_s) || - (float)hrt_elapsed_time(&_airspeed.timestamp) > 1e6f) { + hrt_elapsed_time(&_airspeed.timestamp) > 1e6) { airspeed = _parameters.airspeed_trim; } else { -- cgit v1.2.3