From 0a0a074194705329d19e08f6441a7d6e38d29535 Mon Sep 17 00:00:00 2001 From: tumbili Date: Mon, 7 Sep 2015 09:22:01 +0200 Subject: [PATCH] use virtual attitude setpoint Conflicts: src/modules/vtol_att_control/tailsitter.cpp src/modules/vtol_att_control/tiltrotor.cpp src/modules/vtol_att_control/vtol_att_control_main.cpp src/modules/vtol_att_control/vtol_att_control_main.h src/modules/vtol_att_control/vtol_type.h --- msg/fw_virtual_attitude_setpoint.msg | 23 +++++++++ msg/mc_virtual_attitude_setpoint.msg | 23 +++++++++ src/modules/uORB/objects_common.cpp | 8 +++- src/modules/uORB/uORB.h | 1 - src/modules/vtol_att_control/standard.cpp | 11 ++++- src/modules/vtol_att_control/tailsitter.cpp | 6 +++ src/modules/vtol_att_control/tiltrotor.cpp | 24 ++++------ .../vtol_att_control_main.cpp | 48 +++++++++++++++++-- .../vtol_att_control/vtol_att_control_main.h | 13 ++++- src/modules/vtol_att_control/vtol_type.cpp | 2 + src/modules/vtol_att_control/vtol_type.h | 18 +++---- 11 files changed, 143 insertions(+), 34 deletions(-) create mode 100644 msg/fw_virtual_attitude_setpoint.msg create mode 100644 msg/mc_virtual_attitude_setpoint.msg 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/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..5a329d52f9 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,12 +245,16 @@ 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() { - // in fw mode we need the multirotor motors to stop spinning, in backtransition mode we let them spin up again + // 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); set_idle_fw(); // force them to stop, not just idle diff --git a/src/modules/vtol_att_control/tailsitter.cpp b/src/modules/vtol_att_control/tailsitter.cpp index c557936b15..827e3ec617 100644 --- a/src/modules/vtol_att_control/tailsitter.cpp +++ b/src/modules/vtol_att_control/tailsitter.cpp @@ -231,6 +231,9 @@ void Tailsitter::update_mc_state() _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() @@ -243,6 +246,9 @@ void Tailsitter::update_fw_state() _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() diff --git a/src/modules/vtol_att_control/tiltrotor.cpp b/src/modules/vtol_att_control/tiltrotor.cpp index c4fa37eb2c..24ff7ece2a 100644 --- a/src/modules/vtol_att_control/tiltrotor.cpp +++ b/src/modules/vtol_att_control/tiltrotor.cpp @@ -252,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; @@ -273,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; @@ -363,22 +369,8 @@ 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); - // compute desired attitude and thrust setpoint for the transition - _v_att_sp->timestamp = hrt_absolute_time(); - _v_att_sp->roll_body = 0; - _v_att_sp->pitch_body = 0; - _v_att_sp->yaw_body = _yaw_transition; - _v_att_sp->thrust = _throttle_transition; - - 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)); - _v_att_sp->R_valid = true; - - math::Quaternion q_sp; - q_sp.from_dcm(R_sp); - _v_att_sp->q_d_valid = true; - memcpy(&_v_att_sp->q_d[0], &q_sp.data[0], sizeof(_v_att_sp->q_d)); + // 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 142c2a26ff..80bb62eccd 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -60,7 +60,9 @@ VtolAttitudeControl::VtolAttitudeControl() : //init subscription handlers _v_att_sub(-1), - _v_att_sp_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), @@ -84,6 +86,8 @@ VtolAttitudeControl::VtolAttitudeControl() : _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)); @@ -351,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. */ @@ -471,7 +507,7 @@ void VtolAttitudeControl::fill_fw_att_rates_sp() _v_rates_sp.thrust = _fw_virtual_v_rates_sp.thrust; } -void VtolAttitudeControl::publish_transition_att_sp() +void VtolAttitudeControl::publish_att_sp() { if (_v_att_sp_pub != nullptr) { /* publish the attitude setpoint */ @@ -494,6 +530,9 @@ void VtolAttitudeControl::task_main() fflush(stdout); /* 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)); @@ -566,6 +605,8 @@ 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. @@ -645,7 +686,7 @@ void VtolAttitudeControl::task_main() if (got_new_data) { _vtol_type->update_transition_state(); fill_mc_att_rates_sp(); - publish_transition_att_sp(); + publish_att_sp(); } } else if (_vtol_type->get_mode() == EXTERNAL) { @@ -653,6 +694,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 9485d23d4f..c7c51247ae 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.h +++ b/src/modules/vtol_att_control/vtol_att_control_main.h @@ -63,6 +63,8 @@ #include #include #include +#include +#include #include #include #include @@ -107,6 +109,8 @@ public: 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;} @@ -124,7 +128,6 @@ public: struct Params *get_params() {return &_params;} - private: //******************flags & handlers****************************************************** bool _task_should_exit; @@ -133,6 +136,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 @@ -156,6 +161,8 @@ private: //*******************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 @@ -204,6 +211,8 @@ 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(); @@ -219,7 +228,7 @@ private: void fill_mc_att_rates_sp(); void fill_fw_att_rates_sp(); void handle_command(); - void publish_transition_att_sp(); + 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 5c112ac207..196751016c 100644 --- a/src/modules/vtol_att_control/vtol_type.h +++ b/src/modules/vtol_att_control/vtol_type.h @@ -91,13 +91,15 @@ 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 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 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) @@ -117,8 +119,8 @@ protected: float _mc_yaw_weight; // weight for multicopter attitude controller yaw output float _yaw_transition; // yaw angle in which transition will take place - float _throttle_transition; // throttle value used for the transition phase 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 };