From 4416c4ddb3ef84a958b023f50b066c0c3f1d87d7 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Thu, 7 Sep 2017 13:14:58 -0400 Subject: [PATCH] navigator move parameters out of MissionBlock - MissionBlock is an interface with > 10 implementations --- msg/position_setpoint.msg | 11 +- .../fw_att_control/fw_att_control_main.cpp | 10 +- .../FixedwingPositionControl.cpp | 13 +- .../fw_pos_control_l1_params.c | 14 ++ .../mc_att_control/mc_att_control_main.cpp | 222 +++++++----------- .../mc_pos_control/mc_pos_control_main.cpp | 25 +- src/modules/navigator/follow_target.cpp | 34 +++ src/modules/navigator/follow_target.h | 6 + src/modules/navigator/loiter.cpp | 3 +- src/modules/navigator/loiter.h | 1 - src/modules/navigator/mission.cpp | 8 +- src/modules/navigator/mission.h | 1 - src/modules/navigator/mission_block.cpp | 86 ++----- src/modules/navigator/mission_block.h | 15 +- src/modules/navigator/mission_params.c | 38 --- src/modules/navigator/navigation.h | 1 - src/modules/navigator/navigator.h | 10 +- src/modules/navigator/navigator_main.cpp | 12 +- src/modules/navigator/rtl.cpp | 1 - src/modules/navigator/rtl.h | 1 - src/modules/vtol_att_control/standard.cpp | 3 +- .../vtol_att_control_main.cpp | 31 ++- .../vtol_att_control/vtol_att_control_main.h | 8 +- .../vtol_att_control_params.c | 52 ++-- src/modules/vtol_att_control/vtol_type.cpp | 23 +- src/modules/vtol_att_control/vtol_type.h | 14 +- 26 files changed, 291 insertions(+), 352 deletions(-) diff --git a/msg/position_setpoint.msg b/msg/position_setpoint.msg index c3ff1c095b..29a83ed715 100644 --- a/msg/position_setpoint.msg +++ b/msg/position_setpoint.msg @@ -14,33 +14,40 @@ uint8 VELOCITY_FRAME_BODY_NED = 8 # MAV_FRAME_BODY_NED bool valid # true if setpoint is valid uint8 type # setpoint type to adjust behavior of position controller + float32 x # local position setpoint in m in NED float32 y # local position setpoint in m in NED float32 z # local position setpoint in m in NED bool position_valid # true if local position setpoint valid + float32 vx # local velocity setpoint in m/s in NED float32 vy # local velocity setpoint in m/s in NED float32 vz # local velocity setpoint in m/s in NED bool velocity_valid # true if local velocity setpoint valid uint8 velocity_frame # to set velocity setpoints in NED or body - bool alt_valid # do not set for 3D position control. Set to true if you want z-position control while doing vx,vy velocity control. + float64 lat # latitude, in deg float64 lon # longitude, in deg float32 alt # altitude AMSL, in m + float32 yaw # yaw (only for multirotors), in rad [-PI..PI), NaN = hold current yaw bool yaw_valid # true if yaw setpoint valid -bool disable_mc_yaw_control # control yaw for mc (used for vtol weather-vane mode) + float32 yawspeed # yawspeed (only for multirotors, in rad/s) bool yawspeed_valid # true if yawspeed setpoint valid + float32 loiter_radius # loiter radius (only for fixed wing), in m int8 loiter_direction # loiter direction: 1 = CW, -1 = CCW float32 pitch_min # minimal pitch angle for fixed wing takeoff waypoints + float32 a_x # acceleration x setpoint float32 a_y # acceleration y setpoint float32 a_z # acceleration z setpoint bool acceleration_valid # true if acceleration setpoint is valid/should be used bool acceleration_is_force # interprete acceleration as force + float32 acceptance_radius # navigation acceptance_radius if we're doing waypoint navigation + float32 cruising_speed # the generally desired cruising speed (not a hard constraint) float32 cruising_throttle # the generally desired cruising throttle (not a hard constraint) 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 eb2563edbe..cedeacfe70 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -460,8 +460,6 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _parameter_handles.rattitude_thres = param_find("FW_RATT_TH"); - _parameter_handles.vtol_type = param_find("VT_TYPE"); - _parameter_handles.bat_scale_en = param_find("FW_BAT_SCALE_EN"); /* fetch initial parameter values */ @@ -567,7 +565,9 @@ FixedwingAttitudeControl::parameters_update() param_get(_parameter_handles.rattitude_thres, &_parameters.rattitude_thres); - param_get(_parameter_handles.vtol_type, &_parameters.vtol_type); + if (_vehicle_status.is_vtol) { + param_get(_parameter_handles.vtol_type, &_parameters.vtol_type); + } param_get(_parameter_handles.bat_scale_en, &_parameters.bat_scale_en); @@ -676,6 +676,10 @@ FixedwingAttitudeControl::vehicle_status_poll() _actuators_id = ORB_ID(actuator_controls_virtual_fw); _attitude_setpoint_id = ORB_ID(fw_virtual_attitude_setpoint); + _parameter_handles.vtol_type = param_find("VT_TYPE"); + + parameters_update(); + } else { _rates_sp_id = ORB_ID(vehicle_rates_setpoint); _actuators_id = ORB_ID(actuator_controls_0); diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index c9f1467e3e..886e660d76 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -49,7 +49,6 @@ FixedwingPositionControl::FixedwingPositionControl() : _parameter_handles.airspeed_min = param_find("FW_AIRSPD_MIN"); _parameter_handles.airspeed_trim = param_find("FW_AIRSPD_TRIM"); _parameter_handles.airspeed_max = param_find("FW_AIRSPD_MAX"); - _parameter_handles.airspeed_trans = param_find("VT_ARSP_TRANS"); _parameter_handles.airspeed_disabled = param_find("FW_ARSP_MODE"); _parameter_handles.pitch_limit_min = param_find("FW_P_LIM_MIN"); @@ -94,8 +93,6 @@ FixedwingPositionControl::FixedwingPositionControl() : _parameter_handles.heightrate_ff = param_find("FW_T_HRATE_FF"); _parameter_handles.speedrate_p = param_find("FW_T_SRATE_P"); - _parameter_handles.vtol_type = param_find("VT_TYPE"); - /* fetch initial parameter values */ parameters_update(); } @@ -135,7 +132,6 @@ FixedwingPositionControl::parameters_update() param_get(_parameter_handles.airspeed_min, &(_parameters.airspeed_min)); param_get(_parameter_handles.airspeed_trim, &(_parameters.airspeed_trim)); param_get(_parameter_handles.airspeed_max, &(_parameters.airspeed_max)); - param_get(_parameter_handles.airspeed_trans, &(_parameters.airspeed_trans)); param_get(_parameter_handles.airspeed_disabled, &(_parameters.airspeed_disabled)); param_get(_parameter_handles.pitch_limit_min, &(_parameters.pitch_limit_min)); @@ -194,7 +190,6 @@ FixedwingPositionControl::parameters_update() param_get(_parameter_handles.land_flare_pitch_max_deg, &(_parameters.land_flare_pitch_max_deg)); param_get(_parameter_handles.land_use_terrain_estimate, &(_parameters.land_use_terrain_estimate)); param_get(_parameter_handles.land_airspeed_scale, &(_parameters.land_airspeed_scale)); - param_get(_parameter_handles.vtol_type, &(_parameters.vtol_type)); _l1_control.set_l1_damping(_parameters.l1_damping); _l1_control.set_l1_period(_parameters.l1_period); @@ -288,6 +283,11 @@ FixedwingPositionControl::vehicle_status_poll() if (_vehicle_status.is_vtol) { _attitude_setpoint_id = ORB_ID(fw_virtual_attitude_setpoint); + _parameter_handles.airspeed_trans = param_find("VT_ARSP_TRANS"); + _parameter_handles.vtol_type = param_find("VT_TYPE"); + + parameters_update(); + } else { _attitude_setpoint_id = ORB_ID(vehicle_attitude_setpoint); } @@ -1059,7 +1059,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> &curr_pos, cons if (_runway_takeoff.runwayTakeoffEnabled()) { if (!_runway_takeoff.isInitialized()) { - _runway_takeoff.init(_yaw, _global_pos.lat, _global_pos.lon); + Eulerf euler(Quatf(_att.q)); + _runway_takeoff.init(euler.psi(), _global_pos.lat, _global_pos.lon); /* need this already before takeoff is detected * doesn't matter if it gets reset when takeoff is detected eventually */ diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c index a6fcd5d108..7b5e713c6f 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c @@ -382,6 +382,20 @@ PARAM_DEFINE_FLOAT(FW_AIRSPD_MIN, 10.0f); */ PARAM_DEFINE_FLOAT(FW_AIRSPD_MAX, 20.0f); +/** + * Cruise Airspeed + * + * The fixed wing controller tries to fly at this airspeed. + * + * @unit m/s + * @min 0.0 + * @max 40 + * @decimal 1 + * @increment 0.5 + * @group FW TECS + */ +PARAM_DEFINE_FLOAT(FW_AIRSPD_TRIM, 15.0f); + /** * Maximum climb rate * diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 810e561064..09da01a40c 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -68,7 +68,6 @@ #include #include #include -#include #include #include #include @@ -134,7 +133,6 @@ private: int _v_control_mode_sub; /**< vehicle control mode subscription */ int _params_sub; /**< parameter updates subscription */ int _manual_control_sp_sub; /**< manual control setpoint subscription */ - int _armed_sub; /**< arming status subscription */ int _vehicle_status_sub; /**< vehicle status subscription */ int _motor_limits_sub; /**< motor limits subscription */ int _battery_status_sub; /**< battery status subscription */ @@ -155,18 +153,17 @@ private: bool _actuators_0_circuit_breaker_enabled; /**< circuit breaker to suppress output */ struct vehicle_attitude_s _v_att; /**< vehicle attitude */ - struct vehicle_attitude_setpoint_s _v_att_sp; /**< vehicle attitude setpoint */ + struct vehicle_attitude_setpoint_s _v_att_sp; /**< vehicle attitude setpoint */ struct vehicle_rates_setpoint_s _v_rates_sp; /**< vehicle rates setpoint */ struct manual_control_setpoint_s _manual_control_sp; /**< manual control setpoint */ struct vehicle_control_mode_s _v_control_mode; /**< vehicle control mode */ - struct actuator_controls_s _actuators; /**< actuator controls */ - struct actuator_armed_s _armed; /**< actuator arming status */ - struct vehicle_status_s _vehicle_status; /**< vehicle status */ - struct mc_att_ctrl_status_s _controller_status; /**< controller status */ - struct battery_status_s _battery_status; /**< battery status */ + struct actuator_controls_s _actuators; /**< actuator controls */ + struct vehicle_status_s _vehicle_status; /**< vehicle status */ + struct mc_att_ctrl_status_s _controller_status; /**< controller status */ + struct battery_status_s _battery_status; /**< battery status */ struct sensor_gyro_s _sensor_gyro; /**< gyro data before thermal correctons and ekf bias estimates are applied */ - struct sensor_correction_s _sensor_correction; /**< sensor thermal corrections */ - struct sensor_bias_s _sensor_bias; /**< sensor in-run bias corrections */ + struct sensor_correction_s _sensor_correction; /**< sensor thermal corrections */ + struct sensor_bias_s _sensor_bias; /**< sensor in-run bias corrections */ MultirotorMixer::saturation_status _saturation_status{}; @@ -222,9 +219,10 @@ private: param_t acro_superexpo; param_t rattitude_thres; - param_t vtol_type; param_t roll_tc; param_t pitch_tc; + + param_t vtol_type; param_t vtol_opt_recovery_enabled; param_t vtol_wv_yaw_rate_scale; @@ -262,7 +260,8 @@ private: float acro_expo; /**< function parameter for expo stick curve shape */ float acro_superexpo; /**< function parameter for superexpo stick curve shape */ float rattitude_thres; - int vtol_type; /**< 0 = Tailsitter, 1 = Tiltrotor, 2 = Standard airframe */ + + int32_t vtol_type; /**< 0 = Tailsitter, 1 = Tiltrotor, 2 = Standard airframe */ bool vtol_opt_recovery_enabled; float vtol_wv_yaw_rate_scale; /**< Scale value [0, 1] for yaw rate setpoint */ @@ -274,42 +273,27 @@ private: } _params; - TailsitterRecovery *_ts_opt_recovery; /**< Computes optimal rates for tailsitter recovery */ + TailsitterRecovery *_ts_opt_recovery{nullptr}; /**< Computes optimal rates for tailsitter recovery */ /** * Update our local parameter cache. */ - int parameters_update(); + void parameters_update(); /** * Check for parameter update and handle it. */ + void battery_status_poll(); void parameter_update_poll(); - - /** - * Check for changes in vehicle control mode. - */ - void vehicle_control_mode_poll(); - - /** - * Check for changes in manual inputs. - */ - void vehicle_manual_poll(); - - /** - * Check for attitude setpoint updates. - */ + void sensor_bias_poll(); + void sensor_correction_poll(); + void vehicle_attitude_poll(); void vehicle_attitude_setpoint_poll(); - - /** - * Check for rates setpoint updates. - */ + void vehicle_control_mode_poll(); + void vehicle_manual_poll(); + void vehicle_motor_limits_poll(); void vehicle_rates_setpoint_poll(); - - /** - * Check for arming status updates. - */ - void arming_status_poll(); + void vehicle_status_poll(); /** * Attitude controller. @@ -326,36 +310,6 @@ private: */ math::Vector<3> pid_attenuations(float tpa_breakpoint, float tpa_rate); - /** - * Check for vehicle status updates. - */ - void vehicle_status_poll(); - - /** - * Check for vehicle motor limits status. - */ - void vehicle_motor_limits_poll(); - - /** - * Check for battery status updates. - */ - void battery_status_poll(); - - /** - * Check for control state updates. - */ - void vehicle_attitude_poll(); - - /** - * Check for sensor thermal correction updates. - */ - void sensor_correction_poll(); - - /** - * Check for sensor in-run bias correction updates. - */ - void sensor_bias_poll(); - /** * Shim for calling task_main from task_create. */ @@ -384,7 +338,6 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _v_control_mode_sub(-1), _params_sub(-1), _manual_control_sp_sub(-1), - _armed_sub(-1), _vehicle_status_sub(-1), _motor_limits_sub(-1), _battery_status_sub(-1), @@ -410,7 +363,6 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _manual_control_sp{}, _v_control_mode{}, _actuators{}, - _armed{}, _vehicle_status{}, _controller_status{}, _battery_status{}, @@ -420,9 +372,7 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _saturation_status{}, /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control")), - _controller_latency_perf(perf_alloc_once(PC_ELAPSED, "ctrl_latency")), - _ts_opt_recovery(nullptr) - + _controller_latency_perf(perf_alloc_once(PC_ELAPSED, "ctrl_latency")) { for (uint8_t i = 0; i < MAX_GYRO_COUNT; i++) { _sensor_gyro_sub[i] = -1; @@ -444,8 +394,7 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _params.auto_rate_max.zero(); _params.acro_rate_max.zero(); _params.rattitude_thres = 1.0f; - _params.vtol_opt_recovery_enabled = false; - _params.vtol_wv_yaw_rate_scale = 1.0f; + _params.bat_scale_en = 0; _params.board_rotation = 0; @@ -469,19 +418,22 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _params_handles.roll_rate_i = param_find("MC_ROLLRATE_I"); _params_handles.roll_rate_integ_lim = param_find("MC_RR_INT_LIM"); _params_handles.roll_rate_d = param_find("MC_ROLLRATE_D"); - _params_handles.roll_rate_ff = param_find("MC_ROLLRATE_FF"); + _params_handles.roll_rate_ff = param_find("MC_ROLLRATE_FF"); + _params_handles.pitch_p = param_find("MC_PITCH_P"); - _params_handles.pitch_rate_p = param_find("MC_PITCHRATE_P"); - _params_handles.pitch_rate_i = param_find("MC_PITCHRATE_I"); + _params_handles.pitch_rate_p = param_find("MC_PITCHRATE_P"); + _params_handles.pitch_rate_i = param_find("MC_PITCHRATE_I"); _params_handles.pitch_rate_integ_lim = param_find("MC_PR_INT_LIM"); - _params_handles.pitch_rate_d = param_find("MC_PITCHRATE_D"); - _params_handles.pitch_rate_ff = param_find("MC_PITCHRATE_FF"); + _params_handles.pitch_rate_d = param_find("MC_PITCHRATE_D"); + _params_handles.pitch_rate_ff = param_find("MC_PITCHRATE_FF"); + _params_handles.tpa_breakpoint_p = param_find("MC_TPA_BREAK_P"); _params_handles.tpa_breakpoint_i = param_find("MC_TPA_BREAK_I"); _params_handles.tpa_breakpoint_d = param_find("MC_TPA_BREAK_D"); _params_handles.tpa_rate_p = param_find("MC_TPA_RATE_P"); _params_handles.tpa_rate_i = param_find("MC_TPA_RATE_I"); _params_handles.tpa_rate_d = param_find("MC_TPA_RATE_D"); + _params_handles.yaw_p = param_find("MC_YAW_P"); _params_handles.yaw_rate_p = param_find("MC_YAWRATE_P"); _params_handles.yaw_rate_i = param_find("MC_YAWRATE_I"); @@ -489,41 +441,34 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _params_handles.yaw_rate_d = param_find("MC_YAWRATE_D"); _params_handles.yaw_rate_ff = param_find("MC_YAWRATE_FF"); _params_handles.yaw_ff = param_find("MC_YAW_FF"); - _params_handles.roll_rate_max = param_find("MC_ROLLRATE_MAX"); - _params_handles.pitch_rate_max = param_find("MC_PITCHRATE_MAX"); - _params_handles.yaw_rate_max = param_find("MC_YAWRATE_MAX"); - _params_handles.yaw_auto_max = param_find("MC_YAWRAUTO_MAX"); - _params_handles.acro_roll_max = param_find("MC_ACRO_R_MAX"); - _params_handles.acro_pitch_max = param_find("MC_ACRO_P_MAX"); - _params_handles.acro_yaw_max = param_find("MC_ACRO_Y_MAX"); - _params_handles.acro_expo = param_find("MC_ACRO_EXPO"); - _params_handles.acro_superexpo = param_find("MC_ACRO_SUPEXPO"); - _params_handles.rattitude_thres = param_find("MC_RATT_TH"); - _params_handles.vtol_type = param_find("VT_TYPE"); + + _params_handles.roll_rate_max = param_find("MC_ROLLRATE_MAX"); + _params_handles.pitch_rate_max = param_find("MC_PITCHRATE_MAX"); + _params_handles.yaw_rate_max = param_find("MC_YAWRATE_MAX"); + _params_handles.yaw_auto_max = param_find("MC_YAWRAUTO_MAX"); + + _params_handles.acro_roll_max = param_find("MC_ACRO_R_MAX"); + _params_handles.acro_pitch_max = param_find("MC_ACRO_P_MAX"); + _params_handles.acro_yaw_max = param_find("MC_ACRO_Y_MAX"); + + _params_handles.rattitude_thres = param_find("MC_RATT_TH"); + _params_handles.roll_tc = param_find("MC_ROLL_TC"); _params_handles.pitch_tc = param_find("MC_PITCH_TC"); - _params_handles.vtol_opt_recovery_enabled = param_find("VT_OPT_RECOV_EN"); - _params_handles.vtol_wv_yaw_rate_scale = param_find("VT_WV_YAWR_SCL"); - _params_handles.bat_scale_en = param_find("MC_BAT_SCALE_EN"); + + _params_handles.bat_scale_en = param_find("MC_BAT_SCALE_EN"); /* rotations */ - _params_handles.board_rotation = param_find("SENS_BOARD_ROT"); + _params_handles.board_rotation = param_find("SENS_BOARD_ROT"); /* rotation offsets */ - _params_handles.board_offset[0] = param_find("SENS_BOARD_X_OFF"); - _params_handles.board_offset[1] = param_find("SENS_BOARD_Y_OFF"); - _params_handles.board_offset[2] = param_find("SENS_BOARD_Z_OFF"); - - + _params_handles.board_offset[0] = param_find("SENS_BOARD_X_OFF"); + _params_handles.board_offset[1] = param_find("SENS_BOARD_Y_OFF"); + _params_handles.board_offset[2] = param_find("SENS_BOARD_Z_OFF"); /* fetch initial parameter values */ parameters_update(); - if (_params.vtol_type == 0 && _params.vtol_opt_recovery_enabled) { - // the vehicle is a tailsitter, use optimal recovery control strategy - _ts_opt_recovery = new TailsitterRecovery(); - } - /* initialize thermal corrections as we might not immediately get a topic update (only non-zero values) */ for (unsigned i = 0; i < 3; i++) { // used scale factors to unity @@ -531,7 +476,6 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _sensor_correction.gyro_scale_1[i] = 1.0f; _sensor_correction.gyro_scale_2[i] = 1.0f; } - } MulticopterAttitudeControl::~MulticopterAttitudeControl() @@ -555,14 +499,12 @@ MulticopterAttitudeControl::~MulticopterAttitudeControl() } while (_control_task != -1); } - if (_ts_opt_recovery != nullptr) { - delete _ts_opt_recovery; - } + delete _ts_opt_recovery; mc_att_control::g_control = nullptr; } -int +void MulticopterAttitudeControl::parameters_update() { float v; @@ -652,13 +594,15 @@ MulticopterAttitudeControl::parameters_update() /* stick deflection needed in rattitude mode to control rates not angles */ param_get(_params_handles.rattitude_thres, &_params.rattitude_thres); - param_get(_params_handles.vtol_type, &_params.vtol_type); + if (_vehicle_status.is_vtol) { + param_get(_params_handles.vtol_type, &_params.vtol_type); - int tmp; - param_get(_params_handles.vtol_opt_recovery_enabled, &tmp); - _params.vtol_opt_recovery_enabled = (bool)tmp; + int32_t tmp; + param_get(_params_handles.vtol_opt_recovery_enabled, &tmp); + _params.vtol_opt_recovery_enabled = (tmp == 1); - param_get(_params_handles.vtol_wv_yaw_rate_scale, &_params.vtol_wv_yaw_rate_scale); + param_get(_params_handles.vtol_wv_yaw_rate_scale, &_params.vtol_wv_yaw_rate_scale); + } param_get(_params_handles.bat_scale_en, &_params.bat_scale_en); @@ -672,7 +616,6 @@ MulticopterAttitudeControl::parameters_update() param_get(_params_handles.board_offset[1], &(_params.board_offset[1])); param_get(_params_handles.board_offset[2], &(_params.board_offset[2])); - /* get transformation matrix from sensor/board to body frame */ get_rot_matrix((enum Rotation)_params.board_rotation, &_board_rotation); @@ -682,8 +625,6 @@ MulticopterAttitudeControl::parameters_update() M_DEG_TO_RAD_F * _params.board_offset[1], M_DEG_TO_RAD_F * _params.board_offset[2]); _board_rotation = board_rotation_offset * _board_rotation; - - return OK; } void @@ -751,18 +692,6 @@ MulticopterAttitudeControl::vehicle_rates_setpoint_poll() } } -void -MulticopterAttitudeControl::arming_status_poll() -{ - /* check if there is a new setpoint */ - bool updated; - orb_check(_armed_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed); - } -} - void MulticopterAttitudeControl::vehicle_status_poll() { @@ -774,11 +703,22 @@ MulticopterAttitudeControl::vehicle_status_poll() orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status); /* set correct uORB ID, depending on if vehicle is VTOL or not */ - if (!_rates_sp_id) { + if (_rates_sp_id == nullptr) { if (_vehicle_status.is_vtol) { _rates_sp_id = ORB_ID(mc_virtual_rates_setpoint); _actuators_id = ORB_ID(actuator_controls_virtual_mc); + _params_handles.vtol_type = param_find("VT_TYPE"); + _params_handles.vtol_opt_recovery_enabled = param_find("VT_OPT_RECOV_EN"); + _params_handles.vtol_wv_yaw_rate_scale = param_find("VT_WV_YAWR_SCL"); + + parameters_update(); + + if (_params.vtol_type == 0 && _params.vtol_opt_recovery_enabled) { + // the vehicle is a tailsitter, use optimal recovery control strategy + _ts_opt_recovery = new TailsitterRecovery(); + } + } else { _rates_sp_id = ORB_ID(vehicle_rates_setpoint); _actuators_id = ORB_ID(actuator_controls_0); @@ -970,13 +910,16 @@ MulticopterAttitudeControl::control_attitude(float dt) } } - /* weather-vane mode, dampen yaw rate */ - if ((_v_control_mode.flag_control_velocity_enabled || _v_control_mode.flag_control_auto_enabled) && - _v_att_sp.disable_mc_yaw_control == true && !_v_control_mode.flag_control_manual_enabled) { - float wv_yaw_rate_max = _params.auto_rate_max(2) * _params.vtol_wv_yaw_rate_scale; - _rates_sp(2) = math::constrain(_rates_sp(2), -wv_yaw_rate_max, wv_yaw_rate_max); - // prevent integrator winding up in weathervane mode - _rates_int(2) = 0.0f; + /* VTOL weather-vane mode, dampen yaw rate */ + if (_vehicle_status.is_vtol && _v_att_sp.disable_mc_yaw_control) { + if (_v_control_mode.flag_control_velocity_enabled || _v_control_mode.flag_control_auto_enabled) { + + const float wv_yaw_rate_max = _params.auto_rate_max(2) * _params.vtol_wv_yaw_rate_scale; + _rates_sp(2) = math::constrain(_rates_sp(2), -wv_yaw_rate_max, wv_yaw_rate_max); + + // prevent integrator winding up in weathervane mode + _rates_int(2) = 0.0f; + } } } @@ -1010,7 +953,7 @@ void MulticopterAttitudeControl::control_attitude_rates(float dt) { /* reset integral if disarmed */ - if (!_armed.armed || !_vehicle_status.is_rotary_wing) { + if (!_v_control_mode.flag_armed || !_vehicle_status.is_rotary_wing) { _rates_int.zero(); } @@ -1124,7 +1067,6 @@ MulticopterAttitudeControl::task_main() _v_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); - _armed_sub = orb_subscribe(ORB_ID(actuator_armed)); _vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); _motor_limits_sub = orb_subscribe(ORB_ID(multirotor_motor_limits)); _battery_status_sub = orb_subscribe(ORB_ID(battery_status)); @@ -1191,7 +1133,6 @@ MulticopterAttitudeControl::task_main() /* check for updates in other topics */ parameter_update_poll(); vehicle_control_mode_poll(); - arming_status_poll(); vehicle_manual_poll(); vehicle_status_poll(); vehicle_motor_limits_poll(); @@ -1334,7 +1275,6 @@ MulticopterAttitudeControl::task_main() _thrust_sp = 0.0f; _att_control.zero(); - /* publish actuator controls */ _actuators.control[0] = 0.0f; _actuators.control[1] = 0.0f; diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index fe65cd0391..b6917ced4a 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -254,7 +254,9 @@ private: float slow_land_alt1; float slow_land_alt2; int32_t alt_mode; - int32_t opt_recover; + + bool opt_recover; + float rc_flt_smp_rate; float rc_flt_cutoff; @@ -528,7 +530,6 @@ MulticopterPositionControl::MulticopterPositionControl() : _params_handles.hold_max_xy = param_find("MPC_HOLD_MAX_XY"); _params_handles.hold_max_z = param_find("MPC_HOLD_MAX_Z"); _params_handles.alt_mode = param_find("MPC_ALT_MODE"); - _params_handles.opt_recover = param_find("VT_OPT_RECOV_EN"); _params_handles.rc_flt_cutoff = param_find("RC_FLT_CUTOFF"); _params_handles.rc_flt_smp_rate = param_find("RC_FLT_SMP_RATE"); @@ -655,7 +656,11 @@ MulticopterPositionControl::parameters_update(bool force) param_get(_params_handles.alt_mode, &v_i); _params.alt_mode = v_i; - param_get(_params_handles.opt_recover, &_params.opt_recover); + if (_vehicle_status.is_vtol) { + int32_t i = 0; + param_get(_params_handles.opt_recover, &i); + _params.opt_recover = (i == 1); + } /* mc attitude control parameters*/ /* manual control scale */ @@ -714,6 +719,9 @@ MulticopterPositionControl::poll_subscriptions() if (_vehicle_status.is_vtol) { _attitude_setpoint_id = ORB_ID(mc_virtual_attitude_setpoint); + _params_handles.opt_recover = param_find("VT_OPT_RECOV_EN"); + parameters_update(true); + } else { _attitude_setpoint_id = ORB_ID(vehicle_attitude_setpoint); } @@ -1464,14 +1472,6 @@ MulticopterPositionControl::control_non_manual(float dt) control_auto(dt); } - /* weather-vane mode for vtol: disable yaw control */ - if (_vehicle_status.is_vtol) { - _att_sp.disable_mc_yaw_control = _pos_sp_triplet.current.disable_mc_yaw_control; - - } else { - _att_sp.disable_mc_yaw_control = false; - } - // guard against any bad velocity values bool velocity_valid = PX4_ISFINITE(_pos_sp_triplet.current.vx) && PX4_ISFINITE(_pos_sp_triplet.current.vy) && @@ -2880,7 +2880,7 @@ MulticopterPositionControl::generate_attitude_setpoint(float dt) _att_sp.yaw_body += euler_sp(2); /* only if optimal recovery is not used, modify roll/pitch */ - if (_params.opt_recover <= 0) { + if (!(_vehicle_status.is_vtol && _params.opt_recover)) { // construct attitude setpoint rotation matrix. modify the setpoints for roll // and pitch such that they reflect the user's intention even if a yaw error // (yaw_sp - yaw) is present. In the presence of a yaw error constructing a rotation matrix @@ -3014,7 +3014,6 @@ MulticopterPositionControl::task_main() /* set default max velocity in xy to vel_max */ _vel_max_xy = _params.vel_max_xy; - /* reset flags when landed */ if (_vehicle_land_detected.landed) { _reset_pos_sp = true; diff --git a/src/modules/navigator/follow_target.cpp b/src/modules/navigator/follow_target.cpp index c91a9798a7..b9bb2d6f6e 100644 --- a/src/modules/navigator/follow_target.cpp +++ b/src/modules/navigator/follow_target.cpp @@ -400,3 +400,37 @@ bool FollowTarget::target_position_valid() // need at least 1 continuous data points for position estimate return (_target_updates >= 1); } + +void +FollowTarget::set_follow_target_item(struct mission_item_s *item, float min_clearance, follow_target_s &target, + float yaw) +{ + if (_navigator->get_land_detected()->landed) { + /* landed, don't takeoff, but switch to IDLE mode */ + item->nav_cmd = NAV_CMD_IDLE; + + } else { + + item->nav_cmd = NAV_CMD_DO_FOLLOW_REPOSITION; + + /* use current target position */ + item->lat = target.lat; + item->lon = target.lon; + item->altitude = _navigator->get_home_position()->alt; + + if (min_clearance > 8.0f) { + item->altitude += min_clearance; + + } else { + item->altitude += 8.0f; // if min clearance is bad set it to 8.0 meters (well above the average height of a person) + } + } + + item->altitude_is_relative = false; + item->yaw = yaw; + item->loiter_radius = _navigator->get_loiter_radius(); + item->acceptance_radius = _navigator->get_acceptance_radius(); + item->time_inside = 0.0f; + item->autocontinue = false; + item->origin = ORIGIN_ONBOARD; +} diff --git a/src/modules/navigator/follow_target.h b/src/modules/navigator/follow_target.h index d38feafe2b..eae6eaa1b9 100644 --- a/src/modules/navigator/follow_target.h +++ b/src/modules/navigator/follow_target.h @@ -46,6 +46,7 @@ #include #include "navigator_mode.h" #include "mission_block.h" +#include class FollowTarget : public MissionBlock { @@ -162,4 +163,9 @@ private: void update_position_sp(bool velocity_valid, bool position_valid, float yaw_rate); void update_target_motion(); void update_target_velocity(); + + /** + * Set follow_target item + */ + void set_follow_target_item(struct mission_item_s *item, float min_clearance, follow_target_s &target, float yaw); }; diff --git a/src/modules/navigator/loiter.cpp b/src/modules/navigator/loiter.cpp index aefb6b5d3f..f0d356e86a 100644 --- a/src/modules/navigator/loiter.cpp +++ b/src/modules/navigator/loiter.cpp @@ -58,7 +58,6 @@ Loiter::Loiter(Navigator *navigator, const char *name) : MissionBlock(navigator, name), - _param_min_alt(this, "MIS_LTRMIN_ALT", false), _param_yawmode(this, "MIS_YAWMODE", false), _loiter_pos_set(false) { @@ -123,7 +122,7 @@ Loiter::set_loiter_position() _loiter_pos_set = true; // set current mission item to loiter - set_loiter_item(&_mission_item, _param_min_alt.get()); + set_loiter_item(&_mission_item, _navigator->get_loiter_min_alt()); // convert mission item to current setpoint struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); diff --git a/src/modules/navigator/loiter.h b/src/modules/navigator/loiter.h index 824f294280..b347549935 100644 --- a/src/modules/navigator/loiter.h +++ b/src/modules/navigator/loiter.h @@ -80,7 +80,6 @@ private: */ void set_loiter_position(); - control::BlockParamFloat _param_min_alt; control::BlockParamInt _param_yawmode; bool _loiter_pos_set; }; diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 0ff7c0cf4a..653b059f47 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -66,7 +66,6 @@ Mission::Mission(Navigator *navigator, const char *name) : _param_dist_between_wps(this, "MIS_DIST_WPS", false), _param_altmode(this, "MIS_ALTMODE", false), _param_yawmode(this, "MIS_YAWMODE", false), - _param_force_vtol(this, "NAV_FORCE_VT", false), _param_fw_climbout_diff(this, "FW_CLMBOUT_DIFF", false), _missionFeasibilityChecker(navigator) { @@ -545,9 +544,7 @@ Mission::set_mission_items() if (item_contains_position(_mission_item)) { /* force vtol land */ - if (_mission_item.nav_cmd == NAV_CMD_LAND && _param_force_vtol.get() == 1 - && !_navigator->get_vstatus()->is_rotary_wing - && _navigator->get_vstatus()->is_vtol) { + if (_navigator->force_vtol() && _mission_item.nav_cmd == NAV_CMD_LAND) { _mission_item.nav_cmd = NAV_CMD_VTOL_LAND; } @@ -732,7 +729,6 @@ Mission::set_mission_items() _mission_item.nav_cmd = NAV_CMD_WAYPOINT; _mission_item.autocontinue = true; _mission_item.time_inside = 0.0f; - _mission_item.disable_mc_yaw = true; } /* we just moved to the landing waypoint, now descend */ @@ -1176,7 +1172,7 @@ Mission::do_abort_landing() // or 2 * FW_CLMBOUT_DIFF above the current altitude float alt_landing = get_absolute_altitude_for_item(_mission_item); float min_climbout = _navigator->get_global_position()->alt + (2 * _param_fw_climbout_diff.get()); - float alt_sp = math::max(alt_landing + _param_loiter_min_alt.get(), min_climbout); + float alt_sp = math::max(alt_landing + _navigator->get_loiter_min_alt(), min_climbout); // turn current landing waypoint into an indefinite loiter _mission_item.nav_cmd = NAV_CMD_LOITER_UNLIMITED; diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index 2c28d7ae7a..1652e98965 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -242,7 +242,6 @@ private: control::BlockParamFloat _param_dist_between_wps; control::BlockParamInt _param_altmode; control::BlockParamInt _param_yawmode; - control::BlockParamInt _param_force_vtol; control::BlockParamFloat _param_fw_climbout_diff; struct mission_s _onboard_mission {}; diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index c288aad6ed..107866180c 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -56,13 +56,8 @@ MissionBlock::MissionBlock(Navigator *navigator, const char *name) : NavigatorMode(navigator, name), - _param_loiter_min_alt(this, "MIS_LTRMIN_ALT", false), _param_yaw_timeout(this, "MIS_YAW_TMT", false), _param_yaw_err(this, "MIS_YAW_ERR", false), - _param_vtol_wv_land(this, "VT_WV_LND_EN", false), - _param_vtol_wv_takeoff(this, "VT_WV_TKO_EN", false), - _param_vtol_wv_loiter(this, "VT_WV_LTR_EN", false), - _param_force_vtol(this, "NAV_FORCE_VT", false), _param_back_trans_dec_mss(this, "VT_B_DEC_MSS", false), _param_reverse_delay(this, "VT_B_REV_DEL", false) { @@ -422,19 +417,21 @@ MissionBlock::issue_command(const mission_item_s &item) } if (item.nav_cmd == NAV_CMD_DO_SET_SERVO) { - PX4_INFO("do_set_servo command"); + PX4_INFO("DO_SET_SERVO command"); + // XXX: we should issue a vehicle command and handle this somewhere else - _actuators = {}; + actuator_controls_s actuators = {}; + actuators.timestamp = hrt_absolute_time(); + // params[0] actuator number to be set 0..5 (corresponds to AUX outputs 1..6) // params[1] new value for selected actuator in ms 900...2000 - _actuators.control[(int)item.params[0]] = 1.0f / 2000 * -item.params[1]; - _actuators.timestamp = hrt_absolute_time(); + actuators.control[(int)item.params[0]] = 1.0f / 2000 * -item.params[1]; if (_actuator_pub != nullptr) { - orb_publish(ORB_ID(actuator_controls_2), _actuator_pub, &_actuators); + orb_publish(ORB_ID(actuator_controls_2), _actuator_pub, &actuators); } else { - _actuator_pub = orb_advertise(ORB_ID(actuator_controls_2), &_actuators); + _actuator_pub = orb_advertise(ORB_ID(actuator_controls_2), &actuators); } } else { @@ -498,7 +495,6 @@ MissionBlock::mission_item_to_position_setpoint(const mission_item_s &item, posi _navigator->get_loiter_radius(); sp->loiter_direction = (item.loiter_radius > 0) ? 1 : -1; sp->acceptance_radius = item.acceptance_radius; - sp->disable_mc_yaw_control = item.disable_mc_yaw; sp->cruising_speed = _navigator->get_cruising_speed(); sp->cruising_throttle = _navigator->get_cruising_throttle(); @@ -527,29 +523,19 @@ MissionBlock::mission_item_to_position_setpoint(const mission_item_s &item, posi case NAV_CMD_VTOL_TAKEOFF: sp->type = position_setpoint_s::SETPOINT_TYPE_TAKEOFF; - - if (_navigator->get_vstatus()->is_vtol && _param_vtol_wv_takeoff.get()) { - sp->disable_mc_yaw_control = true; - } - break; case NAV_CMD_LAND: case NAV_CMD_VTOL_LAND: sp->type = position_setpoint_s::SETPOINT_TYPE_LAND; - - if (_navigator->get_vstatus()->is_vtol && _param_vtol_wv_land.get()) { - sp->disable_mc_yaw_control = true; - } - break; case NAV_CMD_LOITER_TO_ALT: // initially use current altitude, and switch to mission item altitude once in loiter position - if (_param_loiter_min_alt.get() > 0.0f) { // ignore _param_loiter_min_alt if smaller then 0 (-1) + if (_navigator->get_loiter_min_alt() > 0.0f) { // ignore _param_loiter_min_alt if smaller then 0 (-1) sp->alt = math::max(_navigator->get_global_position()->alt, - _navigator->get_home_position()->alt + _param_loiter_min_alt.get()); + _navigator->get_home_position()->alt + _navigator->get_loiter_min_alt()); } else { sp->alt = _navigator->get_global_position()->alt; @@ -559,11 +545,6 @@ MissionBlock::mission_item_to_position_setpoint(const mission_item_s &item, posi case NAV_CMD_LOITER_TIME_LIMIT: case NAV_CMD_LOITER_UNLIMITED: sp->type = position_setpoint_s::SETPOINT_TYPE_LOITER; - - if (_navigator->get_vstatus()->is_vtol && _param_vtol_wv_loiter.get()) { - sp->disable_mc_yaw_control = true; - } - break; default: @@ -625,40 +606,6 @@ MissionBlock::set_loiter_item(struct mission_item_s *item, float min_clearance) } } -void -MissionBlock::set_follow_target_item(struct mission_item_s *item, float min_clearance, follow_target_s &target, - float yaw) -{ - if (_navigator->get_land_detected()->landed) { - /* landed, don't takeoff, but switch to IDLE mode */ - item->nav_cmd = NAV_CMD_IDLE; - - } else { - - item->nav_cmd = NAV_CMD_DO_FOLLOW_REPOSITION; - - /* use current target position */ - item->lat = target.lat; - item->lon = target.lon; - item->altitude = _navigator->get_home_position()->alt; - - if (min_clearance > 8.0f) { - item->altitude += min_clearance; - - } else { - item->altitude += 8.0f; // if min clearance is bad set it to 8.0 meters (well above the average height of a person) - } - } - - item->altitude_is_relative = false; - item->yaw = yaw; - item->loiter_radius = _navigator->get_loiter_radius(); - item->acceptance_radius = _navigator->get_acceptance_radius(); - item->time_inside = 0.0f; - item->autocontinue = false; - item->origin = ORIGIN_ONBOARD; -} - void MissionBlock::set_takeoff_item(struct mission_item_s *item, float abs_altitude, float min_pitch) { @@ -681,16 +628,13 @@ MissionBlock::set_takeoff_item(struct mission_item_s *item, float abs_altitude, void MissionBlock::set_land_item(struct mission_item_s *item, bool at_current_location) { - /* VTOL transition to RW before landing */ - if (_navigator->get_vstatus()->is_vtol && - !_navigator->get_vstatus()->is_rotary_wing && - _param_force_vtol.get() == 1) { + if (_navigator->force_vtol()) { - vehicle_command_s cmd = {}; - cmd.command = NAV_CMD_DO_VTOL_TRANSITION; - cmd.param1 = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC; - _navigator->publish_vehicle_cmd(&cmd); + vehicle_command_s vcmd = {}; + vcmd.command = NAV_CMD_DO_VTOL_TRANSITION; + vcmd.param1 = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC; + _navigator->publish_vehicle_cmd(&vcmd); } /* set the land item */ diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h index ab464478e1..8d621ad34c 100644 --- a/src/modules/navigator/mission_block.h +++ b/src/modules/navigator/mission_block.h @@ -44,8 +44,6 @@ #include "navigator_mode.h" #include -#include -#include #include #include #include @@ -115,11 +113,6 @@ protected: */ void set_idle_item(struct mission_item_s *item); - /** - * Set follow_target item - */ - void set_follow_target_item(struct mission_item_s *item, float min_clearance, follow_target_s &target, float yaw); - /** * General function used to adjust the mission item based on vehicle specific limitations */ @@ -138,16 +131,12 @@ protected: hrt_abstime _action_start{0}; hrt_abstime _time_wp_reached{0}; - actuator_controls_s _actuators{}; orb_advert_t _actuator_pub{nullptr}; - control::BlockParamFloat _param_loiter_min_alt; control::BlockParamFloat _param_yaw_timeout; control::BlockParamFloat _param_yaw_err; - control::BlockParamInt _param_vtol_wv_land; - control::BlockParamInt _param_vtol_wv_takeoff; - control::BlockParamInt _param_vtol_wv_loiter; - control::BlockParamInt _param_force_vtol; + + // VTOL parameters control::BlockParamFloat _param_back_trans_dec_mss; control::BlockParamFloat _param_reverse_delay; }; diff --git a/src/modules/navigator/mission_params.c b/src/modules/navigator/mission_params.c index cfff98c88b..de201fb61f 100644 --- a/src/modules/navigator/mission_params.c +++ b/src/modules/navigator/mission_params.c @@ -174,41 +174,3 @@ PARAM_DEFINE_FLOAT(MIS_YAW_TMT, -1.0f); * @group Mission */ PARAM_DEFINE_FLOAT(MIS_YAW_ERR, 12.0f); - -/** - * Weather-vane mode landings for missions - * - * @boolean - * @group Mission - */ -PARAM_DEFINE_INT32(VT_WV_LND_EN, 0); - -/** - * Enable weather-vane mode takeoff for missions - * - * @boolean - * @group Mission - */ -PARAM_DEFINE_INT32(VT_WV_TKO_EN, 0); - -/** - * Weather-vane mode for loiter - * - * @boolean - * @group Mission - */ -PARAM_DEFINE_INT32(VT_WV_LTR_EN, 0); - -/** - * Cruise Airspeed - * - * The fixed wing controller tries to fly at this airspeed. - * - * @unit m/s - * @min 0.0 - * @max 40 - * @decimal 1 - * @increment 0.5 - * @group FW TECS - */ -PARAM_DEFINE_FLOAT(FW_AIRSPD_TRIM, 15.0f); diff --git a/src/modules/navigator/navigation.h b/src/modules/navigator/navigation.h index 40d00f5ac4..1a45c23123 100644 --- a/src/modules/navigator/navigation.h +++ b/src/modules/navigator/navigation.h @@ -141,7 +141,6 @@ struct mission_item_s { force_heading : 1, /**< heading needs to be reached */ altitude_is_relative : 1, /**< true if altitude is relative from start point */ autocontinue : 1, /**< true if next waypoint should follow after this one */ - disable_mc_yaw : 1, /**< weathervane mode */ vtol_back_transition : 1; /**< part of the vtol back transition sequence */ }; }; diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 0a992b865b..f39606b045 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -222,6 +222,10 @@ public: bool abort_landing(); + float get_loiter_min_alt() const { return _param_loiter_min_alt.get(); } + + bool force_vtol() const { return _vstatus.is_vtol && !_vstatus.is_rotary_wing && _param_force_vtol.get(); } + private: bool _task_should_exit{false}; /**< if true, sensor task should exit */ @@ -292,11 +296,15 @@ private: NavigatorMode *_navigation_mode_array[NAVIGATOR_MODE_ARRAY_SIZE]; /**< array of navigation modes */ + // navigator parameters control::BlockParamFloat _param_loiter_radius; /**< loiter radius for fixedwing */ - control::BlockParamFloat _param_acceptance_radius; /**< acceptance for takeoff */ control::BlockParamFloat _param_fw_alt_acceptance_radius; /**< acceptance radius for fixedwing altitude */ control::BlockParamFloat _param_mc_alt_acceptance_radius; /**< acceptance radius for multicopter altitude */ + control::BlockParamInt _param_force_vtol; /**< acceptance radius for multicopter altitude */ + + // non-navigator parameters + control::BlockParamFloat _param_loiter_min_alt; float _mission_cruising_speed_mc{-1.0f}; float _mission_cruising_speed_fw{-1.0f}; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 4b2fd5de77..338c10745b 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -46,6 +46,9 @@ #include "navigator.h" #include +#include +#include +#include #include #include @@ -55,9 +58,6 @@ #include #include #include -#include -#include -#include #include #include #include @@ -96,10 +96,14 @@ Navigator::Navigator() : _engineFailure(this, "EF"), _gpsFailure(this, "GPSF"), _follow_target(this, "TAR"), + // navigator params _param_loiter_radius(this, "LOITER_RAD"), _param_acceptance_radius(this, "ACC_RAD"), _param_fw_alt_acceptance_radius(this, "FW_ALT_RAD"), - _param_mc_alt_acceptance_radius(this, "MC_ALT_RAD") + _param_mc_alt_acceptance_radius(this, "MC_ALT_RAD"), + _param_force_vtol(this, "FORCE_VT"), + // non-navigator params + _param_loiter_min_alt(this, "MIS_LTRMIN_ALT", false) { /* Create a list of our possible navigation types */ _navigation_mode_array[0] = &_mission; diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index 9a8b9e8158..ef59d21f1d 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -54,7 +54,6 @@ RTL::RTL(Navigator *navigator, const char *name) : MissionBlock(navigator, name), _rtl_state(RTL_STATE_NONE), _param_return_alt(this, "RTL_RETURN_ALT", false), - _param_min_loiter_alt(this, "MIS_LTRMIN_ALT", false), _param_descend_alt(this, "RTL_DESCEND_ALT", false), _param_land_delay(this, "RTL_LAND_DELAY", false), _param_rtl_min_dist(this, "RTL_MIN_DIST", false) diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h index 94fa1d530e..78163828b8 100644 --- a/src/modules/navigator/rtl.h +++ b/src/modules/navigator/rtl.h @@ -92,7 +92,6 @@ private: } _rtl_state{RTL_STATE_NONE}; control::BlockParamFloat _param_return_alt; - control::BlockParamFloat _param_min_loiter_alt; control::BlockParamFloat _param_descend_alt; control::BlockParamFloat _param_land_delay; control::BlockParamFloat _param_rtl_min_dist; diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp index 4d82922a20..998c4bb269 100644 --- a/src/modules/vtol_att_control/standard.cpp +++ b/src/modules/vtol_att_control/standard.cpp @@ -426,7 +426,6 @@ void Standard::update_mc_state() return; } - matrix::Dcmf R(matrix::Quatf(_v_att->q)); matrix::Dcmf R_sp(matrix::Quatf(_v_att_sp->q_d)); matrix::Eulerf euler(R); @@ -543,7 +542,7 @@ void Standard::fill_actuator_outputs() } // set the fixed wing throttle control - if (_vtol_schedule.flight_mode == FW_MODE && _armed->armed) { + if (_vtol_schedule.flight_mode == FW_MODE) { // take the throttle value commanded by the fw controller _actuators_out_1->control[actuator_controls_s::INDEX_THROTTLE] = diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index e8df32cce6..7da86efbb0 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -74,7 +74,6 @@ VtolAttitudeControl::VtolAttitudeControl() : _v_control_mode_sub(-1), _params_sub(-1), _manual_control_sp_sub(-1), - _armed_sub(-1), _local_pos_sub(-1), _pos_sp_triplet_sub(-1), _airspeed_sub(-1), @@ -108,7 +107,6 @@ VtolAttitudeControl::VtolAttitudeControl() : memset(&_actuators_out_1, 0, sizeof(_actuators_out_1)); memset(&_actuators_mc_in, 0, sizeof(_actuators_mc_in)); memset(&_actuators_fw_in, 0, sizeof(_actuators_fw_in)); - memset(&_armed, 0, sizeof(_armed)); memset(&_local_pos, 0, sizeof(_local_pos)); memset(&_pos_sp_triplet, 0, sizeof(_pos_sp_triplet)); memset(&_airspeed, 0, sizeof(_airspeed)); @@ -139,6 +137,10 @@ VtolAttitudeControl::VtolAttitudeControl() : _params_handles.front_trans_time_openloop = param_find("VT_F_TR_OL_TM"); _params_handles.front_trans_time_min = param_find("VT_TRANS_MIN_TM"); + _params_handles.wv_takeoff = param_find("VT_WV_TKO_EN"); + _params_handles.wv_land = param_find("VT_WV_LND_EN"); + _params_handles.wv_loiter = param_find("VT_WV_LTR_EN"); + /* fetch initial parameter values */ parameters_update(); @@ -217,19 +219,6 @@ void VtolAttitudeControl::vehicle_manual_poll() orb_copy(ORB_ID(manual_control_setpoint), _manual_control_sp_sub, &_manual_control_sp); } } -/** -* Check for arming status updates. -*/ -void VtolAttitudeControl::arming_status_poll() -{ - /* check if there is a new setpoint */ - bool updated; - orb_check(_armed_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed); - } -} /** * Check for inputs from mc attitude controller. @@ -601,6 +590,16 @@ VtolAttitudeControl::parameters_update() _params.front_trans_time_min = math::min(_params.front_trans_time_openloop * 0.9f, _params.front_trans_time_min); + /* weathervane */ + param_get(_params_handles.wv_takeoff, &l); + _params.wv_takeoff = (l == 1); + + param_get(_params_handles.wv_loiter, &l); + _params.wv_loiter = (l == 1); + + param_get(_params_handles.wv_land, &l); + _params.wv_land = (l == 1); + // update the parameters of the instances of base VtolType if (_vtol_type != nullptr) { _vtol_type->parameters_update(); @@ -666,7 +665,6 @@ void VtolAttitudeControl::task_main() _v_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); - _armed_sub = orb_subscribe(ORB_ID(actuator_armed)); _local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); _pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); @@ -748,7 +746,6 @@ void VtolAttitudeControl::task_main() fw_virtual_att_sp_poll(); vehicle_control_mode_poll(); //Check for changes in vehicle control mode. vehicle_manual_poll(); //Check for changes in manual inputs. - arming_status_poll(); //Check for arming status updates. vehicle_attitude_setpoint_poll();//Check for changes in attitude set points vehicle_attitude_poll(); //Check for changes in attitude actuator_controls_mc_poll(); //Check for changes in mc_attitude_control output diff --git a/src/modules/vtol_att_control/vtol_att_control_main.h b/src/modules/vtol_att_control/vtol_att_control_main.h index 891eebc1b6..9e009dcbde 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.h +++ b/src/modules/vtol_att_control/vtol_att_control_main.h @@ -63,7 +63,6 @@ #include #include -#include #include #include #include @@ -114,7 +113,6 @@ public: struct actuator_controls_s *get_actuators_out1() {return &_actuators_out_1;} struct actuator_controls_s *get_actuators_mc_in() {return &_actuators_mc_in;} struct actuator_controls_s *get_actuators_fw_in() {return &_actuators_fw_in;} - struct actuator_armed_s *get_armed() {return &_armed;} struct vehicle_local_position_s *get_local_pos() {return &_local_pos;} struct position_setpoint_triplet_s *get_pos_sp_triplet() {return &_pos_sp_triplet;} struct airspeed_s *get_airspeed() {return &_airspeed;} @@ -141,7 +139,6 @@ private: int _v_control_mode_sub; //vehicle control mode subscription int _params_sub; //parameter updates subscription int _manual_control_sp_sub; //manual control setpoint subscription - int _armed_sub; //arming status subscription int _local_pos_sub; // sensor subscription int _pos_sp_triplet_sub; // local position setpoint subscription int _airspeed_sub; // airspeed subscription @@ -175,7 +172,6 @@ private: struct actuator_controls_s _actuators_out_1; //actuator controls going to the fw mixer (used for elevons) struct actuator_controls_s _actuators_mc_in; //actuator controls from mc_att_control struct actuator_controls_s _actuators_fw_in; //actuator controls from fw_att_control - struct actuator_armed_s _armed; //actuator arming status struct vehicle_local_position_s _local_pos; struct position_setpoint_triplet_s _pos_sp_triplet; struct airspeed_s _airspeed; // airspeed @@ -204,6 +200,9 @@ private: param_t fw_qc_max_roll; param_t front_trans_time_openloop; param_t front_trans_time_min; + param_t wv_takeoff; + param_t wv_loiter; + param_t wv_land; } _params_handles; /* for multicopters it is usual to have a non-zero idle speed of the engines @@ -221,7 +220,6 @@ private: void vehicle_control_mode_poll(); //Check for changes in vehicle control mode. void vehicle_manual_poll(); //Check for changes in manual inputs. - void arming_status_poll(); //Check for arming status updates. void mc_virtual_att_sp_poll(); void fw_virtual_att_sp_poll(); void actuator_controls_mc_poll(); //Check for changes in mc_attitude_control output diff --git a/src/modules/vtol_att_control/vtol_att_control_params.c b/src/modules/vtol_att_control/vtol_att_control_params.c index c8133e0955..6f8329a200 100644 --- a/src/modules/vtol_att_control/vtol_att_control_params.c +++ b/src/modules/vtol_att_control/vtol_att_control_params.c @@ -271,20 +271,6 @@ PARAM_DEFINE_FLOAT(VT_ARSP_TRANS, 10.0f); */ PARAM_DEFINE_INT32(VT_OPT_RECOV_EN, 0); -/** - * Weather-vane yaw rate scale. - * - * The desired yawrate from the controller will be scaled in order to avoid - * yaw fighting against the wind. - * - * @min 0.0 - * @max 1.0 - * @increment 0.01 - * @decimal 3 - * @group VTOL Attitude Control - */ -PARAM_DEFINE_FLOAT(VT_WV_YAWR_SCL, 0.15f); - /** * Front transition timeout * @@ -356,3 +342,41 @@ PARAM_DEFINE_INT32(VT_FW_QC_R, 0); * @group VTOL Attitude Control */ PARAM_DEFINE_FLOAT(VT_F_TR_OL_TM, 6.0f); + +/** + * Weather-vane yaw rate scale. + * + * The desired yawrate from the controller will be scaled in order to avoid + * yaw fighting against the wind. + * + * @min 0.0 + * @max 1.0 + * @increment 0.01 + * @decimal 3 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VT_WV_YAWR_SCL, 0.15f); + +/** + * Enable weather-vane mode takeoff for missions + * + * @boolean + * @group Mission + */ +PARAM_DEFINE_INT32(VT_WV_TKO_EN, 0); + +/** + * Weather-vane mode for loiter + * + * @boolean + * @group Mission + */ +PARAM_DEFINE_INT32(VT_WV_LTR_EN, 0); + +/** + * Weather-vane mode landings for missions + * + * @boolean + * @group Mission + */ +PARAM_DEFINE_INT32(VT_WV_LND_EN, 0); diff --git a/src/modules/vtol_att_control/vtol_type.cpp b/src/modules/vtol_att_control/vtol_type.cpp index cac0ac2c0d..298c62355e 100644 --- a/src/modules/vtol_att_control/vtol_type.cpp +++ b/src/modules/vtol_att_control/vtol_type.cpp @@ -64,7 +64,6 @@ VtolType::VtolType(VtolAttitudeControl *att_controller) : _actuators_out_1 = _attc->get_actuators_out1(); _actuators_mc_in = _attc->get_actuators_mc_in(); _actuators_fw_in = _attc->get_actuators_fw_in(); - _armed = _attc->get_armed(); _local_pos = _attc->get_local_pos(); _airspeed = _attc->get_airspeed(); _batt_status = _attc->get_batt_status(); @@ -153,6 +152,24 @@ void VtolType::update_mc_state() _mc_roll_weight = 1.0f; _mc_pitch_weight = 1.0f; _mc_yaw_weight = 1.0f; + + // VTOL weathervane + _v_att_sp->disable_mc_yaw_control = false; + + if (_attc->get_pos_sp_triplet()->current.valid && + !_v_control_mode->flag_control_manual_enabled) { + + if (_params->wv_takeoff && _attc->get_pos_sp_triplet()->current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) { + _v_att_sp->disable_mc_yaw_control = true; + + } else if (_params->wv_loiter + && _attc->get_pos_sp_triplet()->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER) { + _v_att_sp->disable_mc_yaw_control = true; + + } else if (_params->wv_land && _attc->get_pos_sp_triplet()->current.type == position_setpoint_s::SETPOINT_TYPE_LAND) { + _v_att_sp->disable_mc_yaw_control = true; + } + } } void VtolType::update_fw_state() @@ -190,13 +207,13 @@ void VtolType::update_transition_state() bool VtolType::can_transition_on_ground() { - return !_armed->armed || _land_detected->landed; + return !_v_control_mode->flag_armed || _land_detected->landed; } void VtolType::check_quadchute_condition() { - if (_armed->armed && !_land_detected->landed) { + if (_v_control_mode->flag_armed && !_land_detected->landed) { matrix::Eulerf euler = matrix::Quatf(_v_att->q); // fixed-wing minimum altitude diff --git a/src/modules/vtol_att_control/vtol_type.h b/src/modules/vtol_att_control/vtol_type.h index b62c05b901..7a05971eb8 100644 --- a/src/modules/vtol_att_control/vtol_type.h +++ b/src/modules/vtol_att_control/vtol_type.h @@ -47,9 +47,9 @@ #include struct Params { - int idle_pwm_mc; // pwm value for idle in mc mode - int vtol_motor_count; // number of motors - int vtol_fw_permanent_stab; // in fw mode stabilize attitude also in manual mode + int32_t idle_pwm_mc; // pwm value for idle in mc mode + int32_t vtol_motor_count; // number of motors + int32_t vtol_fw_permanent_stab; // in fw mode stabilize attitude also in manual mode float mc_airspeed_min; // min airspeed in multicoper mode (including prop-wash) float mc_airspeed_trim; // trim airspeed in multicopter mode float mc_airspeed_max; // max airpseed in multicopter mode @@ -57,13 +57,16 @@ struct Params { float power_max; // maximum power of one engine float prop_eff; // factor to calculate prop efficiency float arsp_lp_gain; // total airspeed estimate low pass gain - int vtol_type; - int elevons_mc_lock; // lock elevons in multicopter mode + int32_t vtol_type; + int32_t elevons_mc_lock; // lock elevons in multicopter mode float fw_min_alt; // minimum relative altitude for FW mode (QuadChute) float fw_qc_max_pitch; // maximum pitch angle FW mode (QuadChute) float fw_qc_max_roll; // maximum roll angle FW mode (QuadChute) float front_trans_time_openloop; float front_trans_time_min; + bool wv_takeoff; + bool wv_loiter; + bool wv_land; }; // Has to match 1:1 msg/vtol_vehicle_status.msg @@ -164,7 +167,6 @@ protected: struct actuator_controls_s *_actuators_out_1; //actuator controls going to the fw mixer (used for elevons) struct actuator_controls_s *_actuators_mc_in; //actuator controls from mc_att_control struct actuator_controls_s *_actuators_fw_in; //actuator controls from fw_att_control - struct actuator_armed_s *_armed; //actuator arming status struct vehicle_local_position_s *_local_pos; struct airspeed_s *_airspeed; // airspeed struct battery_status_s *_batt_status; // battery status