From c5e0bf1c2f4f2cfbc3aa95bfbb14c7b45778b1e0 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sun, 9 Apr 2017 13:17:20 -0400 Subject: [PATCH] uORB combine VTOL FW and MC virtual topics (#7008) --- msg/CMakeLists.txt | 8 +-- msg/fw_virtual_attitude_setpoint.msg | 36 ------------ msg/fw_virtual_rates_setpoint.msg | 15 ----- msg/mc_virtual_attitude_setpoint.msg | 36 ------------ msg/mc_virtual_rates_setpoint.msg | 15 ----- msg/position_setpoint_triplet.msg | 2 - msg/vehicle_attitude_setpoint.msg | 13 +---- msg/vehicle_rates_setpoint.msg | 12 +--- .../fw_att_control/fw_att_control_main.cpp | 2 - .../fw_pos_control_l1_main.cpp | 1 - .../mc_att_control/mc_att_control_main.cpp | 56 ++++++++----------- .../mc_pos_control/mc_pos_control_main.cpp | 34 ++++------- src/modules/navigator/navigator_main.cpp | 56 +++++++------------ src/modules/sdlog2/sdlog2.c | 9 --- src/modules/sdlog2/sdlog2_messages.h | 3 +- .../vtol_att_control/vtol_att_control_main.h | 29 +++------- src/modules/vtol_att_control/vtol_type.h | 10 ++-- 17 files changed, 74 insertions(+), 263 deletions(-) delete mode 100644 msg/fw_virtual_attitude_setpoint.msg delete mode 100644 msg/fw_virtual_rates_setpoint.msg delete mode 100644 msg/mc_virtual_attitude_setpoint.msg delete mode 100644 msg/mc_virtual_rates_setpoint.msg diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 8900b678b7..3f91778971 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -41,8 +41,8 @@ set(msg_file_names att_pos_mocap.msg battery_status.msg camera_trigger.msg - commander_state.msg collision_report.msg + commander_state.msg control_state.msg cpuload.msg debug_key_value.msg @@ -59,8 +59,6 @@ set(msg_file_names filtered_bottom_flow.msg follow_target.msg fw_pos_ctrl_status.msg - fw_virtual_attitude_setpoint.msg - fw_virtual_rates_setpoint.msg geofence_result.msg gps_dump.msg gps_inject_data.msg @@ -72,8 +70,6 @@ set(msg_file_names manual_control_setpoint.msg mavlink_log.msg mc_att_ctrl_status.msg - mc_virtual_attitude_setpoint.msg - mc_virtual_rates_setpoint.msg mission.msg mission_result.msg mount_orientation.msg @@ -112,8 +108,8 @@ set(msg_file_names ulog_stream_ack.msg vehicle_attitude.msg vehicle_attitude_setpoint.msg - vehicle_command_ack.msg vehicle_command.msg + vehicle_command_ack.msg vehicle_control_mode.msg vehicle_force_setpoint.msg vehicle_global_position.msg diff --git a/msg/fw_virtual_attitude_setpoint.msg b/msg/fw_virtual_attitude_setpoint.msg deleted file mode 100644 index ba456c25d4..0000000000 --- a/msg/fw_virtual_attitude_setpoint.msg +++ /dev/null @@ -1,36 +0,0 @@ -############################################################################################### -# The vehicle_attitude_setpoint.msg needs to be in sync with the virtual setpoint messages -# -# Please keep the following messages identical; -# vehicle_attitude_setpoint.msg -# mc_virtual_attitude_setpoint.msg -# fw_virtual_attitude_setpoint.msg -# -############################################################################################### - - -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) - -# 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 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) - -bool fw_control_yaw # control heading with rudder (used for auto takeoff on runway) -bool disable_mc_yaw_control # control yaw for mc (used for vtol weather-vane mode) - -bool apply_flaps - -float32 landing_gear - -# WAS vehicle_attitude_setpoint mc_virtual_attitude_setpoint fw_virtual_attitude_setpoint -# TOPICS fw_virtual_attitude_setpoint diff --git a/msg/fw_virtual_rates_setpoint.msg b/msg/fw_virtual_rates_setpoint.msg deleted file mode 100644 index 369b68270a..0000000000 --- a/msg/fw_virtual_rates_setpoint.msg +++ /dev/null @@ -1,15 +0,0 @@ -############################################################################################### -# The vehicle_rates_setpoint.msg needs to be in sync with the virtual setpoint messages -# -# Please keep the following messages identical; -# vehicle_rates_setpoint.msg -# mc_virtual_rates_setpoint.msg -# fw_virtual_rates_setpoint.msg -# -############################################################################################### - - -float32 roll # body angular rates in NED frame -float32 pitch # body angular rates in NED frame -float32 yaw # body angular rates in NED frame -float32 thrust # thrust normalized to 0..1 diff --git a/msg/mc_virtual_attitude_setpoint.msg b/msg/mc_virtual_attitude_setpoint.msg deleted file mode 100644 index aaea9f951d..0000000000 --- a/msg/mc_virtual_attitude_setpoint.msg +++ /dev/null @@ -1,36 +0,0 @@ -############################################################################################### -# The vehicle_attitude_setpoint.msg needs to be in sync with the virtual setpoint messages -# -# Please keep the following messages identical; -# vehicle_attitude_setpoint.msg -# mc_virtual_attitude_setpoint.msg -# fw_virtual_attitude_setpoint.msg -# -############################################################################################### - - -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) - -# 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 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) - -bool fw_control_yaw # control heading with rudder (used for auto takeoff on runway) -bool disable_mc_yaw_control # control yaw for mc (used for vtol weather-vane mode) - -bool apply_flaps - -float32 landing_gear - -# WAS vehicle_attitude_setpoint mc_virtual_attitude_setpoint fw_virtual_attitude_setpoint -# TOPICS mc_virtual_attitude_setpoint diff --git a/msg/mc_virtual_rates_setpoint.msg b/msg/mc_virtual_rates_setpoint.msg deleted file mode 100644 index 369b68270a..0000000000 --- a/msg/mc_virtual_rates_setpoint.msg +++ /dev/null @@ -1,15 +0,0 @@ -############################################################################################### -# The vehicle_rates_setpoint.msg needs to be in sync with the virtual setpoint messages -# -# Please keep the following messages identical; -# vehicle_rates_setpoint.msg -# mc_virtual_rates_setpoint.msg -# fw_virtual_rates_setpoint.msg -# -############################################################################################### - - -float32 roll # body angular rates in NED frame -float32 pitch # body angular rates in NED frame -float32 yaw # body angular rates in NED frame -float32 thrust # thrust normalized to 0..1 diff --git a/msg/position_setpoint_triplet.msg b/msg/position_setpoint_triplet.msg index 8717f65d0f..51ac92dfdf 100644 --- a/msg/position_setpoint_triplet.msg +++ b/msg/position_setpoint_triplet.msg @@ -4,5 +4,3 @@ px4/position_setpoint previous px4/position_setpoint current px4/position_setpoint next - -uint8 nav_state # report the navigation state diff --git a/msg/vehicle_attitude_setpoint.msg b/msg/vehicle_attitude_setpoint.msg index cb2f62576e..45abc7e818 100644 --- a/msg/vehicle_attitude_setpoint.msg +++ b/msg/vehicle_attitude_setpoint.msg @@ -1,13 +1,3 @@ -############################################################################################### -# The vehicle_attitude_setpoint.msg needs to be in sync with the virtual setpoint messages -# -# Please keep the following messages identical; -# vehicle_attitude_setpoint.msg -# mc_virtual_attitude_setpoint.msg -# fw_virtual_attitude_setpoint.msg -# -############################################################################################### - float32 roll_body # body angle in NED frame float32 pitch_body # body angle in NED frame @@ -32,5 +22,4 @@ bool apply_flaps float32 landing_gear -# WAS vehicle_attitude_setpoint mc_virtual_attitude_setpoint fw_virtual_attitude_setpoint -# TOPICS vehicle_attitude_setpoint +# TOPICS vehicle_attitude_setpoint mc_virtual_attitude_setpoint fw_virtual_attitude_setpoint diff --git a/msg/vehicle_rates_setpoint.msg b/msg/vehicle_rates_setpoint.msg index 369b68270a..13fd4d4b6c 100644 --- a/msg/vehicle_rates_setpoint.msg +++ b/msg/vehicle_rates_setpoint.msg @@ -1,15 +1,7 @@ -############################################################################################### -# The vehicle_rates_setpoint.msg needs to be in sync with the virtual setpoint messages -# -# Please keep the following messages identical; -# vehicle_rates_setpoint.msg -# mc_virtual_rates_setpoint.msg -# fw_virtual_rates_setpoint.msg -# -############################################################################################### - float32 roll # body angular rates in NED frame float32 pitch # body angular rates in NED frame float32 yaw # body angular rates in NED frame float32 thrust # thrust normalized to 0..1 + +# TOPICS vehicle_rates_setpoint mc_virtual_rates_setpoint fw_virtual_rates_setpoint 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 f37e8b6981..9e4900a76e 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -58,8 +58,6 @@ #include #include #include -#include -#include #include #include #include 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 2be826c46b..823971e508 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 @@ -88,7 +88,6 @@ #include #include #include -#include #include #include #include diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 27584d199b..2b397934c2 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -53,45 +53,35 @@ * If rotation matrix setpoint is invalid it will be generated from Euler angles for compatibility with old position controllers. */ +#include +#include +#include +#include +#include #include #include -#include #include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include #include +#include #include #include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include /** * Multicopter attitude control app start / stop handling function 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 b8816d535a..69eebf4082 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -53,36 +53,26 @@ #include #include #include -#include -#include -#include -#include -#include -#include -#include -#include #include -#include -#include -#include -#include -#include -#include #include -#include -#include -#include -#include -#include -#include +#include #include +#include +#include +#include +#include +#include +#include +#include +#include +#include #include -#include -#include #include +#include #include +#include #include #include diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index d875124769..95a43dfdbe 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -43,46 +43,33 @@ * @author Thomas Gubler */ -#include -#include -#include -#include +#include "navigator.h" -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include #include #include -#include - -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include #include #include +#include +#include #include -#include - -#include -#include -#include -#include -#include -#include - -#include "navigator.h" +#include +#include /** * navigator app start / stop handling function @@ -778,9 +765,6 @@ Navigator::status() void Navigator::publish_position_setpoint_triplet() { - /* update navigation state */ - _pos_sp_triplet.nav_state = _vstatus.nav_state; - /* do not publish an empty triplet */ if (!_pos_sp_triplet.current.valid) { return; diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index c618a005d5..4296422fa8 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -57,18 +57,10 @@ #else #include #endif -#include -#include -#include -#include -#include -#include #include #include -#include #include #include -#include #include #include @@ -1965,7 +1957,6 @@ int sdlog2_thread_main(int argc, char *argv[]) if (buf.triplet.current.valid) { log_msg.msg_type = LOG_GPSP_MSG; - log_msg.body.log_GPSP.nav_state = buf.triplet.nav_state; log_msg.body.log_GPSP.lat = (int32_t)(buf.triplet.current.lat * (double)1e7); log_msg.body.log_GPSP.lon = (int32_t)(buf.triplet.current.lon * (double)1e7); log_msg.body.log_GPSP.alt = buf.triplet.current.alt; diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index afd4fc084c..9628c4c123 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -248,7 +248,6 @@ struct log_GPOS_s { /* --- GPSP - GLOBAL POSITION SETPOINT --- */ #define LOG_GPSP_MSG 17 struct log_GPSP_s { - uint8_t nav_state; int32_t lat; int32_t lon; float alt; @@ -693,7 +692,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"), LOG_FORMAT(FLOW, "BffffffLLHhB", "ID,RawX,RawY,RX,RY,RZ,Dist,TSpan,DtSonar,FrmCnt,GT,Qlty"), LOG_FORMAT(GPOS, "LLfffffff", "Lat,Lon,Alt,VelN,VelE,VelD,EPH,EPV,TALT"), - LOG_FORMAT(GPSP, "BLLffBfbf", "NavState,Lat,Lon,Alt,Yaw,Type,LoitR,LoitDir,PitMin"), + LOG_FORMAT(GPSP, "LLffBfbf", "Lat,Lon,Alt,Yaw,Type,LoitR,LoitDir,PitMin"), LOG_FORMAT(ESC, "HBBBHHffiffH", "count,nESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"), LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"), LOG_FORMAT(BATT, "fffffffB", "V,VFilt,C,CFilt,Discharged,Remaining,Scale,Warning"), 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 b23d8e7593..5862317cce 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.h +++ b/src/modules/vtol_att_control/vtol_att_control_main.h @@ -54,15 +54,6 @@ #include #include -#include -#include -#include -#include -#include -#include -#include -#include - #include #include #include @@ -77,11 +68,7 @@ #include #include #include -#include -#include #include -#include -#include #include #include #include @@ -115,11 +102,11 @@ 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_attitude_setpoint_s *get_mc_virtual_att_sp() {return &_mc_virtual_att_sp;} + struct vehicle_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;} + 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_vtol_vehicle_status() {return &_vtol_vehicle_status;} @@ -174,11 +161,11 @@ 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_attitude_setpoint_s _mc_virtual_att_sp; // virtual mc attitude setpoint + struct vehicle_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 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; diff --git a/src/modules/vtol_att_control/vtol_type.h b/src/modules/vtol_att_control/vtol_type.h index e3892b0014..e298ec3a6c 100644 --- a/src/modules/vtol_att_control/vtol_type.h +++ b/src/modules/vtol_att_control/vtol_type.h @@ -152,11 +152,11 @@ 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_attitude_setpoint_s *_mc_virtual_att_sp; // virtual mc attitude setpoint + struct vehicle_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 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; @@ -180,7 +180,7 @@ protected: float _mc_pitch_weight = 1.0f; // weight for multicopter attitude controller pitch output float _mc_yaw_weight = 1.0f; // weight for multicopter attitude controller yaw output float _mc_throttle_weight = 1.0f; // weight for multicopter throttle command. Used to avoid - // motors spinning up or cutting too fast whend doing transitions. + // motors spinning up or cutting too fast when doing transitions. float _thrust_transition = 0.0f; // thrust value applied during a front transition (tailsitter & tiltrotor only) bool _flag_was_in_trans_mode = false; // true if mode has just switched to transition