From 3773eaad99f9ac359b48789177e4d475bc29f43b Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Tue, 9 Feb 2016 22:23:59 +0100 Subject: [PATCH] VTOL/TECS: publish transition throttle after transition until tecs is initialized and fw pos ctl is publishing att setpoint --- src/modules/vtol_att_control/standard.cpp | 7 +++ src/modules/vtol_att_control/standard.h | 1 + .../vtol_att_control_main.cpp | 19 ++++++ .../vtol_att_control/vtol_att_control_main.h | 61 ++++++++++--------- src/modules/vtol_att_control/vtol_type.cpp | 16 +++++ src/modules/vtol_att_control/vtol_type.h | 14 ++++- 6 files changed, 87 insertions(+), 31 deletions(-) diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp index 234cf3d836..2c4f84ab30 100644 --- a/src/modules/vtol_att_control/standard.cpp +++ b/src/modules/vtol_att_control/standard.cpp @@ -169,6 +169,7 @@ void Standard::update_vtol_state() // we can turn off the multirotor motors now _flag_enable_mc_motors = false; // don't set pusher throttle here as it's being ramped up elsewhere + _trans_finished_ts = hrt_absolute_time(); } } else if (_vtol_schedule.flight_mode == TRANSITION_TO_MC) { @@ -316,6 +317,12 @@ void Standard::fill_actuator_outputs() } } +void +Standard::waiting_on_fw_ctl() +{ + _v_att_sp->thrust = _pusher_throttle; +}; + /** * Disable all multirotor motors when in fw mode. */ diff --git a/src/modules/vtol_att_control/standard.h b/src/modules/vtol_att_control/standard.h index 0a48bf15ed..14910556bd 100644 --- a/src/modules/vtol_att_control/standard.h +++ b/src/modules/vtol_att_control/standard.h @@ -61,6 +61,7 @@ public: virtual void update_transition_state(); virtual void update_fw_state(); virtual void fill_actuator_outputs(); + virtual void waiting_on_fw_ctl(); private: 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 770bcf0a26..615c41911f 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -80,6 +80,7 @@ VtolAttitudeControl::VtolAttitudeControl() : _battery_status_sub(-1), _vehicle_cmd_sub(-1), _vehicle_status_sub(-1), + _tecs_status_sub(-1), //init publication handlers _actuators_0_pub(nullptr), @@ -111,6 +112,7 @@ VtolAttitudeControl::VtolAttitudeControl() : memset(&_batt_status, 0, sizeof(_batt_status)); memset(&_vehicle_cmd, 0, sizeof(_vehicle_cmd)); memset(&_vehicle_status, 0, sizeof(_vehicle_status)); + memset(&_tecs_status, 0, sizeof(_tecs_status)); _params.idle_pwm_mc = PWM_DEFAULT_MIN; _params.vtol_motor_count = 0; @@ -425,6 +427,21 @@ VtolAttitudeControl::vehicle_status_poll() } } +/** +* Check for TECS status updates. +*/ +void +VtolAttitudeControl::tecs_status_poll() +{ + bool updated; + + orb_check(_tecs_status_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(tecs_status), _tecs_status_sub , &_tecs_status); + } +} + /** * Check received command */ @@ -596,6 +613,7 @@ void VtolAttitudeControl::task_main() _battery_status_sub = orb_subscribe(ORB_ID(battery_status)); _vehicle_cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); _vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); + _tecs_status_sub = orb_subscribe(ORB_ID(tecs_status)); _actuator_inputs_mc = orb_subscribe(ORB_ID(actuator_controls_virtual_mc)); _actuator_inputs_fw = orb_subscribe(ORB_ID(actuator_controls_virtual_fw)); @@ -683,6 +701,7 @@ void VtolAttitudeControl::task_main() vehicle_battery_poll(); vehicle_cmd_poll(); vehicle_status_poll(); + tecs_status_poll(); // update the vtol state machine which decides which mode we are in _vtol_type->update_vtol_state(); 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 38772cf324..0fa60fa975 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.h +++ b/src/modules/vtol_att_control/vtol_att_control_main.h @@ -84,6 +84,7 @@ #include #include #include +#include #include #include #include @@ -111,10 +112,10 @@ public: bool is_fixed_wing_requested(); void abort_front_transition(); - struct vehicle_attitude_s *get_att() {return &_v_att;} + 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 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 mc_virtual_rates_setpoint_s *get_mc_virtual_rates_sp() {return &_mc_virtual_v_rates_sp;} struct fw_virtual_rates_setpoint_s *get_fw_virtual_rates_sp() {return &_fw_virtual_v_rates_sp;} @@ -125,13 +126,14 @@ 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 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_status_s *get_vehicle_status() {return &_vehicle_status;} + struct airspeed_s *get_airspeed() {return &_airspeed;} + struct battery_status_s *get_batt_status() {return &_batt_status;} + struct vehicle_status_s *get_vehicle_status() {return &_vehicle_status;} + struct tecs_status_s *get_tecs_status() {return &_tecs_status;} - struct Params *get_params() {return &_params;} + struct Params *get_params() {return &_params;} private: @@ -141,21 +143,22 @@ private: int _mavlink_fd; // mavlink log device /* handlers for subscriptions */ - int _v_att_sub; //vehicle attitude subscription - int _v_att_sp_sub; //vehicle attitude setpoint subscription + 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 - int _params_sub; //parameter updates subscription - int _manual_control_sp_sub; //manual control setpoint subscription - int _armed_sub; //arming status subscription + 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 + 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 _airspeed_sub; // airspeed subscription int _battery_status_sub; // battery status subscription int _vehicle_cmd_sub; int _vehicle_status_sub; + int _tecs_status_sub; int _actuator_inputs_mc; //topic on which the mc_att_controller publishes actuator inputs int _actuator_inputs_fw; //topic on which the fw_att_controller publishes actuator inputs @@ -168,25 +171,26 @@ private: 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 mc_virtual_rates_setpoint_s _mc_virtual_v_rates_sp; // virtual mc vehicle rates setpoint - struct fw_virtual_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 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 mc_virtual_rates_setpoint_s _mc_virtual_v_rates_sp; // virtual mc vehicle rates setpoint + struct fw_virtual_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 vehicle_local_position_s _local_pos; - struct airspeed_s _airspeed; // airspeed + struct vehicle_local_position_s _local_pos; + struct airspeed_s _airspeed; // airspeed struct battery_status_s _batt_status; // battery status struct vehicle_command_s _vehicle_cmd; - struct vehicle_status_s _vehicle_status; + struct vehicle_status_s _vehicle_status; + struct tecs_status_s _tecs_status; Params _params; // struct holding the parameters @@ -236,6 +240,7 @@ private: void vehicle_attitude_poll(); //Check for attitude updates. void vehicle_battery_poll(); // Check for battery updates void vehicle_cmd_poll(); + void tecs_status_poll(); void parameters_update_poll(); //Check if parameters have changed void vehicle_status_poll(); int parameters_update(); //Update local paraemter cache diff --git a/src/modules/vtol_att_control/vtol_type.cpp b/src/modules/vtol_att_control/vtol_type.cpp index 335b37bc43..7495d45a8f 100644 --- a/src/modules/vtol_att_control/vtol_type.cpp +++ b/src/modules/vtol_att_control/vtol_type.cpp @@ -35,6 +35,7 @@ * @file vtol_type.cpp * * @author Roman Bapst +* @author Andreas Antener * */ @@ -66,6 +67,7 @@ VtolType::VtolType(VtolAttitudeControl *att_controller) : _airspeed = _attc->get_airspeed(); _batt_status = _attc->get_batt_status(); _vehicle_status = _attc->get_vehicle_status(); + _tecs_status = _attc->get_tecs_status(); _params = _attc->get_params(); flag_idle_mc = true; @@ -152,4 +154,18 @@ void VtolType::update_fw_state() _mc_roll_weight = 0.0f; _mc_pitch_weight = 0.0f; _mc_yaw_weight = 0.0f; + + // tecs didn't publish an update yet after the transition + if (_tecs_status->timestamp < _trans_finished_ts) { + _tecs_running = false; + + } else if (!_tecs_running) { + _tecs_running = true; + _tecs_running_ts = hrt_absolute_time(); + } + + // tecs didn't publish yet and the position controller didn't publish yet AFTER tecs + if (!_tecs_running || (_tecs_running && _fw_virtual_att_sp->timestamp <= _tecs_running_ts)) { + waiting_on_fw_ctl(); + } } diff --git a/src/modules/vtol_att_control/vtol_type.h b/src/modules/vtol_att_control/vtol_type.h index 7809d869b8..854f227bce 100644 --- a/src/modules/vtol_att_control/vtol_type.h +++ b/src/modules/vtol_att_control/vtol_type.h @@ -36,6 +36,7 @@ * * @author Roman Bapst * @author Sander Smeets +* @author Andreas Antener * */ @@ -43,6 +44,7 @@ #define VTOL_TYPE_H #include +#include struct Params { int idle_pwm_mc; // pwm value for idle in mc mode @@ -106,6 +108,8 @@ public: */ virtual void fill_actuator_outputs() = 0; + virtual void waiting_on_fw_ctl() {}; + void set_idle_mc(); void set_idle_fw(); @@ -130,12 +134,13 @@ protected: 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 vehicle_local_position_s *_local_pos; + struct airspeed_s *_airspeed; // airspeed struct battery_status_s *_batt_status; // battery status struct vehicle_status_s *_vehicle_status; // vehicle status from commander app + struct tecs_status_s *_tecs_status; - struct Params *_params; + struct Params *_params; bool flag_idle_mc; //false = "idle is set for fixed wing mode"; true = "idle is set for multicopter mode" @@ -149,6 +154,9 @@ protected: 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 + hrt_abstime _trans_finished_ts; + bool _tecs_running; + hrt_abstime _tecs_running_ts; };