forked from Archive/PX4-Autopilot
navigator move parameters out of MissionBlock
- MissionBlock is an interface with > 10 implementations
This commit is contained in:
Daniel Agar
Lorenz Meier
parent
8e457b6037
commit
4416c4ddb3
|
|
@ -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)
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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 */
|
||||
|
|
|
|||
|
|
@ -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
|
||||
*
|
||||
|
|
|
|||
|
|
@ -68,7 +68,6 @@
|
|||
#include <systemlib/param/param.h>
|
||||
#include <systemlib/perf_counter.h>
|
||||
#include <systemlib/systemlib.h>
|
||||
#include <uORB/topics/actuator_armed.h>
|
||||
#include <uORB/topics/actuator_controls.h>
|
||||
#include <uORB/topics/battery_status.h>
|
||||
#include <uORB/topics/manual_control_setpoint.h>
|
||||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -46,6 +46,7 @@
|
|||
#include <lib/mathlib/math/Matrix.hpp>
|
||||
#include "navigator_mode.h"
|
||||
#include "mission_block.h"
|
||||
#include <uORB/topics/follow_target.h>
|
||||
|
||||
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);
|
||||
};
|
||||
|
|
|
|||
|
|
@ -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();
|
||||
|
|
|
|||
|
|
@ -80,7 +80,6 @@ private:
|
|||
*/
|
||||
void set_loiter_position();
|
||||
|
||||
control::BlockParamFloat _param_min_alt;
|
||||
control::BlockParamInt _param_yawmode;
|
||||
bool _loiter_pos_set;
|
||||
};
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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 {};
|
||||
|
|
|
|||
|
|
@ -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 */
|
||||
|
|
|
|||
|
|
@ -44,8 +44,6 @@
|
|||
#include "navigator_mode.h"
|
||||
|
||||
#include <navigator/navigation.h>
|
||||
#include <uORB/topics/actuator_controls.h>
|
||||
#include <uORB/topics/follow_target.h>
|
||||
#include <uORB/topics/mission.h>
|
||||
#include <uORB/topics/position_setpoint_triplet.h>
|
||||
#include <uORB/topics/vehicle_command.h>
|
||||
|
|
@ -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;
|
||||
};
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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 */
|
||||
};
|
||||
};
|
||||
|
|
|
|||
|
|
@ -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};
|
||||
|
|
|
|||
|
|
@ -46,6 +46,9 @@
|
|||
#include "navigator.h"
|
||||
|
||||
#include <cfloat>
|
||||
#include <sys/ioctl.h>
|
||||
#include <sys/stat.h>
|
||||
#include <sys/types.h>
|
||||
|
||||
#include <dataman/dataman.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
|
|
@ -55,9 +58,6 @@
|
|||
#include <px4_defines.h>
|
||||
#include <px4_posix.h>
|
||||
#include <px4_tasks.h>
|
||||
#include <sys/ioctl.h>
|
||||
#include <sys/stat.h>
|
||||
#include <sys/types.h>
|
||||
#include <systemlib/mavlink_log.h>
|
||||
#include <systemlib/systemlib.h>
|
||||
#include <uORB/topics/fw_pos_ctrl_status.h>
|
||||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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] =
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -63,7 +63,6 @@
|
|||
#include <systemlib/param/param.h>
|
||||
#include <systemlib/systemlib.h>
|
||||
|
||||
#include <uORB/topics/actuator_armed.h>
|
||||
#include <uORB/topics/actuator_controls.h>
|
||||
#include <uORB/topics/airspeed.h>
|
||||
#include <uORB/topics/battery_status.h>
|
||||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -47,9 +47,9 @@
|
|||
#include <drivers/drv_hrt.h>
|
||||
|
||||
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
|
||||
|
|
|
|||
Loading…
Reference in New Issue