diff --git a/ROMFS/px4fmu_common/mixers/quad_x_vtol.main.mix b/ROMFS/px4fmu_common/mixers/quad_x_vtol.main.mix index 4fd323353a..bd332a2688 100644 --- a/ROMFS/px4fmu_common/mixers/quad_x_vtol.main.mix +++ b/ROMFS/px4fmu_common/mixers/quad_x_vtol.main.mix @@ -9,10 +9,10 @@ R: 4x 10000 10000 10000 0 #mixer for the elevons M: 2 O: 10000 10000 0 -10000 10000 -S: 1 0 10000 10000 0 -10000 10000 -S: 1 1 10000 10000 0 -10000 10000 +S: 1 0 5000 5000 0 -10000 10000 +S: 1 1 5000 5000 0 -10000 10000 M: 2 O: 10000 10000 0 -10000 10000 -S: 1 0 10000 10000 0 -10000 10000 -S: 1 1 -10000 -10000 0 -10000 10000 +S: 1 0 5000 5000 0 -10000 10000 +S: 1 1 -5000 -5000 0 -10000 10000 diff --git a/Tools/sitl_run.sh b/Tools/sitl_run.sh index 7dee02868a..f32f6e585e 100755 --- a/Tools/sitl_run.sh +++ b/Tools/sitl_run.sh @@ -79,7 +79,7 @@ touch rootfs/eeprom/parameters # Start Java simulator if [ "$debugger" == "lldb" ] then - lldb -- mainapp ../../../../${rc_script}_${program} + lldb -- mainapp ../../../../${rc_script}_${program}_${model} elif [ "$debugger" == "gdb" ] then gdb --args mainapp ../../../../${rc_script}_${program} diff --git a/msg/fw_virtual_attitude_setpoint.msg b/msg/fw_virtual_attitude_setpoint.msg new file mode 100644 index 0000000000..7bbb670b31 --- /dev/null +++ b/msg/fw_virtual_attitude_setpoint.msg @@ -0,0 +1,23 @@ + +uint64 timestamp # in microseconds since system start, is set whenever the writing thread stores new data + +float32 roll_body # body angle in NED frame +float32 pitch_body # body angle in NED frame +float32 yaw_body # body angle in NED frame + +float32 yaw_sp_move_rate # rad/s (commanded by user) + +float32[9] R_body # Rotation matrix describing the setpoint as rotation from the current body frame +bool R_valid # Set to true if rotation matrix is valid + +# For quaternion-based attitude control +float32[4] q_d # Desired quaternion for quaternion control +bool q_d_valid # Set to true if quaternion vector is valid +float32[4] q_e # Attitude error in quaternion +bool q_e_valid # Set to true if quaternion error vector is valid + +float32 thrust # Thrust in Newton the power system should generate + +bool roll_reset_integral # Reset roll integral part (navigation logic change) +bool pitch_reset_integral # Reset pitch integral part (navigation logic change) +bool yaw_reset_integral # Reset yaw integral part (navigation logic change) diff --git a/msg/mc_virtual_attitude_setpoint.msg b/msg/mc_virtual_attitude_setpoint.msg new file mode 100644 index 0000000000..7bbb670b31 --- /dev/null +++ b/msg/mc_virtual_attitude_setpoint.msg @@ -0,0 +1,23 @@ + +uint64 timestamp # in microseconds since system start, is set whenever the writing thread stores new data + +float32 roll_body # body angle in NED frame +float32 pitch_body # body angle in NED frame +float32 yaw_body # body angle in NED frame + +float32 yaw_sp_move_rate # rad/s (commanded by user) + +float32[9] R_body # Rotation matrix describing the setpoint as rotation from the current body frame +bool R_valid # Set to true if rotation matrix is valid + +# For quaternion-based attitude control +float32[4] q_d # Desired quaternion for quaternion control +bool q_d_valid # Set to true if quaternion vector is valid +float32[4] q_e # Attitude error in quaternion +bool q_e_valid # Set to true if quaternion error vector is valid + +float32 thrust # Thrust in Newton the power system should generate + +bool roll_reset_integral # Reset roll integral part (navigation logic change) +bool pitch_reset_integral # Reset pitch integral part (navigation logic change) +bool yaw_reset_integral # Reset yaw integral part (navigation logic change) diff --git a/msg/vehicle_status.msg b/msg/vehicle_status.msg index 60ab34e90d..b35d0cab60 100644 --- a/msg/vehicle_status.msg +++ b/msg/vehicle_status.msg @@ -113,6 +113,7 @@ uint32 system_id # system id, inspired by MAVLink's system ID field uint32 component_id # subsystem / component id, inspired by MAVLink's component ID field bool is_rotary_wing # True if system is in rotary wing configuration, so for a VTOL this is only true while flying as a multicopter +bool vtol_in_transition # True if VTOL is doing a transition bool is_vtol # True if the system is VTOL capable bool vtol_fw_permanent_stab # True if vtol should stabilize attitude for fw in manual mode bool in_transition_mode diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 851529bb54..20efb4db3c 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1599,7 +1599,7 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vtol_vehicle_status), vtol_vehicle_status_sub, &vtol_status); status.vtol_fw_permanent_stab = vtol_status.fw_permanent_stab; - /* Make sure that this is only adjusted if vehicle really is of type vtol*/ + /* Make sure that this is only adjusted if vehicle really is of type vtol */ if (is_vtol(&status)) { status.is_rotary_wing = vtol_status.vtol_in_rw_mode; status.in_transition_mode = vtol_status.vtol_in_trans_mode; 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 8772ceb835..1328bca48d 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -59,6 +59,7 @@ #include #include #include +#include #include #include #include @@ -142,6 +143,7 @@ private: orb_id_t _rates_sp_id; // pointer to correct rates setpoint uORB metadata structure orb_id_t _actuators_id; // pointer to correct actuator controls0 uORB metadata structure + orb_id_t _attitude_setpoint_id; struct control_state_s _ctrl_state; /**< control state */ struct accel_report _accel; /**< body frame accelerations */ @@ -360,6 +362,7 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _rates_sp_id(0), _actuators_id(0), + _attitude_setpoint_id(0), /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "fw att control")), @@ -633,9 +636,11 @@ FixedwingAttitudeControl::vehicle_status_poll() if (_vehicle_status.is_vtol) { _rates_sp_id = ORB_ID(fw_virtual_rates_setpoint); _actuators_id = ORB_ID(actuator_controls_virtual_fw); + _attitude_setpoint_id = ORB_ID(fw_virtual_attitude_setpoint); } else { _rates_sp_id = ORB_ID(vehicle_rates_setpoint); _actuators_id = ORB_ID(actuator_controls_0); + _attitude_setpoint_id = ORB_ID(vehicle_attitude_setpoint); } } } @@ -1021,15 +1026,12 @@ FixedwingAttitudeControl::task_main() att_sp.thrust = throttle_sp; /* lazily publish the setpoint only once available */ - if (!_vehicle_status.is_rotary_wing && !_vehicle_status.in_transition_mode) { - if (_attitude_sp_pub != nullptr) { - /* publish the attitude setpoint */ - orb_publish(ORB_ID(vehicle_attitude_setpoint), _attitude_sp_pub, &att_sp); - - } else { - /* advertise and publish */ - _attitude_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); - } + if (_attitude_sp_pub != nullptr) { + /* publish the attitude setpoint */ + orb_publish(_attitude_setpoint_id, _attitude_sp_pub, &att_sp); + } else if (_attitude_setpoint_id) { + /* advertise and publish */ + _attitude_sp_pub = orb_advertise(_attitude_setpoint_id, &att_sp); } } diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 24cf1f084f..8b4e4224eb 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -72,6 +72,7 @@ #include #include #include +#include #include #include #include @@ -166,6 +167,8 @@ private: orb_advert_t _tecs_status_pub; /**< TECS status publication */ orb_advert_t _nav_capabilities_pub; /**< navigation capabilities publication */ + orb_id_t _attitude_setpoint_id; + struct control_state_s _ctrl_state; /**< control state */ struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ struct navigation_capabilities_s _nav_capabilities; /**< navigation capabilities */ @@ -506,6 +509,9 @@ FixedwingPositionControl::FixedwingPositionControl() : _tecs_status_pub(nullptr), _nav_capabilities_pub(nullptr), +/* publication ID */ + _attitude_setpoint_id(0), + /* states */ _ctrl_state(), _att_sp(), @@ -766,6 +772,14 @@ FixedwingPositionControl::vehicle_status_poll() if (updated) { orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status); + /* set correct uORB ID, depending on if vehicle is VTOL or not */ + if (!_attitude_setpoint_id) { + if (_vehicle_status.is_vtol) { + _attitude_setpoint_id = ORB_ID(fw_virtual_attitude_setpoint); + } else { + _attitude_setpoint_id = ORB_ID(vehicle_attitude_setpoint); + } + } } } @@ -1990,13 +2004,13 @@ FixedwingPositionControl::task_main() _att_sp.timestamp = hrt_absolute_time(); /* lazily publish the setpoint only once available */ - if (_attitude_sp_pub != nullptr && !_vehicle_status.is_rotary_wing && !_vehicle_status.in_transition_mode) { + if (_attitude_sp_pub != nullptr) { /* publish the attitude setpoint */ - orb_publish(ORB_ID(vehicle_attitude_setpoint), _attitude_sp_pub, &_att_sp); + orb_publish(_attitude_setpoint_id, _attitude_sp_pub, &_att_sp); - } else if (_attitude_sp_pub == nullptr && !_vehicle_status.is_rotary_wing && !_vehicle_status.in_transition_mode) { + } else if (_attitude_setpoint_id) { /* advertise and publish */ - _attitude_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp); + _attitude_sp_pub = orb_advertise(_attitude_setpoint_id, &_att_sp); } /* XXX check if radius makes sense here */ 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 6776d78871..a631336273 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -69,6 +69,7 @@ #include #include #include +#include #include #include #include @@ -141,6 +142,8 @@ private: orb_advert_t _local_pos_sp_pub; /**< vehicle local position setpoint publication */ orb_advert_t _global_vel_sp_pub; /**< vehicle global velocity setpoint publication */ + orb_id_t _attitude_setpoint_id; + struct vehicle_status_s _vehicle_status; /**< vehicle status */ struct control_state_s _ctrl_state; /**< vehicle attitude */ struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ @@ -336,6 +339,7 @@ MulticopterPositionControl::MulticopterPositionControl() : _att_sp_pub(nullptr), _local_pos_sp_pub(nullptr), _global_vel_sp_pub(nullptr), + _attitude_setpoint_id(0), _manual_thr_min(this, "MANTHR_MIN"), _manual_thr_max(this, "MANTHR_MAX"), _vel_x_deriv(this, "VELD"), @@ -533,6 +537,14 @@ MulticopterPositionControl::poll_subscriptions() if (updated) { orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status); + /* set correct uORB ID, depending on if vehicle is VTOL or not */ + if (!_attitude_setpoint_id) { + if (_vehicle_status.is_vtol) { + _attitude_setpoint_id = ORB_ID(mc_virtual_attitude_setpoint); + } else { + _attitude_setpoint_id = ORB_ID(vehicle_attitude_setpoint); + } + } } orb_check(_ctrl_state_sub, &updated); @@ -1183,10 +1195,9 @@ MulticopterPositionControl::task_main() /* publish attitude setpoint */ if (_att_sp_pub != nullptr) { - orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp); - - } else { - _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp); + orb_publish(_attitude_setpoint_id, _att_sp_pub, &_att_sp); + } else if (_attitude_setpoint_id) { + _att_sp_pub = orb_advertise(_attitude_setpoint_id, &_att_sp); } } else { @@ -1610,16 +1621,18 @@ MulticopterPositionControl::task_main() /* publish attitude setpoint * Do not publish if offboard is enabled but position/velocity control is disabled, - * in this case the attitude setpoint is published by the mavlink app + * in this case the attitude setpoint is published by the mavlink app. Also do not publish + * if the vehicle is a VTOL and it's just doing a transition (the VTOL attitude control module will generate + * attitude setpoints for the transition). */ if (!(_control_mode.flag_control_offboard_enabled && - !(_control_mode.flag_control_position_enabled || - _control_mode.flag_control_velocity_enabled))) { - if (_att_sp_pub != nullptr && (_vehicle_status.is_rotary_wing || _vehicle_status.in_transition_mode)) { - orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp); + !(_control_mode.flag_control_position_enabled || + _control_mode.flag_control_velocity_enabled))) { - } else if (_att_sp_pub == nullptr && (_vehicle_status.is_rotary_wing || _vehicle_status.in_transition_mode)) { - _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp); + if (_att_sp_pub != nullptr) { + orb_publish(_attitude_setpoint_id, _att_sp_pub, &_att_sp); + } else if (_attitude_setpoint_id) { + _att_sp_pub = orb_advertise(_attitude_setpoint_id, &_att_sp); } } diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 8fe70d7604..1b8fb18735 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -164,8 +164,12 @@ ORB_DEFINE(fence_vertex, struct fence_vertex_s); #include "topics/vehicle_attitude_setpoint.h" ORB_DEFINE(vehicle_attitude_setpoint, struct vehicle_attitude_setpoint_s); -ORB_DEFINE(mc_virtual_attitude_setpoint, struct vehicle_attitude_setpoint_s); -ORB_DEFINE(fw_virtual_attitude_setpoint, struct vehicle_attitude_setpoint_s); + +#include "topics/mc_virtual_attitude_setpoint.h" +ORB_DEFINE(mc_virtual_attitude_setpoint, struct mc_virtual_attitude_setpoint_s); + +#include "topics/fw_virtual_attitude_setpoint.h" +ORB_DEFINE(fw_virtual_attitude_setpoint, struct fw_virtual_attitude_setpoint_s); #include "topics/manual_control_setpoint.h" ORB_DEFINE(manual_control_setpoint, struct manual_control_setpoint_s); diff --git a/src/modules/uORB/uORB.h b/src/modules/uORB/uORB.h index 6ead26186b..cfcf39b408 100644 --- a/src/modules/uORB/uORB.h +++ b/src/modules/uORB/uORB.h @@ -366,7 +366,6 @@ __END_DECLS /* Diverse uORB header defines */ //XXX: move to better location #define ORB_ID_VEHICLE_ATTITUDE_CONTROLS ORB_ID(actuator_controls_0) -typedef struct vehicle_attitude_setpoint_s fw_virtual_attitude_setpoint_s; typedef uint8_t arming_state_t; typedef uint8_t main_state_t; typedef uint8_t hil_state_t; diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp index 05c1a525b1..8a5896999e 100644 --- a/src/modules/vtol_att_control/standard.cpp +++ b/src/modules/vtol_att_control/standard.cpp @@ -186,6 +186,9 @@ void Standard::update_vtol_state() void Standard::update_transition_state() { + // copy virtual attitude setpoint to real attitude setpoint + memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s)); + if (_vtol_schedule.flight_mode == TRANSITION_TO_FW) { if (_params_standard.front_trans_dur <= 0.0f) { // just set the final target throttle value @@ -242,11 +245,15 @@ void Standard::update_transition_state() void Standard::update_mc_state() { - // do nothing + // copy virtual attitude setpoint to real attitude setpoint + memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s)); } void Standard::update_fw_state() { + // copy virtual attitude setpoint to real attitude setpoint + memcpy(_v_att_sp, _fw_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s)); + // in fw mode we need the multirotor motors to stop spinning, in backtransition mode we let them spin up again if (!_flag_enable_mc_motors) { set_max_mc(950); diff --git a/src/modules/vtol_att_control/tailsitter.cpp b/src/modules/vtol_att_control/tailsitter.cpp index 7368870847..106f91a8e4 100644 --- a/src/modules/vtol_att_control/tailsitter.cpp +++ b/src/modules/vtol_att_control/tailsitter.cpp @@ -35,18 +35,42 @@ * @file tailsitter.cpp * * @author Roman Bapst +* @author David Vorsin * */ #include "tailsitter.h" #include "vtol_att_control_main.h" -Tailsitter::Tailsitter(VtolAttitudeControl *att_controller) : - VtolType(att_controller), - _airspeed_tot(0), +#define ARSP_YAW_CTRL_DISABLE 7.0f // airspeed at which we stop controlling yaw during a front transition +#define THROTTLE_TRANSITION_MAX 0.25f // maximum added thrust above last value in transition +#define PITCH_TRANSITION_FRONT_P1 -1.1f // pitch angle to switch to TRANSITION_P2 +#define PITCH_TRANSITION_FRONT_P2 -1.2f // pitch angle to switch to FW +#define PITCH_TRANSITION_BACK -0.25f // pitch angle to switch to MC + +Tailsitter::Tailsitter(VtolAttitudeControl *attc) : + VtolType(attc), + _airspeed_tot(0.0f), + _min_front_trans_dur(0.5f), + _loop_perf(perf_alloc(PC_ELAPSED, "vtol_att_control-tailsitter")), _nonfinite_input_perf(perf_alloc(PC_COUNT, "vtol att control-tailsitter nonfinite input")) { + _vtol_schedule.flight_mode = MC_MODE; + _vtol_schedule.transition_start = 0; + + _mc_roll_weight = 1.0f; + _mc_pitch_weight = 1.0f; + _mc_yaw_weight = 1.0f; + + _flag_was_in_trans_mode = false; + + _params_handles_tailsitter.front_trans_dur = param_find("VT_F_TRANS_DUR"); + _params_handles_tailsitter.front_trans_dur_p2 = param_find("VT_TRANS_P2_DUR"); + _params_handles_tailsitter.back_trans_dur = param_find("VT_B_TRANS_DUR"); + _params_handles_tailsitter.airspeed_trans = param_find("VT_ARSP_TRANS"); + _params_handles_tailsitter.airspeed_blend_start = param_find("VT_ARSP_BLEND"); + _params_handles_tailsitter.elevons_mc_lock = param_find("VT_ELEV_MC_LOCK"); } @@ -55,23 +79,170 @@ Tailsitter::~Tailsitter() } +int +Tailsitter::parameters_update() +{ + float v; + int l; + + /* vtol duration of a front transition */ + param_get(_params_handles_tailsitter.front_trans_dur, &v); + _params_tailsitter.front_trans_dur = math::constrain(v, 1.0f, 5.0f); + + /* vtol front transition phase 2 duration */ + param_get(_params_handles_tailsitter.front_trans_dur_p2, &v); + _params_tailsitter.front_trans_dur_p2 = v; + + /* vtol duration of a back transition */ + param_get(_params_handles_tailsitter.back_trans_dur, &v); + _params_tailsitter.back_trans_dur = math::constrain(v, 0.0f, 5.0f); + + /* vtol airspeed at which it is ok to switch to fw mode */ + param_get(_params_handles_tailsitter.airspeed_trans, &v); + _params_tailsitter.airspeed_trans = v; + + /* vtol airspeed at which we start blending mc/fw controls */ + param_get(_params_handles_tailsitter.airspeed_blend_start, &v); + _params_tailsitter.airspeed_blend_start = v; + + /* vtol lock elevons in multicopter */ + param_get(_params_handles_tailsitter.elevons_mc_lock, &l); + _params_tailsitter.elevons_mc_lock = l; + + /* avoid parameters which will lead to zero division in the transition code */ + _params_tailsitter.front_trans_dur = math::max(_params_tailsitter.front_trans_dur, _min_front_trans_dur); + + if (_params_tailsitter.airspeed_trans < _params_tailsitter.airspeed_blend_start + 1.0f) { + _params_tailsitter.airspeed_trans = _params_tailsitter.airspeed_blend_start + 1.0f; + } + + return OK; +} + void Tailsitter::update_vtol_state() { - // simply switch between the two modes - if (!_attc->is_fixed_wing_requested()) { - _vtol_mode = ROTARY_WING; + parameters_update(); - } else { + /* simple logic using a two way switch to perform transitions. + * after flipping the switch the vehicle will start tilting in MC control mode, picking up + * forward speed. After the vehicle has picked up enough and sufficient pitch angle the uav will go into FW mode. + * For the backtransition the pitch is controlled in MC mode again and switches to full MC control reaching the sufficient pitch angle. + */ + + if (_manual_control_sp->aux1 < 0.0f) { + + + switch (_vtol_schedule.flight_mode) { // user switchig to MC mode + case MC_MODE: + break; + + case FW_MODE: + _vtol_schedule.flight_mode = TRANSITION_BACK; + _vtol_schedule.transition_start = hrt_absolute_time(); + break; + + case TRANSITION_FRONT_P1: + // failsafe into multicopter mode + _vtol_schedule.flight_mode = MC_MODE; + break; + + case TRANSITION_FRONT_P2: + // NOT USED + // failsafe into multicopter mode + //_vtol_schedule.flight_mode = MC_MODE; + break; + + case TRANSITION_BACK: + + // check if we have reached pitch angle to switch to MC mode + if (_v_att->pitch >= PITCH_TRANSITION_BACK) { + _vtol_schedule.flight_mode = MC_MODE; + } + + break; + } + + } else { // user switchig to FW mode + + switch (_vtol_schedule.flight_mode) { + case MC_MODE: + // initialise a front transition + _vtol_schedule.flight_mode = TRANSITION_FRONT_P1; + _vtol_schedule.transition_start = hrt_absolute_time(); + break; + + case FW_MODE: + break; + + case TRANSITION_FRONT_P1: + + // check if we have reached airspeed and pitch angle to switch to TRANSITION P2 mode + if ((_airspeed->true_airspeed_m_s >= _params_tailsitter.airspeed_trans + && _v_att->pitch <= PITCH_TRANSITION_FRONT_P1) || !_armed->armed) { + _vtol_schedule.flight_mode = FW_MODE; + //_vtol_schedule.transition_start = hrt_absolute_time(); + } + + break; + + case TRANSITION_FRONT_P2: + + case TRANSITION_BACK: + // failsafe into fixed wing mode + _vtol_schedule.flight_mode = FW_MODE; + + /* **LATER*** if pitch is closer to mc (-45>) + * go to transition P1 + */ + break; + } + } + + // map tailsitter specific control phases to simple control modes + switch (_vtol_schedule.flight_mode) { + case MC_MODE: + _vtol_mode = ROTARY_WING; + _vtol_vehicle_status->vtol_in_trans_mode = false; + _flag_was_in_trans_mode = false; + break; + + case FW_MODE: _vtol_mode = FIXED_WING; + _vtol_vehicle_status->vtol_in_trans_mode = false; + _flag_was_in_trans_mode = false; + break; + + case TRANSITION_FRONT_P1: + _vtol_mode = TRANSITION; + _vtol_vehicle_status->vtol_in_trans_mode = true; + + case TRANSITION_FRONT_P2: + _vtol_mode = TRANSITION; + _vtol_vehicle_status->vtol_in_trans_mode = true; + + case TRANSITION_BACK: + _vtol_mode = TRANSITION; + _vtol_vehicle_status->vtol_in_trans_mode = true; + break; } } + + void Tailsitter::update_mc_state() { + // set idle speed for rotary wing mode if (!flag_idle_mc) { set_idle_mc(); flag_idle_mc = true; } + + _mc_roll_weight = 1.0f; + _mc_pitch_weight = 1.0f; + _mc_yaw_weight = 1.0f; + + // copy virtual attitude setpoint to real attitude setpoint + memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s)); } void Tailsitter::update_fw_state() @@ -80,13 +251,137 @@ void Tailsitter::update_fw_state() set_idle_fw(); flag_idle_mc = false; } + + _mc_roll_weight = 0.0f; + _mc_pitch_weight = 0.0f; + _mc_yaw_weight = 0.0f; + + // copy virtual attitude setpoint to real attitude setpoint + memcpy(_v_att_sp, _fw_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s)); } void Tailsitter::update_transition_state() { + if (!_flag_was_in_trans_mode) { + // save desired heading for transition and last thrust value + _yaw_transition = _v_att_sp->yaw_body; + _pitch_transition_start = _v_att_sp->pitch_body; + _throttle_transition = _v_att_sp->thrust; + _flag_was_in_trans_mode = true; + } + if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P1) { + + /** create time dependant pitch angle set point + 0.2 rad overlap over the switch value*/ + _v_att_sp->pitch_body = _pitch_transition_start - (fabsf(PITCH_TRANSITION_FRONT_P1 - _pitch_transition_start) * + (float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_tailsitter.front_trans_dur * 1000000.0f)); + _v_att_sp->pitch_body = math::constrain(_v_att_sp->pitch_body , PITCH_TRANSITION_FRONT_P1 - 0.2f , + _pitch_transition_start); + + /** create time dependant throttle signal higher than in MC and growing untill P2 switch speed reached */ + if (_airspeed->true_airspeed_m_s <= _params_tailsitter.airspeed_trans) { + _v_att_sp->thrust = _throttle_transition + (fabsf(THROTTLE_TRANSITION_MAX * _throttle_transition) * + (float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_tailsitter.front_trans_dur * 1000000.0f)); + _v_att_sp->thrust = math::constrain(_v_att_sp->thrust , _throttle_transition , + (1.0f + THROTTLE_TRANSITION_MAX) * _throttle_transition); + } + + // disable mc yaw control once the plane has picked up speed + if (_airspeed->true_airspeed_m_s > ARSP_YAW_CTRL_DISABLE) { + _mc_yaw_weight = 0.0f; + + } else { + _mc_yaw_weight = 1.0f; + } + + _mc_roll_weight = 1.0f; + _mc_pitch_weight = 1.0f; + + } else if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P2) { + // the plane is ready to go into fixed wing mode, smoothly switch the actuator controls, keep pitching down + + /** no motor switching */ + + if (flag_idle_mc) { + set_idle_fw(); + flag_idle_mc = false; + } + + /** create time dependant pitch angle set point + 0.2 rad overlap over the switch value*/ + if (_v_att_sp->pitch_body >= (PITCH_TRANSITION_FRONT_P2 - 0.2f)) { + _v_att_sp->pitch_body = PITCH_TRANSITION_FRONT_P1 - + (fabsf(PITCH_TRANSITION_FRONT_P2 - PITCH_TRANSITION_FRONT_P1) * (float)hrt_elapsed_time( + &_vtol_schedule.transition_start) / (_params_tailsitter.front_trans_dur_p2 * 1000000.0f)); + + if (_v_att_sp->pitch_body <= (PITCH_TRANSITION_FRONT_P2 - 0.2f)) { + _v_att_sp->pitch_body = PITCH_TRANSITION_FRONT_P2 - 0.2f; + } + + } + + /** start blending MC and FW controls from pitch -45 to pitch -70 for smooth control takeover*/ + + //_mc_roll_weight = 1.0f - 1.0f * ((float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_tailsitter.front_trans_dur_p2 * 1000000.0f)); + //_mc_pitch_weight = 1.0f - 1.0f * ((float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_tailsitter.front_trans_dur_p2 * 1000000.0f)); + + + _mc_roll_weight = 0.0f; + _mc_pitch_weight = 0.0f; + + /** keep yaw disabled */ + _mc_yaw_weight = 0.0f; + + + } else if (_vtol_schedule.flight_mode == TRANSITION_BACK) { + + if (!flag_idle_mc) { + set_idle_mc(); + flag_idle_mc = true; + } + + /** create time dependant pitch angle set point stating at -pi/2 + 0.2 rad overlap over the switch value*/ + _v_att_sp->pitch_body = M_PI_2_F + _pitch_transition_start + fabsf(PITCH_TRANSITION_BACK + 1.57f) * + (float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_tailsitter.back_trans_dur * 1000000.0f); + _v_att_sp->pitch_body = math::constrain(_v_att_sp->pitch_body , -2.0f , PITCH_TRANSITION_BACK + 0.2f); + + // throttle value is decreesed + _v_att_sp->thrust = _throttle_transition * 0.9f; + + /** keep yaw disabled */ + _mc_yaw_weight = 0.0f; + + /** smoothly move control weight to MC */ + _mc_roll_weight = 1.0f * (float)hrt_elapsed_time(&_vtol_schedule.transition_start) / + (_params_tailsitter.back_trans_dur * 1000000.0f); + _mc_pitch_weight = 1.0f * (float)hrt_elapsed_time(&_vtol_schedule.transition_start) / + (_params_tailsitter.back_trans_dur * 1000000.0f); + + } + + + + + _mc_roll_weight = math::constrain(_mc_roll_weight, 0.0f, 1.0f); + _mc_yaw_weight = math::constrain(_mc_yaw_weight, 0.0f, 1.0f); + _mc_pitch_weight = math::constrain(_mc_pitch_weight, 0.0f, 1.0f); + + // compute desired attitude and thrust setpoint for the transition + + _v_att_sp->timestamp = hrt_absolute_time(); + _v_att_sp->roll_body = 0.0f; + _v_att_sp->yaw_body = _yaw_transition; + _v_att_sp->R_valid = true; + + math::Matrix<3, 3> R_sp; + R_sp.from_euler(_v_att_sp->roll_body, _v_att_sp->pitch_body, _v_att_sp->yaw_body); + memcpy(&_v_att_sp->R_body[0], R_sp.data, sizeof(_v_att_sp->R_body)); + + math::Quaternion q_sp; + q_sp.from_dcm(R_sp); + memcpy(&_v_att_sp->q_d[0], &q_sp.data[0], sizeof(_v_att_sp->q_d)); } + void Tailsitter::update_external_state() { @@ -110,8 +405,7 @@ void Tailsitter::calc_tot_airspeed() _airspeed_tot = _params->arsp_lp_gain * (_airspeed_tot - airspeed_raw) + airspeed_raw; } -void -Tailsitter::scale_mc_output() +void Tailsitter::scale_mc_output() { // scale around tuning airspeed float airspeed; @@ -167,8 +461,7 @@ void Tailsitter::fill_actuator_outputs() // NOTE: There is no mistake in the line below, multicopter yaw axis is controlled by elevon roll actuation! _actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW]; //roll elevon - _actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = - _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH]; //pitch elevon + _actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH]; //pitch elevon } break; @@ -192,6 +485,26 @@ void Tailsitter::fill_actuator_outputs() break; case TRANSITION: + // in transition engines are mixed by weight (BACK TRANSITION ONLY) + _actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_ROLL] + * _mc_roll_weight; + _actuators_out_0->control[actuator_controls_s::INDEX_PITCH] = + _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight; + _actuators_out_0->control[actuator_controls_s::INDEX_YAW] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW] * + _mc_yaw_weight; + _actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = + _actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE]; + + // NOTE: There is no mistake in the line below, multicopter yaw axis is controlled by elevon roll actuation! + _actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = -_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL] + * (1 - _mc_roll_weight); + _actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = + _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight; + // **LATER** + (_actuators_fw_in->control[actuator_controls_s::INDEX_PITCH] + _params->fw_pitch_trim) *(1 - _mc_pitch_weight); + _actuators_out_1->control[actuator_controls_s::INDEX_THROTTLE] = + _actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE]; + break; + case EXTERNAL: // not yet implemented, we are switching brute force at the moment break; diff --git a/src/modules/vtol_att_control/tailsitter.h b/src/modules/vtol_att_control/tailsitter.h index ec8d007567..f68fbe9106 100644 --- a/src/modules/vtol_att_control/tailsitter.h +++ b/src/modules/vtol_att_control/tailsitter.h @@ -35,6 +35,7 @@ * @file tiltrotor.h * * @author Roman Bapst +* @author David Vorsin * */ @@ -42,7 +43,9 @@ #define TAILSITTER_H #include "vtol_type.h" -#include +#include /** is it necsacery? **/ +#include +#include class Tailsitter : public VtolType { @@ -51,21 +54,96 @@ public: Tailsitter(VtolAttitudeControl *_att_controller); ~Tailsitter(); + /** + * Update vtol state. + */ void update_vtol_state(); + + /** + * Update multicopter state. + */ void update_mc_state(); + + /** + * Update fixed wing state. + */ void update_fw_state(); + + /** + * Update transition state. + */ void update_transition_state(); + + /** + * Update external state. + */ void update_external_state(); private: - void fill_actuator_outputs(); - void calc_tot_airspeed(); - void scale_mc_output(); - float _airspeed_tot; + + struct { + float front_trans_dur; /**< duration of first part of front transition */ + float front_trans_dur_p2; + float back_trans_dur; /**< duration of back transition */ + float airspeed_trans; /**< airspeed at which we switch to fw mode after transition */ + float airspeed_blend_start; /**< airspeed at which we start blending mc/fw controls */ + int elevons_mc_lock; /**< lock elevons in multicopter mode */ + + } _params_tailsitter; + + struct { + param_t front_trans_dur; + param_t front_trans_dur_p2; + param_t back_trans_dur; + param_t airspeed_trans; + param_t airspeed_blend_start; + param_t elevons_mc_lock; + + } _params_handles_tailsitter; + + enum vtol_mode { + MC_MODE = 0, /**< vtol is in multicopter mode */ + TRANSITION_FRONT_P1, /**< vtol is in front transition part 1 mode */ + TRANSITION_FRONT_P2, /**< vtol is in front transition part 2 mode */ + TRANSITION_BACK, /**< vtol is in back transition mode */ + FW_MODE /**< vtol is in fixed wing mode */ + }; + + struct { + vtol_mode flight_mode; /**< vtol flight mode, defined by enum vtol_mode */ + hrt_abstime transition_start; /**< absoulte time at which front transition started */ + } _vtol_schedule; + + float _airspeed_tot; /** speed estimation for propwash controlled surfaces */ + + /** not sure about it yet ?! **/ + float _min_front_trans_dur; /**< min possible time in which rotors are rotated into the first position */ + + + /** should this anouncement stay? **/ perf_counter_t _loop_perf; /**< loop performance counter */ perf_counter_t _nonfinite_input_perf; /**< performance counter for non finite input */ + /** + * Speed estimation for propwash controlled surfaces. + */ + void calc_tot_airspeed(); + + + /** is this one still needed? */ + void scale_mc_output(); + + /** + * Write control values to actuator output topics. + */ + void fill_actuator_outputs(); + + /** + * Update parameters. + */ + int parameters_update(); + }; #endif diff --git a/src/modules/vtol_att_control/tailsitter_params.c b/src/modules/vtol_att_control/tailsitter_params.c new file mode 100644 index 0000000000..af38b55693 --- /dev/null +++ b/src/modules/vtol_att_control/tailsitter_params.c @@ -0,0 +1,56 @@ +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file tailsitter_params.c + * Parameters for vtol attitude controller. + * + * @author Roman Bapst + * @author David Vorsin + */ + +#include + +/** + * Duration of front transition phase 2 + * + * Time in seconds it should take for the rotors to rotate forward completely from the point + * when the plane has picked up enough airspeed and is ready to go into fixed wind mode. + * + * @min 0.1 + * @max 2 + * @group VTOL Attitude Control + +PARAM_DEFINE_FLOAT(VT_TRANS_P2_DUR, 0.5f);*/ + + diff --git a/src/modules/vtol_att_control/tiltrotor.cpp b/src/modules/vtol_att_control/tiltrotor.cpp index 270aa065a9..3890a71843 100644 --- a/src/modules/vtol_att_control/tiltrotor.cpp +++ b/src/modules/vtol_att_control/tiltrotor.cpp @@ -56,6 +56,8 @@ Tiltrotor::Tiltrotor(VtolAttitudeControl *attc) : _mc_pitch_weight = 1.0f; _mc_yaw_weight = 1.0f; + _flag_was_in_trans_mode = false; + _params_handles_tiltrotor.front_trans_dur = param_find("VT_F_TRANS_DUR"); _params_handles_tiltrotor.back_trans_dur = param_find("VT_B_TRANS_DUR"); _params_handles_tiltrotor.tilt_mc = param_find("VT_TILT_MC"); @@ -250,6 +252,9 @@ void Tiltrotor::update_vtol_state() void Tiltrotor::update_mc_state() { + // copy virtual attitude setpoint to real attitude setpoint + memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s)); + // make sure motors are not tilted _tilt_control = _params_tiltrotor.tilt_mc; @@ -271,6 +276,9 @@ void Tiltrotor::update_mc_state() void Tiltrotor::update_fw_state() { + // copy virtual attitude setpoint to real attitude setpoint + memcpy(_v_att_sp, _fw_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s)); + // make sure motors are tilted forward _tilt_control = _params_tiltrotor.tilt_fw; @@ -292,6 +300,13 @@ void Tiltrotor::update_fw_state() void Tiltrotor::update_transition_state() { + if (!_flag_was_in_trans_mode) { + // save desired heading for transition and last thrust value + _yaw_transition = _v_att->yaw; + _throttle_transition = _actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE]; + _flag_was_in_trans_mode = true; + } + if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P1) { // for the first part of the transition the rear rotors are enabled if (_rear_motors != ENABLED) { @@ -354,6 +369,9 @@ void Tiltrotor::update_transition_state() _mc_roll_weight = math::constrain(_mc_roll_weight, 0.0f, 1.0f); _mc_yaw_weight = math::constrain(_mc_yaw_weight, 0.0f, 1.0f); + + // copy virtual attitude setpoint to real attitude setpoint (we use multicopter att sp) + memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s)); } void Tiltrotor::update_external_state() 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 0248a20727..b9c2d64fe2 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -41,6 +41,7 @@ * @author Roman Bapst * @author Lorenz Meier * @author Thomas Gubler + * @author David Vorsin * */ #include "vtol_att_control_main.h" @@ -60,6 +61,8 @@ VtolAttitudeControl::VtolAttitudeControl() : //init subscription handlers _v_att_sub(-1), _v_att_sp_sub(-1), + _mc_virtual_att_sp_sub(-1), + _fw_virtual_att_sp_sub(-1), _mc_virtual_v_rates_sp_sub(-1), _fw_virtual_v_rates_sp_sub(-1), _v_control_mode_sub(-1), @@ -75,13 +78,16 @@ VtolAttitudeControl::VtolAttitudeControl() : _actuators_0_pub(nullptr), _actuators_1_pub(nullptr), _vtol_vehicle_status_pub(nullptr), - _v_rates_sp_pub(nullptr) + _v_rates_sp_pub(nullptr), + _v_att_sp_pub(nullptr) { memset(& _vtol_vehicle_status, 0, sizeof(_vtol_vehicle_status)); _vtol_vehicle_status.vtol_in_rw_mode = true; /* start vtol in rotary wing mode*/ memset(&_v_att, 0, sizeof(_v_att)); memset(&_v_att_sp, 0, sizeof(_v_att_sp)); + memset(&_mc_virtual_att_sp, 0, sizeof(_mc_virtual_att_sp)); + memset(&_fw_virtual_att_sp, 0, sizeof(_fw_virtual_att_sp)); memset(&_v_rates_sp, 0, sizeof(_v_rates_sp)); memset(&_mc_virtual_v_rates_sp, 0, sizeof(_mc_virtual_v_rates_sp)); memset(&_fw_virtual_v_rates_sp, 0, sizeof(_fw_virtual_v_rates_sp)); @@ -271,6 +277,36 @@ VtolAttitudeControl::vehicle_airspeed_poll() } } +/** +* Check for attitude set points update. +*/ +void +VtolAttitudeControl::vehicle_attitude_setpoint_poll() +{ + /* check if there is a new setpoint */ + bool updated; + orb_check(_v_att_sp_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(vehicle_attitude_setpoint), _v_att_sp_sub, &_v_att_sp); + } +} + +/** +* Check for attitude update. +*/ +void +VtolAttitudeControl::vehicle_attitude_poll() +{ + /* check if there is a new setpoint */ + bool updated; + orb_check(_v_att_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(vehicle_attitude), _v_att_sub, &_v_att); + } +} + /** * Check for battery updates. */ @@ -319,6 +355,38 @@ VtolAttitudeControl::vehicle_local_pos_poll() } +/** +* Check for mc virtual attitude setpoint updates. +*/ +void +VtolAttitudeControl::mc_virtual_att_sp_poll() +{ + bool updated; + + orb_check(_mc_virtual_att_sp_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(mc_virtual_attitude_setpoint), _mc_virtual_att_sp_sub , &_mc_virtual_att_sp); + } + +} + +/** +* Check for fw virtual attitude setpoint updates. +*/ +void +VtolAttitudeControl::fw_virtual_att_sp_poll() +{ + bool updated; + + orb_check(_fw_virtual_att_sp_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(fw_virtual_attitude_setpoint), _fw_virtual_att_sp_sub , &_fw_virtual_att_sp); + } + +} + /** * Check for command updates. */ @@ -439,6 +507,18 @@ void VtolAttitudeControl::fill_fw_att_rates_sp() _v_rates_sp.thrust = _fw_virtual_v_rates_sp.thrust; } +void VtolAttitudeControl::publish_att_sp() +{ + if (_v_att_sp_pub != nullptr) { + /* publish the attitude setpoint */ + orb_publish(ORB_ID(vehicle_attitude_setpoint), _v_att_sp_pub, &_v_att_sp); + + } else { + /* advertise and publish */ + _v_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_v_att_sp); + } +} + void VtolAttitudeControl::task_main_trampoline(int argc, char *argv[]) { @@ -452,9 +532,12 @@ void VtolAttitudeControl::task_main() /* do subscriptions */ _v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); + _mc_virtual_att_sp_sub = orb_subscribe(ORB_ID(mc_virtual_attitude_setpoint)); + _fw_virtual_att_sp_sub = orb_subscribe(ORB_ID(fw_virtual_attitude_setpoint)); _mc_virtual_v_rates_sp_sub = orb_subscribe(ORB_ID(mc_virtual_rates_setpoint)); _fw_virtual_v_rates_sp_sub = orb_subscribe(ORB_ID(fw_virtual_rates_setpoint)); _v_att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + _v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); _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)); @@ -523,9 +606,13 @@ void VtolAttitudeControl::task_main() _vtol_vehicle_status.fw_permanent_stab = _params.vtol_fw_permanent_stab == 1 ? true : false; + mc_virtual_att_sp_poll(); + 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 actuator_controls_fw_poll(); //Check for changes in fw_attitude_control output vehicle_rates_sp_mc_poll(); @@ -582,6 +669,7 @@ void VtolAttitudeControl::task_main() } else if (_vtol_type->get_mode() == TRANSITION) { // vehicle is doing a transition _vtol_vehicle_status.vtol_in_trans_mode = true; + _vtol_vehicle_status.vtol_in_rw_mode = true; //making mc attitude controller work during transition bool got_new_data = false; @@ -599,6 +687,7 @@ void VtolAttitudeControl::task_main() if (got_new_data) { _vtol_type->update_transition_state(); fill_mc_att_rates_sp(); + publish_att_sp(); } } else if (_vtol_type->get_mode() == EXTERNAL) { @@ -606,6 +695,7 @@ void VtolAttitudeControl::task_main() _vtol_type->update_external_state(); } + publish_att_sp(); _vtol_type->fill_actuator_outputs(); /* Only publish if the proper mode(s) are enabled */ 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 c10f7b0dd7..0e99d58aab 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.h +++ b/src/modules/vtol_att_control/vtol_att_control_main.h @@ -41,6 +41,7 @@ * @author Roman Bapst * @author Lorenz Meier * @author Thomas Gubler + * @author David Vorsin * */ #ifndef VTOL_ATT_CONTROL_MAIN_H @@ -62,6 +63,8 @@ #include #include #include +#include +#include #include #include #include @@ -104,22 +107,24 @@ public: int start(); /* start the task and return OK on success */ bool is_fixed_wing_requested(); - struct vehicle_attitude_s *get_att() {return &_v_att;} - struct vehicle_attitude_setpoint_s *get_att_sp() {return &_v_att_sp;} - struct vehicle_rates_setpoint_s *get_rates_sp() {return &_v_rates_sp;} - struct vehicle_rates_setpoint_s *get_mc_virtual_rates_sp() {return &_mc_virtual_v_rates_sp;} - struct vehicle_rates_setpoint_s *get_fw_virtual_rates_sp() {return &_fw_virtual_v_rates_sp;} - struct manual_control_setpoint_s *get_manual_control_sp() {return &_manual_control_sp;} - struct vehicle_control_mode_s *get_control_mode() {return &_v_control_mode;} - struct vtol_vehicle_status_s *get_vehicle_status() {return &_vtol_vehicle_status;} - struct actuator_controls_s *get_actuators_out0() {return &_actuators_out_0;} - 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 airspeed_s *get_airspeed() {return &_airspeed;} - struct battery_status_s *get_batt_status() {return &_batt_status;} + struct vehicle_attitude_s *get_att() {return &_v_att;} + struct vehicle_attitude_setpoint_s *get_att_sp() {return &_v_att_sp;} + struct mc_virtual_attitude_setpoint_s *get_mc_virtual_att_sp() {return &_mc_virtual_att_sp;} + struct fw_virtual_attitude_setpoint_s *get_fw_virtual_att_sp() {return &_fw_virtual_att_sp;} + struct vehicle_rates_setpoint_s *get_rates_sp() {return &_v_rates_sp;} + struct vehicle_rates_setpoint_s *get_mc_virtual_rates_sp() {return &_mc_virtual_v_rates_sp;} + struct vehicle_rates_setpoint_s *get_fw_virtual_rates_sp() {return &_fw_virtual_v_rates_sp;} + struct manual_control_setpoint_s *get_manual_control_sp() {return &_manual_control_sp;} + struct vehicle_control_mode_s *get_control_mode() {return &_v_control_mode;} + struct vtol_vehicle_status_s *get_vehicle_status() {return &_vtol_vehicle_status;} + struct actuator_controls_s *get_actuators_out0() {return &_actuators_out_0;} + 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 airspeed_s *get_airspeed() {return &_airspeed;} + struct battery_status_s *get_batt_status() {return &_batt_status;} struct Params *get_params() {return &_params;} @@ -132,6 +137,8 @@ private: /* handlers for subscriptions */ int _v_att_sub; //vehicle attitude subscription int _v_att_sp_sub; //vehicle attitude setpoint subscription + int _mc_virtual_att_sp_sub; + int _fw_virtual_att_sp_sub; int _mc_virtual_v_rates_sp_sub; //vehicle rates setpoint subscription int _fw_virtual_v_rates_sp_sub; //vehicle rates setpoint subscription int _v_control_mode_sub; //vehicle control mode subscription @@ -151,9 +158,12 @@ private: orb_advert_t _actuators_1_pub; orb_advert_t _vtol_vehicle_status_pub; orb_advert_t _v_rates_sp_pub; + orb_advert_t _v_att_sp_pub; //*******************data containers*********************************************************** struct vehicle_attitude_s _v_att; //vehicle attitude struct vehicle_attitude_setpoint_s _v_att_sp; //vehicle attitude setpoint + struct mc_virtual_attitude_setpoint_s _mc_virtual_att_sp; // virtual mc attitude setpoint + struct fw_virtual_attitude_setpoint_s _fw_virtual_att_sp; // virtual fw attitude setpoint struct vehicle_rates_setpoint_s _v_rates_sp; //vehicle rates setpoint struct vehicle_rates_setpoint_s _mc_virtual_v_rates_sp; // virtual mc vehicle rates setpoint struct vehicle_rates_setpoint_s _fw_virtual_v_rates_sp; // virtual fw vehicle rates setpoint @@ -187,6 +197,9 @@ private: param_t elevons_mc_lock; } _params_handles; + /* for multicopters it is usual to have a non-zero idle speed of the engines + * for fixed wings we want to have an idle speed of zero since we do not want + * to waste energy when gliding. */ int _transition_command; VtolType *_vtol_type; // base class for different vtol types @@ -202,12 +215,16 @@ 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 void actuator_controls_fw_poll(); //Check for changes in fw_attitude_control output void vehicle_rates_sp_mc_poll(); void vehicle_rates_sp_fw_poll(); void vehicle_local_pos_poll(); // Check for changes in sensor values void vehicle_airspeed_poll(); // Check for changes in airspeed + void vehicle_attitude_setpoint_poll(); //Check for attitude setpoint updates. + void vehicle_attitude_poll(); //Check for attitude updates. void vehicle_battery_poll(); // Check for battery updates void vehicle_cmd_poll(); void parameters_update_poll(); //Check if parameters have changed @@ -215,6 +232,7 @@ private: void fill_mc_att_rates_sp(); void fill_fw_att_rates_sp(); void handle_command(); + void publish_att_sp(); }; #endif diff --git a/src/modules/vtol_att_control/vtol_type.cpp b/src/modules/vtol_att_control/vtol_type.cpp index f57080a2a7..0120cadc84 100644 --- a/src/modules/vtol_att_control/vtol_type.cpp +++ b/src/modules/vtol_att_control/vtol_type.cpp @@ -49,6 +49,8 @@ VtolType::VtolType(VtolAttitudeControl *att_controller) : { _v_att = _attc->get_att(); _v_att_sp = _attc->get_att_sp(); + _mc_virtual_att_sp = _attc->get_mc_virtual_att_sp(); + _fw_virtual_att_sp = _attc->get_fw_virtual_att_sp(); _v_rates_sp = _attc->get_rates_sp(); _mc_virtual_v_rates_sp = _attc->get_mc_virtual_rates_sp(); _fw_virtual_v_rates_sp = _attc->get_fw_virtual_rates_sp(); diff --git a/src/modules/vtol_att_control/vtol_type.h b/src/modules/vtol_att_control/vtol_type.h index d8557110c0..196751016c 100644 --- a/src/modules/vtol_att_control/vtol_type.h +++ b/src/modules/vtol_att_control/vtol_type.h @@ -93,20 +93,22 @@ protected: struct vehicle_attitude_s *_v_att; //vehicle attitude struct vehicle_attitude_setpoint_s *_v_att_sp; //vehicle attitude setpoint + struct mc_virtual_attitude_setpoint_s *_mc_virtual_att_sp; // virtual mc attitude setpoint + struct fw_virtual_attitude_setpoint_s *_fw_virtual_att_sp; // virtual fw attitude setpoint struct vehicle_rates_setpoint_s *_v_rates_sp; //vehicle rates setpoint struct vehicle_rates_setpoint_s *_mc_virtual_v_rates_sp; // virtual mc vehicle rates setpoint struct vehicle_rates_setpoint_s *_fw_virtual_v_rates_sp; // virtual fw 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 vtol_vehicle_status_s *_vtol_vehicle_status; - struct actuator_controls_s *_actuators_out_0; //actuator controls going to the mc mixer - 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 actuator_controls_s *_actuators_out_0; //actuator controls going to the mc mixer + 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 + struct airspeed_s *_airspeed; // airspeed + struct battery_status_s *_batt_status; // battery status struct Params *_params; @@ -116,6 +118,11 @@ protected: float _mc_pitch_weight; // weight for multicopter attitude controller pitch output float _mc_yaw_weight; // weight for multicopter attitude controller yaw output + float _yaw_transition; // yaw angle in which transition will take place + float _pitch_transition_start; // pitch angle at the start of transition (tailsitter) + float _throttle_transition; // throttle value used for the transition phase + bool _flag_was_in_trans_mode; // true if mode has just switched to transition + }; #endif