Browse Source

FixedwingAttitudeControl: remove SuperBlock dependency

sbg
Beat Küng 7 years ago
parent
commit
51ca01ce04
  1. 11
      src/modules/fw_att_control/FixedwingAttitudeControl.cpp
  2. 6
      src/modules/fw_att_control/FixedwingAttitudeControl.hpp

11
src/modules/fw_att_control/FixedwingAttitudeControl.cpp

@ -41,8 +41,7 @@
extern "C" __EXPORT int fw_att_control_main(int argc, char *argv[]); extern "C" __EXPORT int fw_att_control_main(int argc, char *argv[]);
FixedwingAttitudeControl::FixedwingAttitudeControl() : FixedwingAttitudeControl::FixedwingAttitudeControl() :
SuperBlock(nullptr, "FW_ATT"), _airspeed_sub(ORB_ID(airspeed)),
_sub_airspeed(ORB_ID(airspeed), 0, 0, &getSubscriptions()),
/* performance counters */ /* performance counters */
_loop_perf(perf_alloc(PC_ELAPSED, "fwa_dt")), _loop_perf(perf_alloc(PC_ELAPSED, "fwa_dt")),
@ -527,7 +526,7 @@ void FixedwingAttitudeControl::run()
matrix::Eulerf euler_angles(R); matrix::Eulerf euler_angles(R);
updateSubscriptions(); _airspeed_sub.update();
vehicle_setpoint_poll(); vehicle_setpoint_poll();
vehicle_control_mode_poll(); vehicle_control_mode_poll();
vehicle_manual_poll(); vehicle_manual_poll();
@ -563,12 +562,12 @@ void FixedwingAttitudeControl::run()
float airspeed; float airspeed;
/* if airspeed is non-finite or not valid or if we are asked not to control it, we assume the normal average speed */ /* if airspeed is non-finite or not valid or if we are asked not to control it, we assume the normal average speed */
const bool airspeed_valid = PX4_ISFINITE(_sub_airspeed.get().indicated_airspeed_m_s) const bool airspeed_valid = PX4_ISFINITE(_airspeed_sub.get().indicated_airspeed_m_s)
&& (hrt_elapsed_time(&_sub_airspeed.get().timestamp) < 1e6); && (hrt_elapsed_time(&_airspeed_sub.get().timestamp) < 1e6);
if (airspeed_valid) { if (airspeed_valid) {
/* prevent numerical drama by requiring 0.5 m/s minimal speed */ /* prevent numerical drama by requiring 0.5 m/s minimal speed */
airspeed = math::max(0.5f, _sub_airspeed.get().indicated_airspeed_m_s); airspeed = math::max(0.5f, _airspeed_sub.get().indicated_airspeed_m_s);
} else { } else {
airspeed = _parameters.airspeed_trim; airspeed = _parameters.airspeed_trim;

6
src/modules/fw_att_control/FixedwingAttitudeControl.hpp

@ -31,8 +31,6 @@
* *
****************************************************************************/ ****************************************************************************/
#include <controllib/block/BlockParam.hpp>
#include <controllib/blocks.hpp>
#include <px4_module.h> #include <px4_module.h>
#include <drivers/drv_hrt.h> #include <drivers/drv_hrt.h>
#include <ecl/attitude_fw/ecl_pitch_controller.h> #include <ecl/attitude_fw/ecl_pitch_controller.h>
@ -68,7 +66,7 @@ using matrix::Quatf;
using uORB::Subscription; using uORB::Subscription;
class FixedwingAttitudeControl final : public control::SuperBlock, public ModuleBase<FixedwingAttitudeControl> class FixedwingAttitudeControl final : public ModuleBase<FixedwingAttitudeControl>
{ {
public: public:
FixedwingAttitudeControl(); FixedwingAttitudeControl();
@ -124,7 +122,7 @@ private:
vehicle_rates_setpoint_s _rates_sp {}; /* attitude rates setpoint */ vehicle_rates_setpoint_s _rates_sp {}; /* attitude rates setpoint */
vehicle_status_s _vehicle_status {}; /**< vehicle status */ vehicle_status_s _vehicle_status {}; /**< vehicle status */
Subscription<airspeed_s> _sub_airspeed; Subscription<airspeed_s> _airspeed_sub;
perf_counter_t _loop_perf; /**< loop performance counter */ perf_counter_t _loop_perf; /**< loop performance counter */
perf_counter_t _nonfinite_input_perf; /**< performance counter for non finite input */ perf_counter_t _nonfinite_input_perf; /**< performance counter for non finite input */

Loading…
Cancel
Save