From 5648deb5a18cd9c5402d02ff5f3441b4839a3bef Mon Sep 17 00:00:00 2001 From: Thomas Stastny Date: Tue, 14 Jun 2022 03:24:00 -0500 Subject: [PATCH] fw pos ctrl: organize state variables --- .../FixedwingPositionControl.cpp | 6 +- .../FixedwingPositionControl.hpp | 172 +++++++++--------- 2 files changed, 94 insertions(+), 84 deletions(-) diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index ac0e31a295..d7188bf412 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -1518,10 +1518,10 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo /* Perform launch detection */ /* Inform user that launchdetection is running every 4s */ - if ((now - _launch_detection_notify) > 4_s) { + if ((now - _last_time_launch_detection_notified) > 4_s) { mavlink_log_critical(&_mavlink_log_pub, "Launch detection running\t"); events::send(events::ID("fixedwing_position_control_launch_detection"), events::Log::Info, "Launch detection running"); - _launch_detection_notify = now; + _last_time_launch_detection_notified = now; } /* Detect launch using body X (forward) acceleration */ @@ -2465,7 +2465,7 @@ FixedwingPositionControl::reset_takeoff_state() _launchDetector.reset(); _launch_detection_state = LAUNCHDETECTION_RES_NONE; - _launch_detection_notify = 0; + _last_time_launch_detection_notified = 0; _launch_detected = false; } diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp index a346877c0d..22af98eeb0 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp @@ -204,23 +204,57 @@ private: vehicle_local_position_s _local_pos{}; vehicle_status_s _vehicle_status{}; + perf_counter_t _loop_perf; // loop performance counter + + // [us] Last absolute time position control has been called + hrt_abstime _last_time_position_control_called{0}; + + uint8_t _position_sp_type{0}; + + enum FW_POSCTRL_MODE { + FW_POSCTRL_MODE_AUTO, + FW_POSCTRL_MODE_AUTO_ALTITUDE, + FW_POSCTRL_MODE_AUTO_CLIMBRATE, + FW_POSCTRL_MODE_AUTO_TAKEOFF, + FW_POSCTRL_MODE_AUTO_LANDING, + FW_POSCTRL_MODE_MANUAL_POSITION, + FW_POSCTRL_MODE_MANUAL_ALTITUDE, + FW_POSCTRL_MODE_OTHER + } _control_mode_current{FW_POSCTRL_MODE_OTHER}; // used to check if the mode has changed + + enum StickConfig { + STICK_CONFIG_SWAP_STICKS_BIT = (1 << 0), + STICK_CONFIG_ENABLE_AIRSPEED_SP_MANUAL_BIT = (1 << 1) + }; + + // VEHICLE STATES + double _current_latitude{0}; double _current_longitude{0}; float _current_altitude{0.f}; - perf_counter_t _loop_perf; // loop performance counter + float _pitch{0.0f}; + float _yaw{0.0f}; + float _yawrate{0.0f}; + + matrix::Vector3f _body_acceleration{}; + matrix::Vector3f _body_velocity{}; MapProjection _global_local_proj_ref{}; float _global_local_alt0{NAN}; - // [m] ground altitude where the plane was launched - float _takeoff_ground_alt{0.0f}; + bool _landed{true}; - // true if a launch, specifically using the launch detector, has been detected - bool _launch_detected{false}; + // indicates whether the plane was in the air in the previous interation + bool _was_in_air{false}; - // [deg] global position of the vehicle at the time launch is detected (using launch detector) - Vector2d _launch_global_position{0, 0}; + // [us] time at which the plane went in the air + hrt_abstime _time_went_in_air{0}; + + // MANUAL MODES + + // indicates whether we have completed a manual takeoff in a position control mode + bool _completed_manual_takeoff{false}; // [rad] yaw setpoint for manual position mode heading hold float _hdg_hold_yaw{0.0f}; @@ -228,15 +262,43 @@ private: bool _hdg_hold_enabled{false}; // heading hold enabled bool _yaw_lock_engaged{false}; // yaw is locked for heading hold - float _min_current_sp_distance_xy{FLT_MAX}; - position_setpoint_s _hdg_hold_prev_wp{}; // position where heading hold started position_setpoint_s _hdg_hold_curr_wp{}; // position to which heading hold flies - // [us] Last absolute time position control has been called - hrt_abstime _last_time_position_control_called{0}; + // [.] normalized setpoint for manual altitude control [-1,1]; -1,0,1 maps to min,zero,max height rate commands + float _manual_control_setpoint_for_height_rate{0.0f}; - bool _landed{true}; + // [.] normalized setpoint for manual airspeed control [0,1]; 0,0.5,1 maps to min,cruise,max airspeed commands + float _manual_control_setpoint_for_airspeed{0.0f}; + + // [m/s] airspeed setpoint for manual modes commanded via MAV_CMD_DO_CHANGE_SPEED + float _commanded_manual_airspeed_setpoint{NAN}; + + // AUTO TAKEOFF + + // [m] ground altitude where the plane was launched + float _takeoff_ground_alt{0.0f}; + + // class handling launch detection methods for fixed-wing takeoff + LaunchDetector _launchDetector; + + LaunchDetectionResult _launch_detection_state{LAUNCHDETECTION_RES_NONE}; + + // [us] logs the last time the launch detection notification was sent (used not to spam notifications during launch detection) + hrt_abstime _last_time_launch_detection_notified{0}; + + // true if a launch, specifically using the launch detector, has been detected + bool _launch_detected{false}; + + // [deg] global position of the vehicle at the time launch is detected (using launch detector) + Vector2d _launch_global_position{0, 0}; + + // class handling runway takeoff for fixed-wing UAVs with steerable wheels + RunwayTakeoff _runway_takeoff; + + bool _skipping_takeoff_detection{false}; + + // AUTO LANDING /* Landing */ bool _land_noreturn_horizontal{false}; @@ -266,38 +328,17 @@ private: // [m] estimated height to ground at which flare started float _flare_curve_alt_rel_last{0.0f}; - float _target_bearing{0.0f}; // [rad] - - // indicates whether the plane was in the air in the previous interation - bool _was_in_air{false}; - - // [us] time at which the plane went in the air - hrt_abstime _time_went_in_air{0}; - - // indicates whether we have completed a manual takeoff in a position control mode - bool _completed_manual_takeoff{false}; - - // Takeoff launch detection and runway - LaunchDetector _launchDetector; - LaunchDetectionResult _launch_detection_state{LAUNCHDETECTION_RES_NONE}; - hrt_abstime _launch_detection_notify{0}; - - RunwayTakeoff _runway_takeoff; - - bool _skipping_takeoff_detection{false}; - - /* throttle and airspeed states */ + // AIRSPEED + float _airspeed{0.0f}; + float _eas2tas{1.0f}; bool _airspeed_valid{false}; + float _air_density{CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C}; // [us] last time airspeed was received. used to detect timeouts. hrt_abstime _time_airspeed_last_valid{0}; - float _airspeed{0.0f}; - float _eas2tas{1.0f}; - float _air_density{CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C}; - - /* wind estimates */ + // WIND // [m/s] wind velocity vector Vector2f _wind_vel{0.0f, 0.0f}; @@ -306,26 +347,25 @@ private: hrt_abstime _time_wind_last_received{0}; // [us] - float _pitch{0.0f}; - float _yaw{0.0f}; - float _yawrate{0.0f}; + // TECS - matrix::Vector3f _body_acceleration{}; - matrix::Vector3f _body_velocity{}; + // total energy control system - airspeed / altitude control + TECS _tecs; bool _reinitialize_tecs{true}; bool _tecs_is_running{false}; - hrt_abstime _time_last_tecs_update{0}; // [us] + // VTOL / TRANSITION + float _airspeed_after_transition{0.0f}; bool _was_in_transition{false}; - bool _is_vtol_tailsitter{false}; - matrix::Vector2d _transition_waypoint{(double)NAN, (double)NAN}; + param_t _param_handle_airspeed_trans{PARAM_INVALID}; + float _param_airspeed_trans{NAN}; // [m/s] - // estimator reset counters + // ESTIMATOR RESET COUNTERS // captures the number of times the estimator has reset the horizontal position uint8_t _pos_reset_counter{0}; @@ -333,16 +373,7 @@ private: // captures the number of times the estimator has reset the altitude state uint8_t _alt_reset_counter{0}; - // [.] normalized setpoint for manual altitude control [-1,1]; -1,0,1 maps to min,zero,max height rate commands - float _manual_control_setpoint_for_height_rate{0.0f}; - - // [.] normalized setpoint for manual airspeed control [0,1]; 0,0.5,1 maps to min,cruise,max airspeed commands - float _manual_control_setpoint_for_airspeed{0.0f}; - - // [m/s] airspeed setpoint for manual modes commanded via MAV_CMD_DO_CHANGE_SPEED - float _commanded_manual_airspeed_setpoint{NAN}; - - hrt_abstime _time_in_fixed_bank_loiter{0}; // [us] + // LATERAL-DIRECTIONAL GUIDANCE // L1 guidance - lateral-directional position control ECL_L1_Pos_Controller _l1_control; @@ -350,30 +381,9 @@ private: // nonlinear path following guidance - lateral-directional position control NPFG _npfg; - // total energy control system - airspeed / altitude control - TECS _tecs; - - uint8_t _position_sp_type{0}; - - enum FW_POSCTRL_MODE { - FW_POSCTRL_MODE_AUTO, - FW_POSCTRL_MODE_AUTO_ALTITUDE, - FW_POSCTRL_MODE_AUTO_CLIMBRATE, - FW_POSCTRL_MODE_AUTO_TAKEOFF, - FW_POSCTRL_MODE_AUTO_LANDING, - FW_POSCTRL_MODE_MANUAL_POSITION, - FW_POSCTRL_MODE_MANUAL_ALTITUDE, - FW_POSCTRL_MODE_OTHER - } _control_mode_current{FW_POSCTRL_MODE_OTHER}; // used to check if the mode has changed - - param_t _param_handle_airspeed_trans{PARAM_INVALID}; - - float _param_airspeed_trans{NAN}; // [m/s] - - enum StickConfig { - STICK_CONFIG_SWAP_STICKS_BIT = (1 << 0), - STICK_CONFIG_ENABLE_AIRSPEED_SP_MANUAL_BIT = (1 << 1) - }; + hrt_abstime _time_in_fixed_bank_loiter{0}; // [us] + float _min_current_sp_distance_xy{FLT_MAX}; + float _target_bearing{0.0f}; // [rad] // Update our local parameter cache. int parameters_update();