diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp index 27ed3359f5..234cf3d836 100644 --- a/src/modules/vtol_att_control/standard.cpp +++ b/src/modules/vtol_att_control/standard.cpp @@ -36,6 +36,7 @@ * * @author Simon Wilks * @author Roman Bapst + * @author Andreas Antener * @author Sander Smeets * */ @@ -265,16 +266,9 @@ void Standard::update_transition_state() _mc_throttle_weight = math::constrain(_mc_throttle_weight, 0.0f, 1.0f); } -void Standard::update_mc_state() -{ - // 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)); + VtolType::update_fw_state(); // 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) { @@ -284,10 +278,6 @@ void Standard::update_fw_state() } } -void Standard::update_external_state() -{ -} - /** * Prepare message to acutators with data from mc and fw attitude controllers. An mc attitude weighting will determine * what proportion of control should be applied to each of the control groups (mc and fw). diff --git a/src/modules/vtol_att_control/standard.h b/src/modules/vtol_att_control/standard.h index 19341c1366..0a48bf15ed 100644 --- a/src/modules/vtol_att_control/standard.h +++ b/src/modules/vtol_att_control/standard.h @@ -38,6 +38,7 @@ * * @author Simon Wilks * @author Roman Bapst +* @author Andreas Antener * @author Sander Smeets * */ @@ -56,11 +57,10 @@ public: Standard(VtolAttitudeControl *_att_controller); ~Standard(); - void update_vtol_state(); - void update_mc_state(); - void update_fw_state(); - void update_transition_state(); - void update_external_state(); + virtual void update_vtol_state(); + virtual void update_transition_state(); + virtual void update_fw_state(); + virtual void fill_actuator_outputs(); private: @@ -98,7 +98,6 @@ private: float _pusher_throttle; float _airspeed_trans_blend_margin; - void fill_actuator_outputs(); void set_max_mc(unsigned pwm_value); int parameters_update(); diff --git a/src/modules/vtol_att_control/tailsitter.cpp b/src/modules/vtol_att_control/tailsitter.cpp index a1680cc09c..170625343f 100644 --- a/src/modules/vtol_att_control/tailsitter.cpp +++ b/src/modules/vtol_att_control/tailsitter.cpp @@ -227,39 +227,6 @@ void Tailsitter::update_vtol_state() } } - - -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() -{ - if (flag_idle_mc) { - 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) { @@ -381,12 +348,6 @@ void Tailsitter::update_transition_state() memcpy(&_v_att_sp->q_d[0], &q_sp.data[0], sizeof(_v_att_sp->q_d)); } - -void Tailsitter::update_external_state() -{ - -} - void Tailsitter::calc_tot_airspeed() { float airspeed = math::max(1.0f, _airspeed->true_airspeed_m_s); // prevent numerical drama @@ -439,6 +400,27 @@ void Tailsitter::scale_mc_output() -1.0f, 1.0f); } +void Tailsitter::update_mc_state() +{ + VtolType::update_mc_state(); + + // set idle speed for rotary wing mode + if (!flag_idle_mc) { + set_idle_mc(); + flag_idle_mc = true; + } +} + +void Tailsitter::update_fw_state() +{ + VtolType::update_fw_state(); + + if (flag_idle_mc) { + set_idle_fw(); + flag_idle_mc = false; + } +} + /** * Write data to actuator output topic. */ diff --git a/src/modules/vtol_att_control/tailsitter.h b/src/modules/vtol_att_control/tailsitter.h index f68fbe9106..8bbcca50b2 100644 --- a/src/modules/vtol_att_control/tailsitter.h +++ b/src/modules/vtol_att_control/tailsitter.h @@ -54,35 +54,14 @@ 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(); + virtual void update_vtol_state(); + virtual void update_transition_state(); + virtual void update_mc_state(); + virtual void update_fw_state(); + virtual void fill_actuator_outputs(); private: - - struct { float front_trans_dur; /**< duration of first part of front transition */ float front_trans_dur_p2; @@ -135,11 +114,6 @@ private: /** is this one still needed? */ void scale_mc_output(); - /** - * Write control values to actuator output topics. - */ - void fill_actuator_outputs(); - /** * Update parameters. */ diff --git a/src/modules/vtol_att_control/tiltrotor.cpp b/src/modules/vtol_att_control/tiltrotor.cpp index 81cb1cb710..1f5df2639b 100644 --- a/src/modules/vtol_att_control/tiltrotor.cpp +++ b/src/modules/vtol_att_control/tiltrotor.cpp @@ -252,8 +252,7 @@ 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)); + VtolType::update_mc_state(); // make sure motors are not tilted _tilt_control = _params_tiltrotor.tilt_mc; @@ -268,16 +267,11 @@ void Tiltrotor::update_mc_state() set_idle_mc(); flag_idle_mc = true; } - - _mc_roll_weight = 1.0f; - _mc_pitch_weight = 1.0f; - _mc_yaw_weight = 1.0f; } 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)); + VtolType::update_fw_state(); // make sure motors are tilted forward _tilt_control = _params_tiltrotor.tilt_fw; @@ -292,10 +286,6 @@ void Tiltrotor::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; } void Tiltrotor::update_transition_state() @@ -374,11 +364,6 @@ void Tiltrotor::update_transition_state() memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s)); } -void Tiltrotor::update_external_state() -{ - -} - /** * Write data to actuator output topic. */ diff --git a/src/modules/vtol_att_control/tiltrotor.h b/src/modules/vtol_att_control/tiltrotor.h index 507920cb42..e18d2ca0f6 100644 --- a/src/modules/vtol_att_control/tiltrotor.h +++ b/src/modules/vtol_att_control/tiltrotor.h @@ -52,30 +52,11 @@ public: Tiltrotor(VtolAttitudeControl *_att_controller); ~Tiltrotor(); - /** - * 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(); + virtual void update_vtol_state(); + virtual void update_transition_state(); + virtual void fill_actuator_outputs(); + virtual void update_mc_state(); + virtual void update_fw_state(); private: @@ -143,11 +124,6 @@ private: */ bool is_motor_off_channel(const int channel); - /** - * Write control values to actuator output topics. - */ - void fill_actuator_outputs(); - /** * Adjust the state of the rear motors. In fw mode they shouldn't spin. */ 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 10724d7ea3..770bcf0a26 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -79,6 +79,7 @@ VtolAttitudeControl::VtolAttitudeControl() : _airspeed_sub(-1), _battery_status_sub(-1), _vehicle_cmd_sub(-1), + _vehicle_status_sub(-1), //init publication handlers _actuators_0_pub(nullptr), @@ -109,6 +110,7 @@ VtolAttitudeControl::VtolAttitudeControl() : memset(&_airspeed, 0, sizeof(_airspeed)); memset(&_batt_status, 0, sizeof(_batt_status)); memset(&_vehicle_cmd, 0, sizeof(_vehicle_cmd)); + memset(&_vehicle_status, 0, sizeof(_vehicle_status)); _params.idle_pwm_mc = PWM_DEFAULT_MIN; _params.vtol_motor_count = 0; @@ -408,6 +410,21 @@ VtolAttitudeControl::vehicle_cmd_poll() } } +/** +* Check for vehicle status updates. +*/ +void +VtolAttitudeControl::vehicle_status_poll() +{ + bool updated; + + orb_check(_vehicle_status_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub , &_vehicle_status); + } +} + /** * Check received command */ @@ -578,6 +595,7 @@ void VtolAttitudeControl::task_main() _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); _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)); _actuator_inputs_mc = orb_subscribe(ORB_ID(actuator_controls_virtual_mc)); _actuator_inputs_fw = orb_subscribe(ORB_ID(actuator_controls_virtual_fw)); @@ -664,6 +682,7 @@ void VtolAttitudeControl::task_main() vehicle_airspeed_poll(); vehicle_battery_poll(); vehicle_cmd_poll(); + vehicle_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 0fa934abaa..38772cf324 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.h +++ b/src/modules/vtol_att_control/vtol_att_control_main.h @@ -83,6 +83,7 @@ #include #include #include +#include #include #include #include @@ -119,7 +120,7 @@ public: struct fw_virtual_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 vtol_vehicle_status_s *get_vtol_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;} @@ -128,6 +129,7 @@ public: 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 Params *get_params() {return &_params;} @@ -153,6 +155,7 @@ private: int _airspeed_sub; // airspeed subscription int _battery_status_sub; // battery status subscription int _vehicle_cmd_sub; + int _vehicle_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 @@ -164,25 +167,26 @@ private: 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 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 battery_status_s _batt_status; // battery status - struct vehicle_command_s _vehicle_cmd; + 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 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 vehicle_command_s _vehicle_cmd; + struct vehicle_status_s _vehicle_status; Params _params; // struct holding the parameters @@ -233,6 +237,7 @@ private: void vehicle_battery_poll(); // Check for battery updates void vehicle_cmd_poll(); void parameters_update_poll(); //Check if parameters have changed + void vehicle_status_poll(); int parameters_update(); //Update local paraemter cache void fill_mc_att_rates_sp(); void fill_fw_att_rates_sp(); diff --git a/src/modules/vtol_att_control/vtol_type.cpp b/src/modules/vtol_att_control/vtol_type.cpp index db4c472e0c..335b37bc43 100644 --- a/src/modules/vtol_att_control/vtol_type.cpp +++ b/src/modules/vtol_att_control/vtol_type.cpp @@ -56,7 +56,7 @@ VtolType::VtolType(VtolAttitudeControl *att_controller) : _fw_virtual_v_rates_sp = _attc->get_fw_virtual_rates_sp(); _manual_control_sp = _attc->get_manual_control_sp(); _v_control_mode = _attc->get_control_mode(); - _vtol_vehicle_status = _attc->get_vehicle_status(); + _vtol_vehicle_status = _attc->get_vtol_vehicle_status(); _actuators_out_0 = _attc->get_actuators_out0(); _actuators_out_1 = _attc->get_actuators_out1(); _actuators_mc_in = _attc->get_actuators_mc_in(); @@ -65,6 +65,7 @@ VtolType::VtolType(VtolAttitudeControl *att_controller) : _local_pos = _attc->get_local_pos(); _airspeed = _attc->get_airspeed(); _batt_status = _attc->get_batt_status(); + _vehicle_status = _attc->get_vehicle_status(); _params = _attc->get_params(); flag_idle_mc = true; @@ -132,3 +133,23 @@ void VtolType::set_idle_fw() px4_close(fd); } + +void VtolType::update_mc_state() +{ + // copy virtual attitude setpoint to real attitude setpoint + memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s)); + + _mc_roll_weight = 1.0f; + _mc_pitch_weight = 1.0f; + _mc_yaw_weight = 1.0f; +} + +void VtolType::update_fw_state() +{ + // copy virtual attitude setpoint to real attitude setpoint + memcpy(_v_att_sp, _fw_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s)); + + _mc_roll_weight = 0.0f; + _mc_pitch_weight = 0.0f; + _mc_yaw_weight = 0.0f; +} diff --git a/src/modules/vtol_att_control/vtol_type.h b/src/modules/vtol_att_control/vtol_type.h index a0a25cad2f..7809d869b8 100644 --- a/src/modules/vtol_att_control/vtol_type.h +++ b/src/modules/vtol_att_control/vtol_type.h @@ -76,11 +76,34 @@ public: virtual ~VtolType(); + /** + * Update vtol state. + */ virtual void update_vtol_state() = 0; - virtual void update_mc_state() = 0; - virtual void update_fw_state() = 0; + + /** + * Update transition state. + */ virtual void update_transition_state() = 0; - virtual void update_external_state() = 0; + + /** + * Update multicopter state. + */ + virtual void update_mc_state(); + + /** + * Update fixed wing state. + */ + virtual void update_fw_state(); + + /** + * Update external state. + */ + virtual void update_external_state() {}; + + /** + * Write control values to actuator output topics. + */ virtual void fill_actuator_outputs() = 0; void set_idle_mc(); @@ -92,26 +115,27 @@ protected: VtolAttitudeControl *_attc; mode _vtol_mode; - 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 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 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 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 vehicle_status_s *_vehicle_status; // vehicle status from commander app - 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"