From bb021edd15b9114883fd62f944f1b381f729207b Mon Sep 17 00:00:00 2001 From: Juan Paredes Date: Fri, 12 Feb 2021 04:31:35 -0500 Subject: [PATCH 1/9] Updated mavlink/include/mavlink/v2.0 --- mavlink/include/mavlink/v2.0 | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mavlink/include/mavlink/v2.0 b/mavlink/include/mavlink/v2.0 index 076158a97560..0ff95751c7a1 160000 --- a/mavlink/include/mavlink/v2.0 +++ b/mavlink/include/mavlink/v2.0 @@ -1 +1 @@ -Subproject commit 076158a9756064c78817a425f67991acba95b8ec +Subproject commit 0ff95751c7a10afdb7c56850e13e71529ea1c2c3 From b607d76c793442603200083c7e76841d5a8038e2 Mon Sep 17 00:00:00 2001 From: Juan Paredes Date: Fri, 12 Feb 2021 18:35:11 -0500 Subject: [PATCH 2/9] Updated mavlink/include/mavlink/v2.0 --- mavlink/include/mavlink/v2.0 | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mavlink/include/mavlink/v2.0 b/mavlink/include/mavlink/v2.0 index 0ff95751c7a1..31c22b50c26c 160000 --- a/mavlink/include/mavlink/v2.0 +++ b/mavlink/include/mavlink/v2.0 @@ -1 +1 @@ -Subproject commit 0ff95751c7a10afdb7c56850e13e71529ea1c2c3 +Subproject commit 31c22b50c26c04ccbd893f83eb4eacf070437028 From f9cd678eeef5320ffa99fabdbed87d68a08d844e Mon Sep 17 00:00:00 2001 From: Juan Paredes Date: Fri, 12 Feb 2021 18:51:22 -0500 Subject: [PATCH 3/9] Updated mavlink/include/mavlink/v2.0 --- mavlink/include/mavlink/v2.0 | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mavlink/include/mavlink/v2.0 b/mavlink/include/mavlink/v2.0 index 31c22b50c26c..795264b0059f 160000 --- a/mavlink/include/mavlink/v2.0 +++ b/mavlink/include/mavlink/v2.0 @@ -1 +1 @@ -Subproject commit 31c22b50c26c04ccbd893f83eb4eacf070437028 +Subproject commit 795264b0059f02fdd5f7141670eae9aa83d819c6 From ddcde318fe0c9f5d7a9619eb8e60a66084a835ba Mon Sep 17 00:00:00 2001 From: Juan Paredes Date: Fri, 12 Feb 2021 19:29:38 -0500 Subject: [PATCH 4/9] Updated all files to enable RCAC functionality. Can build jmavSim --- .gitmodules | 4 +- ROMFS/px4fmu_common/init.d-posix/rcS | 6 + ROMFS/px4fmu_common/init.d/rcS | 12 + msg/CMakeLists.txt | 3 + msg/rcac_att_variables.msg | 14 + msg/rcac_pos_vel_variables.msg | 26 ++ msg/rcac_rate_variables.msg | 14 + src/modules/logger/logged_topics.cpp | 5 + src/modules/mavlink/mavlink_main.cpp | 15 + src/modules/mavlink/mavlink_messages.cpp | 252 ++++++++++++++- src/modules/mavlink/mavlink_receiver.cpp | 2 +- .../AttitudeControl/AttitudeControl.cpp | 49 ++- .../AttitudeControl/AttitudeControl.hpp | 174 ++++++++++- src/modules/mc_att_control/mc_att_control.hpp | 18 +- .../mc_att_control/mc_att_control_main.cpp | 66 +++- .../mc_att_control/mc_att_control_params.c | 32 ++ .../MulticopterPositionControl.cpp | 64 ++++ .../MulticopterPositionControl.hpp | 24 ++ .../PositionControl/PositionControl.cpp | 295 ++++++++++++++++++ .../PositionControl/PositionControl.hpp | 172 ++++++++++ .../mc_pos_control/mc_pos_control_params.c | 64 ++++ .../MulticopterRateControl.cpp | 63 ++++ .../MulticopterRateControl.hpp | 15 +- .../RateControl/RateControl.cpp | 84 ++++- .../RateControl/RateControl.hpp | 196 ++++++++++++ .../mc_rate_control/mc_rate_control_params.c | 32 ++ 26 files changed, 1689 insertions(+), 12 deletions(-) create mode 100644 msg/rcac_att_variables.msg create mode 100644 msg/rcac_pos_vel_variables.msg create mode 100644 msg/rcac_rate_variables.msg diff --git a/.gitmodules b/.gitmodules index 8338c4fa2d4c..63cbe99fa1a7 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,7 +1,7 @@ [submodule "mavlink/include/mavlink/v2.0"] path = mavlink/include/mavlink/v2.0 - url = https://github.com/mavlink/c_library_v2.git - branch = master + url = https://github.com/JAParedes/c_library_v2.git + branch = mavlink_rcac [submodule "src/drivers/uavcan/libuavcan"] path = src/drivers/uavcan/libuavcan url = https://github.com/PX4/libuavcan.git diff --git a/ROMFS/px4fmu_common/init.d-posix/rcS b/ROMFS/px4fmu_common/init.d-posix/rcS index edf49e203a5b..e573050d7163 100644 --- a/ROMFS/px4fmu_common/init.d-posix/rcS +++ b/ROMFS/px4fmu_common/init.d-posix/rcS @@ -254,9 +254,15 @@ else mavlink stream -r 50 -s SERVO_OUTPUT_RAW_0 -u $udp_gcs_port_local mavlink stream -r 20 -s RC_CHANNELS -u $udp_gcs_port_local mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u $udp_gcs_port_local + mavlink stream -r 2 -s RCAC_ATT_VARIABLES -u $udp_gcs_port_local + mavlink stream -r 2 -s RCAC_RATE_VARIABLES -u $udp_gcs_port_local + mavlink stream -r 2 -s RCAC_POS_VEL_VARIABLES -u $udp_gcs_port_local + + # API/Offboard link mavlink start -x -u $udp_offboard_port_local -r 4000000 -m onboard -o $udp_offboard_port_remote + mavlink stream -r 50 -s LOCAL_POSITION_NED -u $udp_offboard_port_local # Onboard link to camera mavlink start -x -u $udp_onboard_payload_port_local -r 4000 -f -m onboard -o $udp_onboard_payload_port_remote diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 50d42d4dab45..25dd309ce268 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -612,3 +612,15 @@ unset VEHICLE_TYPE # Boot is complete, inform MAVLink app(s) that the system is now fully up and running. # mavlink boot_complete + +mavlink stream -d /dev/ttyACM0 -s RCAC_ATT_VARIABLES -r 2 +mavlink stream -d /dev/ttyS1 -s RCAC_ATT_VARIABLES -r 2 +mavlink stream -d /dev/ttyS2 -s RCAC_ATT_VARIABLES -r 2 +mavlink stream -d /dev/ttyACM0 -s RCAC_RATE_VARIABLES -r 2 +mavlink stream -d /dev/ttyS1 -s RCAC_RATE_VARIABLES -r 2 +mavlink stream -d /dev/ttyS2 -s RCAC_RATE_VARIABLES -r 2 +mavlink stream -d /dev/ttyACM0 -s RCAC_POS_VEL_VARIABLES -r 2 +mavlink stream -d /dev/ttyS1 -s RCAC_POS_VEL_VARIABLES -r 2 +mavlink stream -d /dev/ttyS2 -s RCAC_POS_VEL_VARIABLES -r 2 +mavlink stream -d /dev/ttyS1 -s ODOMETRY -r 10 +mavlink stream -d /dev/ttyS2 -s LOCAL_POSITION_NED -10 diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 64cfd389d4cd..791ad706a45b 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -107,6 +107,9 @@ set(msg_files qshell_retval.msg radio_status.msg rate_ctrl_status.msg + rcac_att_variables.msg + rcac_pos_vel_variables.msg + rcac_rate_variables.msg rc_channels.msg rc_parameter_map.msg rpm.msg diff --git a/msg/rcac_att_variables.msg b/msg/rcac_att_variables.msg new file mode 100644 index 000000000000..e1ab8137941b --- /dev/null +++ b/msg/rcac_att_variables.msg @@ -0,0 +1,14 @@ +# Fused local position in NED. + +uint64 timestamp # time since system start (microseconds) + +uint64 ii_att # iteration step of the RCAC Attitude Controller +bool switch_att # RCAC Attitude Controller switch +float32 alpha_pid_att # Gain that multiplies the attitude PID gains +float32[3] rcac_att_z # Performance variable for the Attitude Controller +float32[3] rcac_att_u # Control generated by the Attitude Controller +float32[3] rcac_att_theta # P gains for the Attitude Controller + +float32[3] px4_att_theta # P gains for the PX4 Attitude Controller + +float32 p11_att # P(1,1) of the RCAC Attitude Controller diff --git a/msg/rcac_pos_vel_variables.msg b/msg/rcac_pos_vel_variables.msg new file mode 100644 index 000000000000..4376d41d7e2d --- /dev/null +++ b/msg/rcac_pos_vel_variables.msg @@ -0,0 +1,26 @@ +# Fused local position in NED. + +uint64 timestamp # time since system start (microseconds) + +float32 pid_factor # PID scaling +float32 rcac_master_sw # Master RCAC switch + +uint64 ii_pos # iteration step of the RCAC Position Controller +bool switch_pos # RCAC Position Controller switch +float32 alpha_pid_pos # Gain that multiplies the position PID gains +float32[3] rcac_pos_z # Performance variable for the Position Controller +float32[3] rcac_pos_u # Control generated by the Position Controller +float32[3] rcac_pos_theta # P gains for the Position Controller + +uint64 ii_vel # iteration step of the RCAC Velocity Controller +bool switch_vel # RCAC Velocity Controller switch +float32 alpha_pid_vel # Gain that multiplies the velocity PID gains +float32[3] rcac_vel_z # Performance variable for the Velocity Controller +float32[3] rcac_vel_u # Control generated by the Velocity Controller +float32[9] rcac_vel_theta # PID gains for the Velocity Controller + +float32[9] px4_ol_theta # P and PID gains for the PX4 outer loop Controller + # First 3 are P, next 3 are PID for XY, last 3 are PID for Z + +float32 p11_pos # P(1,1) of the RCAC Position Controller +float32 p11_velx # P(1,1) of the RCAC Velocity Controller diff --git a/msg/rcac_rate_variables.msg b/msg/rcac_rate_variables.msg new file mode 100644 index 000000000000..7b4a95fb031e --- /dev/null +++ b/msg/rcac_rate_variables.msg @@ -0,0 +1,14 @@ +# Fused local position in NED. + +uint64 timestamp # time since system start (microseconds) + +uint64 ii_rate # iteration step of the RCAC Rate Controller +bool switch_rate # RCAC Rate Controller switch +float32 alpha_pid_rate # Gain that multiplies the rate PID gains +float32[3] rcac_rate_z # Performance variable for the Rate Controller +float32[3] rcac_rate_u # Control generated by the Rate Controller +float32[12] rcac_rate_theta # PID+FF gains for the Rate Controller + +float32[12] px4_rate_theta # P gains for the PX4 Rate Controller + +float32 p11_ratex # P(1,1) of the RCAC Rate Controller diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 38ccef0e7574..3ff3012baabf 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -111,6 +111,11 @@ void LoggedTopics::add_default_topics() add_topic("vehicle_status"); add_topic("vehicle_status_flags"); add_topic("vtol_vehicle_status", 200); + add_topic("rcac_pos_vel_variables", 100); + add_topic("rcac_att_variables", 100); + add_topic("rcac_rate_variables", 100); + + // Control allocation topics add_topic("vehicle_angular_acceleration_setpoint", 20); diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index a8fbf7454f24..228dea8d10a9 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1600,6 +1600,9 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("POSITION_TARGET_LOCAL_NED", 1.5f); configure_stream_local("RAW_RPM", 2.0f); configure_stream_local("RC_CHANNELS", 5.0f); + configure_stream_local("RCAC_ATT_VARIABLES", 2.0f); + configure_stream_local("RCAC_RATE_VARIABLES", 2.0f); + configure_stream_local("RCAC_POS_VEL_VARIABLES", 2.0f); configure_stream_local("SERVO_OUTPUT_RAW_0", 1.0f); configure_stream_local("SYS_STATUS", 1.0f); configure_stream_local("UTM_GLOBAL_POSITION", 0.5f); @@ -1655,6 +1658,9 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("POSITION_TARGET_LOCAL_NED", 10.0f); configure_stream_local("RAW_RPM", 5.0f); configure_stream_local("RC_CHANNELS", 20.0f); + configure_stream_local("RCAC_ATT_VARIABLES", 2.0f); + configure_stream_local("RCAC_RATE_VARIABLES", 2.0f); + configure_stream_local("RCAC_POS_VEL_VARIABLES", 2.0f); configure_stream_local("SERVO_OUTPUT_RAW_0", 10.0f); configure_stream_local("SYS_STATUS", 5.0f); configure_stream_local("SYSTEM_TIME", 1.0f); @@ -1708,6 +1714,9 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("POSITION_TARGET_GLOBAL_INT", 1.5f); configure_stream_local("POSITION_TARGET_LOCAL_NED", 1.5f); configure_stream_local("RC_CHANNELS", 5.0f); + configure_stream_local("RCAC_ATT_VARIABLES", 2.0f); + configure_stream_local("RCAC_RATE_VARIABLES", 2.0f); + configure_stream_local("RCAC_POS_VEL_VARIABLES", 2.0f); configure_stream_local("SERVO_OUTPUT_RAW_0", 1.0f); configure_stream_local("SYS_STATUS", 5.0f); configure_stream_local("TRAJECTORY_REPRESENTATION_WAYPOINTS", 5.0f); @@ -1737,6 +1746,9 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("GPS_RAW_INT", 1.0f); configure_stream_local("HOME_POSITION", 0.5f); configure_stream_local("RC_CHANNELS", 5.0f); + configure_stream_local("RCAC_ATT_VARIABLES", 2.0f); + configure_stream_local("RCAC_RATE_VARIABLES", 2.0f); + configure_stream_local("RCAC_POS_VEL_VARIABLES", 2.0f); configure_stream_local("SERVO_OUTPUT_RAW_0", 1.0f); configure_stream_local("SYS_STATUS", 5.0f); configure_stream_local("SYSTEM_TIME", 1.0f); @@ -1788,6 +1800,9 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("POSITION_TARGET_GLOBAL_INT", 10.0f); configure_stream_local("RAW_RPM", 5.0f); configure_stream_local("RC_CHANNELS", 10.0f); + configure_stream_local("RCAC_ATT_VARIABLES", 2.0f); + configure_stream_local("RCAC_RATE_VARIABLES", 2.0f); + configure_stream_local("RCAC_POS_VEL_VARIABLES", 2.0f); configure_stream_local("SCALED_IMU", 25.0f); configure_stream_local("SCALED_IMU2", 25.0f); configure_stream_local("SCALED_IMU3", 25.0f); diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 170b83868fb0..c8521ffacb1d 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -52,6 +52,10 @@ #include #include #include +#include +#include +#include +#include #include #include @@ -71,6 +75,9 @@ #include #include #include +#include +#include +#include #include #include #include @@ -2224,8 +2231,10 @@ class MavlinkStreamOdometry : public MavlinkStream odom_updated = _vodom_sub.update(&odom); // set the frame_id according to the local frame of the data + //Juan: Take into account for MOCAP frame if (odom.local_frame == vehicle_odometry_s::LOCAL_FRAME_NED) { - msg.frame_id = MAV_FRAME_LOCAL_NED; + //msg.frame_id = MAV_FRAME_LOCAL_NED; + msg.frame_id = MAV_FRAME_LOCAL_FRD; } else { msg.frame_id = MAV_FRAME_LOCAL_FRD; @@ -2237,7 +2246,8 @@ class MavlinkStreamOdometry : public MavlinkStream } else { odom_updated = _odom_sub.update(&odom); - msg.frame_id = MAV_FRAME_LOCAL_NED; + //msg.frame_id = MAV_FRAME_LOCAL_NED; + msg.frame_id = MAV_FRAME_LOCAL_FRD; // source: PX4 estimator msg.estimator_type = MAV_ESTIMATOR_TYPE_AUTOPILOT; @@ -3005,6 +3015,241 @@ class MavlinkStreamCameraCapture : public MavlinkStream } }; +// Juan: Added classes for RCAC messages streaming +class MavlinkStreamRcacAttVariables : public MavlinkStream +{ +public: + const char *get_name() const override + { + return MavlinkStreamRcacAttVariables::get_name_static(); + } + static constexpr const char *get_name_static() + { + return "RCAC_ATT_VARIABLES"; + } + static constexpr uint16_t get_id_static() + { + return MAVLINK_MSG_ID_RCAC_ATT_VARIABLES; + } + uint16_t get_id() override + { + return get_id_static(); + } + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamRcacAttVariables(mavlink); + } + unsigned get_size() override + { + return MAVLINK_MSG_ID_RCAC_ATT_VARIABLES_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + +private: + + //MavlinkOrbSubscription *_sub; + //uint64_t _rcac_arv_time; + uORB::Subscription _rcac_att_sub{ORB_ID(rcac_att_variables)}; + + /* do not allow top copying this class */ + MavlinkStreamRcacAttVariables(MavlinkStreamRcacAttVariables &) = delete; + MavlinkStreamRcacAttVariables& operator = (const MavlinkStreamRcacAttVariables &) = delete; + +protected: + /*explicit MavlinkStreamRcacAttVariables(Mavlink *mavlink) : MavlinkStream(mavlink), + _sub(_mavlink->add_orb_subscription(ORB_ID(rcac_att_variables))), + _rcac_arv_time(0) + {}*/ + explicit MavlinkStreamRcacAttVariables(Mavlink *mavlink) : MavlinkStream(mavlink) + {} + + //bool send(const hrt_abstime t) + bool send() override + { + //struct rcac_att_variables_struct_s _rcac_att_variables; + rcac_att_variables_s _rcac_att_variables; + + if (_rcac_att_sub.update(&_rcac_att_variables)) { + mavlink_rcac_att_variables_t _msg_rcac_att_variables {}; + + _msg_rcac_att_variables.timestamp = _rcac_att_variables.timestamp; + _msg_rcac_att_variables.switch_att = _rcac_att_variables.switch_att; + _msg_rcac_att_variables.alpha_pid_att = _rcac_att_variables.alpha_pid_att; + _msg_rcac_att_variables.p11_att = _rcac_att_variables.p11_att; + _msg_rcac_att_variables.rcac_att_theta[0] = _rcac_att_variables.rcac_att_theta[0]; + _msg_rcac_att_variables.rcac_att_theta[1] = _rcac_att_variables.rcac_att_theta[1]; + _msg_rcac_att_variables.rcac_att_theta[2] = _rcac_att_variables.rcac_att_theta[2]; + + mavlink_msg_rcac_att_variables_send_struct(_mavlink->get_channel(), &_msg_rcac_att_variables); + } + + return true; + } +}; +/******************************************/ +class MavlinkStreamRcacRateVariables : public MavlinkStream +{ +public: + const char *get_name() const override + { + return MavlinkStreamRcacRateVariables::get_name_static(); + } + static constexpr const char *get_name_static() + { + return "RCAC_RATE_VARIABLES"; + } + static constexpr uint16_t get_id_static() + { + return MAVLINK_MSG_ID_RCAC_RATE_VARIABLES; + } + uint16_t get_id() override + { + return get_id_static(); + } + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamRcacRateVariables(mavlink); + } + unsigned get_size() override + { + return MAVLINK_MSG_ID_RCAC_RATE_VARIABLES_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + +private: + + //MavlinkOrbSubscription *_sub; + //uint64_t _rcac_arv_time; + uORB::Subscription _rcac_rate_sub{ORB_ID(rcac_rate_variables)}; + + /* do not allow top copying this class */ + MavlinkStreamRcacRateVariables(MavlinkStreamRcacRateVariables &) = delete; + MavlinkStreamRcacRateVariables& operator = (const MavlinkStreamRcacRateVariables &) = delete; + +protected: + /*explicit MavlinkStreamRcacRateVariables(Mavlink *mavlink) : MavlinkStream(mavlink), + _sub(_mavlink->add_orb_subscription(ORB_ID(rcac_rate_variables))), + _rcac_arv_time(0) + {}*/ + explicit MavlinkStreamRcacRateVariables(Mavlink *mavlink) : MavlinkStream(mavlink) + {} + + //bool send(const hrt_abstime t) + bool send() override + { + //struct rcac_rate_variables_struct_s _rcac_rate_variables; + rcac_rate_variables_s _rcac_rate_variables; + + if (_rcac_rate_sub.update(&_rcac_rate_variables)) { + mavlink_rcac_rate_variables_t _msg_rcac_rate_variables {}; + + _msg_rcac_rate_variables.timestamp = _rcac_rate_variables.timestamp; + _msg_rcac_rate_variables.switch_rate = _rcac_rate_variables.switch_rate; + _msg_rcac_rate_variables.alpha_pid_rate = _rcac_rate_variables.alpha_pid_rate; + _msg_rcac_rate_variables.p11_ratex = _rcac_rate_variables.p11_ratex; + _msg_rcac_rate_variables.rcac_rate_theta[0] = _rcac_rate_variables.rcac_rate_theta[0]; + _msg_rcac_rate_variables.rcac_rate_theta[1] = _rcac_rate_variables.rcac_rate_theta[1]; + _msg_rcac_rate_variables.rcac_rate_theta[2] = _rcac_rate_variables.rcac_rate_theta[2]; + _msg_rcac_rate_variables.rcac_rate_theta[3] = _rcac_rate_variables.rcac_rate_theta[3]; + _msg_rcac_rate_variables.rcac_rate_theta[4] = _rcac_rate_variables.rcac_rate_theta[4]; + _msg_rcac_rate_variables.rcac_rate_theta[5] = _rcac_rate_variables.rcac_rate_theta[5]; + _msg_rcac_rate_variables.rcac_rate_theta[6] = _rcac_rate_variables.rcac_rate_theta[6]; + _msg_rcac_rate_variables.rcac_rate_theta[7] = _rcac_rate_variables.rcac_rate_theta[7]; + _msg_rcac_rate_variables.rcac_rate_theta[8] = _rcac_rate_variables.rcac_rate_theta[8]; + _msg_rcac_rate_variables.rcac_rate_theta[9] = _rcac_rate_variables.rcac_rate_theta[9]; + _msg_rcac_rate_variables.rcac_rate_theta[10] = _rcac_rate_variables.rcac_rate_theta[10]; + _msg_rcac_rate_variables.rcac_rate_theta[11] = _rcac_rate_variables.rcac_rate_theta[11]; + + mavlink_msg_rcac_rate_variables_send_struct(_mavlink->get_channel(), &_msg_rcac_rate_variables); + } + + return true; + } +}; +/******************************************/ +class MavlinkStreamRcacPosVelVariables : public MavlinkStream +{ +public: + const char *get_name() const override + { + return MavlinkStreamRcacPosVelVariables::get_name_static(); + } + static constexpr const char *get_name_static() + { + return "RCAC_POS_VEL_VARIABLES"; + } + static constexpr uint16_t get_id_static() + { + return MAVLINK_MSG_ID_RCAC_POS_VEL_VARIABLES; + } + uint16_t get_id() override + { + return get_id_static(); + } + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamRcacPosVelVariables(mavlink); + } + unsigned get_size() override + { + return MAVLINK_MSG_ID_RCAC_POS_VEL_VARIABLES_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + +private: + /*MavlinkOrbSubscription *_sub; + uint64_t _rcac_pvv_time;*/ + uORB::Subscription _rcac_pos_vel_sub{ORB_ID(rcac_pos_vel_variables)}; + + /* do not allow top copying this class */ + MavlinkStreamRcacPosVelVariables(MavlinkStreamRcacPosVelVariables &) = delete; + MavlinkStreamRcacPosVelVariables& operator = (const MavlinkStreamRcacPosVelVariables &) = delete; + +protected: + /*explicit MavlinkStreamRcacPosVelVariables(Mavlink *mavlink) : MavlinkStream(mavlink), + _sub(_mavlink->add_orb_subscription(ORB_ID(rcac_pos_vel_variables))), + _rcac_pvv_time(0) + {}*/ + explicit MavlinkStreamRcacPosVelVariables(Mavlink *mavlink) : MavlinkStream(mavlink) + {} + + //bool send(const hrt_abstime t) + bool send() override + { + //struct rcac_pos_vel_variables_struct_s _rcac_pos_vel_variables; + rcac_pos_vel_variables_s _rcac_pos_vel_variables; + + /*if (_sub->update(&_rcac_pvv_time, &_rcac_pos_vel_variables)) {*/ + if (_rcac_pos_vel_sub.update(&_rcac_pos_vel_variables)) { + mavlink_rcac_pos_vel_variables_t _msg_rcac_pos_vel_variables{}; + + _msg_rcac_pos_vel_variables.timestamp = _rcac_pos_vel_variables.timestamp; + _msg_rcac_pos_vel_variables.pid_factor = _rcac_pos_vel_variables.pid_factor; + _msg_rcac_pos_vel_variables.rcac_master_sw = _rcac_pos_vel_variables.rcac_master_sw; + _msg_rcac_pos_vel_variables.switch_pos = _rcac_pos_vel_variables.switch_pos; + _msg_rcac_pos_vel_variables.switch_vel = _rcac_pos_vel_variables.switch_vel; + _msg_rcac_pos_vel_variables.alpha_pid_pos = _rcac_pos_vel_variables.alpha_pid_pos; + _msg_rcac_pos_vel_variables.alpha_pid_vel = _rcac_pos_vel_variables.alpha_pid_vel; + _msg_rcac_pos_vel_variables.p11_pos = _rcac_pos_vel_variables.p11_pos; + _msg_rcac_pos_vel_variables.p11_velx = _rcac_pos_vel_variables.p11_velx; + _msg_rcac_pos_vel_variables.rcac_pos_theta[0] = _rcac_pos_vel_variables.rcac_pos_theta[0]; + _msg_rcac_pos_vel_variables.rcac_pos_theta[1] = _rcac_pos_vel_variables.rcac_pos_theta[1]; + _msg_rcac_pos_vel_variables.rcac_pos_theta[2] = _rcac_pos_vel_variables.rcac_pos_theta[2]; + _msg_rcac_pos_vel_variables.rcac_vel_theta[0] = _rcac_pos_vel_variables.rcac_vel_theta[0]; + _msg_rcac_pos_vel_variables.rcac_vel_theta[1] = _rcac_pos_vel_variables.rcac_vel_theta[1]; + _msg_rcac_pos_vel_variables.rcac_vel_theta[2] = _rcac_pos_vel_variables.rcac_vel_theta[2]; + _msg_rcac_pos_vel_variables.rcac_vel_theta[3] = _rcac_pos_vel_variables.rcac_vel_theta[3]; + _msg_rcac_pos_vel_variables.rcac_vel_theta[4] = _rcac_pos_vel_variables.rcac_vel_theta[4]; + _msg_rcac_pos_vel_variables.rcac_vel_theta[5] = _rcac_pos_vel_variables.rcac_vel_theta[5]; + _msg_rcac_pos_vel_variables.rcac_vel_theta[6] = _rcac_pos_vel_variables.rcac_vel_theta[6]; + _msg_rcac_pos_vel_variables.rcac_vel_theta[7] = _rcac_pos_vel_variables.rcac_vel_theta[7]; + _msg_rcac_pos_vel_variables.rcac_vel_theta[8] = _rcac_pos_vel_variables.rcac_vel_theta[8]; + + mavlink_msg_rcac_pos_vel_variables_send_struct(_mavlink->get_channel(), &_msg_rcac_pos_vel_variables); + } + + return true; + } +}; +/******************************************/ + static const StreamListItem streams_list[] = { create_stream_list_item(), #if defined(STATUSTEXT_HPP) @@ -3093,6 +3338,9 @@ static const StreamListItem streams_list[] = { create_stream_list_item(), create_stream_list_item(), create_stream_list_item(), + create_stream_list_item(), + create_stream_list_item(), + create_stream_list_item(), #if defined(DISTANCE_SENSOR_HPP) create_stream_list_item(), #endif // DISTANCE_SENSOR_HPP diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 56a63bac3b43..3182a601dc02 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1405,7 +1405,7 @@ MavlinkReceiver::handle_message_odometry(mavlink_message_t *msg) } else { odometry.local_frame = vehicle_odometry_s::LOCAL_FRAME_FRD; } - + // Juan: Keep this in mind when trying MOCAP out. if ((odom.estimator_type == MAV_ESTIMATOR_TYPE_VISION) || (odom.estimator_type == MAV_ESTIMATOR_TYPE_VIO) || (odom.estimator_type == MAV_ESTIMATOR_TYPE_UNKNOWN)) { diff --git a/src/modules/mc_att_control/AttitudeControl/AttitudeControl.cpp b/src/modules/mc_att_control/AttitudeControl/AttitudeControl.cpp index de0ba91b05c0..0298f9817f75 100644 --- a/src/modules/mc_att_control/AttitudeControl/AttitudeControl.cpp +++ b/src/modules/mc_att_control/AttitudeControl/AttitudeControl.cpp @@ -52,7 +52,8 @@ void AttitudeControl::setProportionalGain(const matrix::Vector3f &proportional_g } } -matrix::Vector3f AttitudeControl::update(const Quatf &q) const +//matrix::Vector3f AttitudeControl::update(const Quatf &q) const +matrix::Vector3f AttitudeControl::update(const Quatf &q, const bool landed) { Quatf qd = _attitude_setpoint_q; @@ -85,10 +86,56 @@ matrix::Vector3f AttitudeControl::update(const Quatf &q) const // using sin(alpha/2) scaled rotation axis as attitude error (see quaternion definition by axis angle) // also taking care of the antipodal unit quaternion ambiguity + //const Vector3f eq = 2.f * qe.canonical().imag(); const Vector3f eq = 2.f * qe.canonical().imag(); // calculate angular rates setpoint matrix::Vector3f rate_setpoint = eq.emult(_proportional_gain); + + //// rate_setpoint = alpha_PID*rate_setpoint; + //// rate_setpoint = alpha_PID_att*rate_setpoint; + + //this->z_k_Pq_R.setZero(); + z_k_Pq_R = rate_setpoint; + u_k_Pq_R.setZero(); + if (landed) + { + ii_Pq_R = 0; + } + + if ((RCAC_Aq_ON) && (!landed)) + { + ii_Pq_R = ii_Pq_R + 1; + if (ii_Pq_R == 1) + { + init_RCAC_att(); + //P_Pq_R = eye() * _param_mpc_rcac_att_p0.get(); + } + for (int i = 0; i <= 2; i++) { + phi_k_Pq_R(i, i) = eq(i); + theta_k_Pq_PID(i) = _proportional_gain(i); //Ankit: Not needed now, but keep it just in case + } + + //z_k_Pq_R.setZero(); + //z_k_Pq_R += rate_setpoint; + + Gamma_Pq_R = phi_km1_Pq_R * P_Pq_R * phi_km1_Pq_R.T() + I3; + Gamma_Pq_R = Gamma_Pq_R.I(); + P_Pq_R = P_Pq_R - (P_Pq_R * phi_km1_Pq_R.T()) * Gamma_Pq_R * (phi_km1_Pq_R * P_Pq_R); + //theta_k_Pq_R = theta_k_Pq_R + (P_Pq_R * phi_km1_Pq_R.T()) * + // (z_k_Pq_R + (-1.0f)*(phi_km1_Pq_R * theta_k_Pq_R - u_km1_Pq_R) * (-1.0f)); + theta_k_Pq_R = theta_k_Pq_R + (P_Pq_R * phi_km1_Pq_R.T()) * N1_Pq * + (z_k_Pq_R + N1_Pq*(phi_km1_Pq_R * theta_k_Pq_R - u_km1_Pq_R) ); + + //u_k_Pq_R = phi_k_Pq_R * (theta_k_Pq_R+ 0*alpha_PID *theta_k_Pq_PID); + u_k_Pq_R = phi_k_Pq_R * (theta_k_Pq_R+ 0*alpha_PID_att *theta_k_Pq_PID); + u_km1_Pq_R = u_k_Pq_R; + phi_km1_Pq_R = phi_k_Pq_R; + } + //rate_setpoint = alpha_PID * rate_setpoint + u_k_Pq_R; + rate_setpoint = alpha_PID_att * rate_setpoint + u_k_Pq_R; + + // Feed forward the yaw setpoint rate. // yawspeed_setpoint is the feed forward commanded rotation around the world z-axis, diff --git a/src/modules/mc_att_control/AttitudeControl/AttitudeControl.hpp b/src/modules/mc_att_control/AttitudeControl/AttitudeControl.hpp index 70980e2b3531..38372ff138a5 100644 --- a/src/modules/mc_att_control/AttitudeControl/AttitudeControl.hpp +++ b/src/modules/mc_att_control/AttitudeControl/AttitudeControl.hpp @@ -89,7 +89,164 @@ class AttitudeControl * @param q estimation of the current vehicle attitude unit quaternion * @return [rad/s] body frame 3D angular rate setpoint vector to be executed by the rate controller */ - matrix::Vector3f update(const matrix::Quatf &q) const; + //matrix::Vector3f update(const matrix::Quatf &q) const; + matrix::Vector3f update(const matrix::Quatf &q, const bool landed); + + /** + * Get the + * @see z_k_Pr_R + * @return The z variable used by RCAC in the PID+FF controller + */ + const matrix::Vector3f get_RCAC_att_z() + { + matrix::Vector3f RCAC_z{}; + + for (int i = 0; i <= 2; i++) { + RCAC_z(i) = z_k_Pq_R(i); + } + + return RCAC_z; + } + + /** + * Get the + * @see u_k_Pr_R + * @return The u variable computed by RCAC in the PID+FF controller + */ + const matrix::Vector3f get_RCAC_att_u() + { + matrix::Vector3f RCAC_u{}; + + for (int i = 0; i <= 2; i++) { + RCAC_u(i) = u_k_Pq_R(i); + } + + return RCAC_u; + } + + /** + * Get the + * @see theta_k_Pr_R + * @return The theta variable computed by RCAC in the PID+FF controller + */ + const matrix::Vector3f get_RCAC_att_theta() + { + matrix::Vector3f RCAC_theta{}; + + for (int i = 0; i <= 2; i++) { + RCAC_theta(i) = theta_k_Pq_R(i); + } + + return RCAC_theta; + } + + /** + * Get the + * @see _proportional_gain + * @return PX4 PID gains for the attitude controller + */ + const matrix::Vector3f get_PX4_att_theta() + { + matrix::Vector3f PX4_att_theta{}; + + for (int i = 0; i <= 2; i++) { + PX4_att_theta(i) = _proportional_gain(i); + } + + return PX4_att_theta; + } + + /** + * Get the + * @see ii + * @return Iteration step of the RCAC attitude controller + */ + const int &get_RCAC_att_ii() { return ii_Pq_R; } + + /** + * Get the + * @see RCAC_Aq_ON + * @return Get RCAC attitude controller switch + */ + const bool &get_RCAC_att_switch() {return RCAC_Aq_ON;} + + /** + * Get the + * @see alpha_PID_att + * @return Get the gain that multiplies the attitude PID gains. + */ + const float &get_alpha_PID_att() {return alpha_PID_att;} + + /** + * Set the RCAC Attitude switch. + * @see RCAC_Aq_ON + */ + void set_RCAC_att_switch(float switch_RCAC) + { + RCAC_Aq_ON = 1; + if (switch_RCAC<0.0f) { + RCAC_Aq_ON = 0; + } + } + + /** + * Set the PID scaling factor. + * @see alpha_PID + */ + void set_PID_att_factor(float PID_factor, float PID_val) + { + //alpha_PID = 1; + alpha_PID_att = 1.0f; + if (PID_factor<0.0f) { + //alpha_PID = 0.5; + alpha_PID_att = PID_val; + } + } + + /** + * Get the + * @see P_Pq_R + * @return RCAC P(1,1) of the Attitude controller + */ + const float &get_RCAC_P11_Att() { return P_Pq_R(0,0); } + + /** + * Set RCAC variables. + * @see all RCAC variables + */ + void init_RCAC_att() + { + // P_Pq_R = eye() * 0.010; + // N1_Pq = eye(); + // I3 = eye(); + P_Pq_R.setZero(); + N1_Pq.setZero(); + I3.setZero(); + for (int i = 0; i <= 2; i++) { + P_Pq_R(i,i) = 0.01f; + P_Pq_R(i,i) = rcac_att_P0; + N1_Pq(i,i) = 1.0f; + I3(i,i) = 1.0f; + } + phi_k_Pq_R.setZero(); + phi_km1_Pq_R.setZero(); + theta_k_Pq_R.setZero(); + z_k_Pq_R.setZero(); + z_km1_Pq_R.setZero(); + u_k_Pq_R.setZero(); + u_km1_Pq_R.setZero(); + Gamma_Pq_R.setZero(); + } + + /** + * Set the RCAC Attiude Controller P0. + * @see rcac_att_P0 + */ + void set_RCAC_att_P0(float att_P0) + { + rcac_att_P0 = att_P0; + } + // const float &get_RCAC_att_P0() {return P_Pq_R(0,0);} private: matrix::Vector3f _proportional_gain; @@ -98,4 +255,19 @@ class AttitudeControl matrix::Quatf _attitude_setpoint_q; ///< latest known attitude setpoint e.g. from position control float _yawspeed_setpoint{0.f}; ///< latest known yawspeed feed-forward setpoint + + int ii_Pq_R = 0; + bool RCAC_Aq_ON=1; + matrix::SquareMatrix P_Pq_R; + matrix::Matrix phi_k_Pq_R, phi_km1_Pq_R; + //matrix::Matrix theta_k_Pq_R,theta_k_Pq_PID; + //matrix::Matrix z_k_Pq_R, z_km1_Pq_R,u_k_Pq_R, u_km1_Pq_R; + matrix::Vector3f theta_k_Pq_R,theta_k_Pq_PID; + matrix::Vector3f z_k_Pq_R, z_km1_Pq_R,u_k_Pq_R, u_km1_Pq_R; + matrix::SquareMatrix Gamma_Pq_R, I3, N1_Pq; + + //float alpha_PID = 1.0f; + float alpha_PID_att = 1.0f; + float rcac_att_P0 = 0.011f; + }; diff --git a/src/modules/mc_att_control/mc_att_control.hpp b/src/modules/mc_att_control/mc_att_control.hpp index 41b4e3a95eea..73701d1ad910 100644 --- a/src/modules/mc_att_control/mc_att_control.hpp +++ b/src/modules/mc_att_control/mc_att_control.hpp @@ -53,9 +53,11 @@ #include #include #include +#include #include #include +#include #include using namespace time_literals; @@ -91,6 +93,8 @@ class MulticopterAttitudeControl : public ModuleBase */ void parameters_updated(); + void publish_rcac_att_variables(); + float throttle_curve(float throttle_stick_input); /** @@ -108,14 +112,21 @@ class MulticopterAttitudeControl : public ModuleBase uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; /**< manual control setpoint subscription */ uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; /**< vehicle status subscription */ uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; /**< vehicle land detected subscription */ +uORB::Subscription _rc_channels_sub{ORB_ID(rc_channels)}; /**< RC switch>*/ uORB::SubscriptionCallbackWorkItem _vehicle_attitude_sub{this, ORB_ID(vehicle_attitude)}; uORB::Publication _v_rates_sp_pub{ORB_ID(vehicle_rates_setpoint)}; /**< rate setpoint publication */ uORB::Publication _vehicle_attitude_setpoint_pub; + uORB::Publication _rcac_att_variables_pub{ORB_ID(rcac_att_variables)}; /**< RCAC attitude variables log */ + + + struct manual_control_setpoint_s _manual_control_setpoint {}; /**< manual control setpoint */ struct vehicle_control_mode_s _v_control_mode {}; /**< vehicle control mode */ + struct rc_channels_s _rc_channels_switch{}; /**< Switch from the RC channel */ + rcac_att_variables_s _rcac_att_variables{}; /**< RCAC variables */ perf_counter_t _loop_perf; /**< loop duration performance counter */ @@ -129,6 +140,7 @@ class MulticopterAttitudeControl : public ModuleBase hrt_abstime _last_run{0}; bool _landed{true}; + bool _maybe_landed{true}; bool _reset_yaw_sp{true}; bool _vehicle_type_rotary_wing{true}; bool _vtol{false}; @@ -160,7 +172,11 @@ class MulticopterAttitudeControl : public ModuleBase (ParamInt) _param_mpc_thr_curve, /**< throttle curve behavior */ (ParamInt) _param_mc_airmode, - (ParamFloat) _param_mc_man_tilt_tau + (ParamFloat) _param_mc_man_tilt_tau, + (ParamFloat) _param_mpc_rcac_att_sw, + (ParamFloat) _param_mpc_att_alpha, + (ParamFloat) _param_mpc_rcac_att_P0 + ) }; 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 c1a732704731..e86e5f42900e 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -98,6 +98,12 @@ MulticopterAttitudeControl::parameters_updated() radians(_param_mc_yawrate_max.get()))); _man_tilt_max = math::radians(_param_mpc_man_tilt_max.get()); + + //Set P0 for RCAC attitude and Rate controller + _attitude_control.set_RCAC_att_P0(_param_mpc_rcac_att_P0.get()); + _attitude_control.init_RCAC_att(); + PX4_INFO("Att Control P0:\t%8.6f", (double)_param_mpc_rcac_att_P0.get()); + } float @@ -216,6 +222,30 @@ MulticopterAttitudeControl::generate_attitude_setpoint(const Quatf &q, float dt, _vehicle_attitude_setpoint_pub.publish(attitude_setpoint); } +void +MulticopterAttitudeControl::publish_rcac_att_variables() +{ + _rcac_att_variables.timestamp = hrt_absolute_time(); + _rcac_att_variables.ii_att = _attitude_control.get_RCAC_att_ii(); + + + _rcac_att_variables.switch_att = _attitude_control.get_RCAC_att_switch(); + + _rcac_att_variables.alpha_pid_att = _attitude_control.get_alpha_PID_att(); + + _rcac_att_variables.p11_att = _attitude_control.get_RCAC_P11_Att(); + + for (int i = 0; i <= 2; i++) { + _rcac_att_variables.rcac_att_z[i] = _attitude_control.get_RCAC_att_z()(i); + _rcac_att_variables.rcac_att_u[i] = _attitude_control.get_RCAC_att_u()(i); + _rcac_att_variables.rcac_att_theta[i] = _attitude_control.get_RCAC_att_theta()(i); + + _rcac_att_variables.px4_att_theta[i] = _attitude_control.get_PX4_att_theta()(i); + + } + _rcac_att_variables_pub.publish(_rcac_att_variables); +} + void MulticopterAttitudeControl::Run() { @@ -260,6 +290,32 @@ MulticopterAttitudeControl::Run() _quat_reset_counter = v_att.quat_reset_counter; } + //RCtopic + // Ankit: RCAC switches and the PID scaling factor controlled by the RC transmitter + _rc_channels_sub.update(&_rc_channels_switch); + float RCAC_switch = _rc_channels_switch.channels[14]; + float PID_scale_f = _rc_channels_switch.channels[13]; + // SITL + //RCAC_switch = -1.0f; + //PID_scale_f = 1.0f; + if (RCAC_switch>0.0f) + { + _attitude_control.set_RCAC_att_switch(_param_mpc_rcac_att_sw.get()); + } + else + { + _attitude_control.set_RCAC_att_switch(RCAC_switch); + } + //_attitude_control.set_RCAC_att_switch(RCAC_switch); + _attitude_control.set_PID_att_factor(PID_scale_f, _param_mpc_att_alpha.get()); + + // _attitude_control.set_RCAC_att_switch(_rc_channels_switch.channels[14]); + // _attitude_control.set_PID_att_factor(_rc_channels_switch.channels[13]); + + // // Ankit: RCAC switches and the PID scaling factor for SITL testing + // _attitude_control.set_RCAC_att_switch(-1.0f); + // _attitude_control.set_PID_att_factor(1.0f); + // Guard against too small (< 0.2ms) and too large (> 20ms) dt's. const float dt = math::constrain(((v_att.timestamp - _last_run) * 1e-6f), 0.0002f, 0.02f); _last_run = v_att.timestamp; @@ -268,11 +324,13 @@ MulticopterAttitudeControl::Run() _manual_control_setpoint_sub.update(&_manual_control_setpoint); _v_control_mode_sub.update(&_v_control_mode); + if (_vehicle_land_detected_sub.updated()) { vehicle_land_detected_s vehicle_land_detected; if (_vehicle_land_detected_sub.copy(&vehicle_land_detected)) { _landed = vehicle_land_detected.landed; + _maybe_landed = vehicle_land_detected.maybe_landed; } } @@ -321,8 +379,10 @@ MulticopterAttitudeControl::Run() _man_x_input_filter.reset(0.f); _man_y_input_filter.reset(0.f); } - - Vector3f rates_sp = _attitude_control.update(q); + + //Juan: Made changes to controller inputs + const bool landed = _maybe_landed || _landed; + Vector3f rates_sp = _attitude_control.update(q, landed); if (_v_control_mode.flag_control_yawrate_override_enabled) { /* Yaw rate override enabled, overwrite the yaw setpoint */ @@ -343,6 +403,7 @@ MulticopterAttitudeControl::Run() v_rates_sp.timestamp = hrt_absolute_time(); _v_rates_sp_pub.publish(v_rates_sp); + publish_rcac_att_variables(); } // reset yaw setpoint during transitions, tailsitter.cpp generates @@ -355,6 +416,7 @@ MulticopterAttitudeControl::Run() perf_end(_loop_perf); } + int MulticopterAttitudeControl::task_spawn(int argc, char *argv[]) { bool vtol = false; diff --git a/src/modules/mc_att_control/mc_att_control_params.c b/src/modules/mc_att_control/mc_att_control_params.c index 6ac8fd1479b4..6e6e7696d888 100644 --- a/src/modules/mc_att_control/mc_att_control_params.c +++ b/src/modules/mc_att_control/mc_att_control_params.c @@ -175,3 +175,35 @@ PARAM_DEFINE_FLOAT(MC_RATT_TH, 0.8f); * @group Multicopter Position Control */ PARAM_DEFINE_FLOAT(MC_MAN_TILT_TAU, 0.0f); + +/** + * RCAC Attitude switches. + * + * Specifies RCAC state. + * + * @min -10 + * @max 10 + * @group Multicopter Attitude Control + */ +PARAM_DEFINE_FLOAT(MPC_RCAC_ATT_SW, -1.0f); + +/** + * PID Attitude gain. + * + * Specifies gain applied to the PID attitude gains. + * + * @min -10 + * @max 10 + * @group Multicopter RCAC Control + */ +PARAM_DEFINE_FLOAT(MPC_ATT_ALPHA, 1.0f); + +/** + * P0 for the Attitude controller + * + * @min 0.0 + * @max 10.0 + * @decimal 8 + * @group Multicopter Attitude Control + */ +PARAM_DEFINE_FLOAT(MPC_RCAC_ATT_P0, 0.01f); diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.cpp b/src/modules/mc_pos_control/MulticopterPositionControl.cpp index d2c4f6b47ee8..aebe55e3e7fc 100644 --- a/src/modules/mc_pos_control/MulticopterPositionControl.cpp +++ b/src/modules/mc_pos_control/MulticopterPositionControl.cpp @@ -73,6 +73,8 @@ bool MulticopterPositionControl::init() _time_stamp_last_loop = hrt_absolute_time(); + _control.init_RCAC(_param_mpc_rcac_pos_p0.get(), _param_mpc_rcac_vel_p0.get()); + return true; } @@ -140,6 +142,8 @@ int MulticopterPositionControl::parameters_update(bool force) _param_mpc_land_speed.set(math::min(_param_mpc_land_speed.get(), _param_mpc_z_vel_max_dn.get())); } + _control.resetRCAC(_param_mpc_rcac_pos_p0.get(), _param_mpc_rcac_vel_p0.get()); + return OK; } @@ -156,6 +160,7 @@ void MulticopterPositionControl::poll_subscriptions() } } } + _rc_channels_sub.update(&_rc_channels_switch); } void MulticopterPositionControl::set_vehicle_states(const float &vel_sp_z) @@ -213,6 +218,40 @@ void MulticopterPositionControl::set_vehicle_states(const float &vel_sp_z) } } +void MulticopterPositionControl::publish_rcac_pos_vel_variables(float pid_scale, float rcac_switch) +{ + _rcac_pos_vel_variables.timestamp = hrt_absolute_time(); + // _rcac_pos_vel_variables.rcac_alpha[0] = _rc_channels_switch.channels[13]; + // _rcac_pos_vel_variables.rcac_alpha[1] = _rc_channels_switch.channels[14]; + // _rcac_pos_vel_variables.rcac_alpha[0] = PID_scale_f; + // _rcac_pos_vel_variables.rcac_alpha[1] = RCAC_switch; + _rcac_pos_vel_variables.pid_factor = pid_scale; + _rcac_pos_vel_variables.rcac_master_sw = rcac_switch; + _rcac_pos_vel_variables.ii_pos = _control.get_RCAC_pos_ii(); + _rcac_pos_vel_variables.ii_vel = _control.get_RCAC_vel_ii(); + _rcac_pos_vel_variables.switch_pos = _control.get_RCAC_pos_switch(); + _rcac_pos_vel_variables.switch_vel = _control.get_RCAC_vel_switch(); + _rcac_pos_vel_variables.alpha_pid_pos = _control.get_pid_pos_alpha(); + _rcac_pos_vel_variables.alpha_pid_vel = _control.get_pid_vel_alpha(); + _rcac_pos_vel_variables.p11_pos = _control.get_RCAC_P11_Pos(); + _rcac_pos_vel_variables.p11_velx = _control.get_RCAC_P11_Velx(); + + for (int i = 0; i <= 2; i++) { + _rcac_pos_vel_variables.rcac_pos_z[i] = _control.get_RCAC_pos_z()(i); + _rcac_pos_vel_variables.rcac_pos_u[i] = _control.get_RCAC_pos_u()(i); + _rcac_pos_vel_variables.rcac_pos_theta[i] = _control.get_RCAC_pos_theta()(i); + + _rcac_pos_vel_variables.rcac_vel_z[i] = _control.get_RCAC_vel_z()(i); + _rcac_pos_vel_variables.rcac_vel_u[i] = _control.get_RCAC_vel_u()(i); + } + for (int i = 0; i <= 8; i++) { + _rcac_pos_vel_variables.rcac_vel_theta[i] = _control.get_RCAC_vel_theta()(i,0); + _rcac_pos_vel_variables.px4_ol_theta[i] = _control.get_PX4_ol_theta()(i,0); + } + _rcac_pos_vel_variables_pub.publish(_rcac_pos_vel_variables); + +} + void MulticopterPositionControl::Run() { if (should_exit()) { @@ -226,6 +265,27 @@ void MulticopterPositionControl::Run() if (_local_pos_sub.update(&_local_pos)) { poll_subscriptions(); + + + float RCAC_switch = _rc_channels_switch.channels[14]; + // SITL 1 + //RCAC_switch = -1.0f; + if (RCAC_switch>0.0f) + { + _control.set_RCAC_pos_switch(_param_mpc_rcac_pos_sw.get()); + _control.set_RCAC_vel_switch(_param_mpc_rcac_vel_sw.get()); + + } + else + { + _control.set_RCAC_pos_switch(RCAC_switch); + _control.set_RCAC_vel_switch(RCAC_switch); + } + float PID_scale_f = _rc_channels_switch.channels[13]; + // SITL 2 + //PID_scale_f = 1.0f; + _control.set_PID_pv_factor(PID_scale_f, _param_mpc_pos_alpha.get(), _param_mpc_vel_alpha.get()); + parameters_update(false); const hrt_abstime time_stamp_now = _local_pos.timestamp; @@ -256,6 +316,7 @@ void MulticopterPositionControl::Run() if (constraints.reset_integral) { _control.resetIntegral(); + _control.resetRCAC(_param_mpc_rcac_pos_p0.get(), _param_mpc_rcac_vel_p0.get()); } } @@ -298,11 +359,14 @@ void MulticopterPositionControl::Run() _control.getAttitudeSetpoint(attitude_setpoint); _vehicle_attitude_setpoint_pub.publish(attitude_setpoint); +publish_rcac_pos_vel_variables(PID_scale_f, RCAC_switch); + } else { // reset the numerical derivatives to not generate d term spikes when coming from non-position controlled operation _vel_x_deriv.reset(); _vel_y_deriv.reset(); _vel_z_deriv.reset(); + publish_rcac_pos_vel_variables(PID_scale_f, RCAC_switch); } } diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.hpp b/src/modules/mc_pos_control/MulticopterPositionControl.hpp index 4a3ebba52178..4466e2c5559e 100644 --- a/src/modules/mc_pos_control/MulticopterPositionControl.hpp +++ b/src/modules/mc_pos_control/MulticopterPositionControl.hpp @@ -60,6 +60,8 @@ #include #include #include +#include +#include #include "PositionControl/PositionControl.hpp" @@ -91,6 +93,10 @@ class MulticopterPositionControl : public ModuleBase uORB::Publication _local_pos_sp_pub{ORB_ID(vehicle_local_position_setpoint)}; /**< vehicle local position setpoint publication */ + uORB::Publication _rcac_pos_vel_variables_pub{ORB_ID(rcac_pos_vel_variables)}; /**< RCAC variables log */ + + + uORB::SubscriptionCallbackWorkItem _local_pos_sub{this, ORB_ID(vehicle_local_position)}; /**< vehicle local position */ uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; @@ -99,6 +105,7 @@ class MulticopterPositionControl : public ModuleBase uORB::Subscription _hover_thrust_estimate_sub{ORB_ID(hover_thrust_estimate)}; uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)}; uORB::Subscription _vehicle_constraints_sub{ORB_ID(vehicle_constraints)}; + uORB::Subscription _rc_channels_sub{ORB_ID(rc_channels)}; /**< Switch from the RC channel */ hrt_abstime _time_stamp_last_loop{0}; /**< time stamp of last loop iteration */ @@ -106,6 +113,8 @@ class MulticopterPositionControl : public ModuleBase vehicle_control_mode_s _control_mode{}; /**< vehicle control mode */ vehicle_local_position_s _local_pos{}; /**< vehicle local position */ + rcac_pos_vel_variables_s _rcac_pos_vel_variables{}; /**< RCAC variables */ + rc_channels_s _rc_channels_switch{}; /**< Switch from the RC channel */ DEFINE_PARAMETERS( // Position Control @@ -123,6 +132,12 @@ class MulticopterPositionControl : public ModuleBase (ParamFloat) _param_mpc_tiltmax_air, (ParamFloat) _param_mpc_thr_hover, (ParamBool) _param_mpc_use_hte, + (ParamFloat) _param_mpc_rcac_pos_sw, + (ParamFloat) _param_mpc_rcac_vel_sw, + (ParamFloat) _param_mpc_pos_alpha, + (ParamFloat) _param_mpc_vel_alpha, + (ParamFloat) _param_mpc_rcac_pos_p0, + (ParamFloat) _param_mpc_rcac_vel_p0, // Takeoff / Land (ParamFloat) _param_mpc_tko_speed, @@ -149,6 +164,8 @@ class MulticopterPositionControl : public ModuleBase bool _in_failsafe = false; /**< true if failsafe was entered within current cycle */ + bool _rcac_logging = true; /**< True if logging the aircraft state variable */ //TODO: MAV integration + bool _hover_thrust_initialized{false}; /** Timeout in us for trajectory data to get considered invalid */ @@ -164,6 +181,13 @@ class MulticopterPositionControl : public ModuleBase perf_counter_t _cycle_perf; + /** + * Publish RCAC position and velocity variables for logging and mavlink send commands + * @param pid_scale Denotes the gain by which regular PID commands are scaled. + * @param rcac_switch Denotes the state of the swicth taht enables RCAC. + */ + void publish_rcac_pos_vel_variables(float pid_scale, float rcac_switch); + /** * Update our local parameter cache. * Parameter update can be forced when argument is true. diff --git a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp index 8cdafc284793..eadd95c2272a 100644 --- a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp +++ b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp @@ -129,6 +129,34 @@ void PositionControl::_positionControl() { // P-position controller Vector3f vel_sp_position = (_pos_sp - _pos).emult(_gain_pos_p); + + z_k_Pr_R = (_pos_sp - _pos); + u_k_Pr_R.setZero(); + if (RCAC_Pr_ON) + { + ii_Pr_R += 1; + + // Regressor + for (int i=0; i<3; i++) { + phi_k_Pr_R(i, i) = z_k_Pr_R(i,0); + } + Gamma_Pr_R = phi_km1_Pr_R * P_Pr_R * phi_km1_Pr_R.T() + I3; + Gamma_Pr_R = Gamma_Pr_R.I(); + P_Pr_R -= (P_Pr_R * phi_km1_Pr_R.T()) * Gamma_Pr_R * (phi_km1_Pr_R * P_Pr_R); + theta_k_Pr_R += (P_Pr_R * phi_km1_Pr_R.T()) * N1_Pr * + (z_k_Pr_R + N1_Pr*(phi_km1_Pr_R * theta_k_Pr_R - u_km1_Pr_R) ); + u_k_Pr_R = phi_k_Pr_R * theta_k_Pr_R; + u_km1_Pr_R = u_k_Pr_R; + phi_km1_Pr_R = phi_k_Pr_R; + + + // PX4_INFO("Pos Control u :\t%8.6f\t%8.6f\t%8.6f", (double)P_Pr_R(0,0), (double)P_Pr_R(1,1), (double)P_Pr_R(2,2)); + // PX4_INFO("Pos Control u :\t%8.6f\t%8.6f\t%8.6f", (double)N1_vel(0), (double)N1_vel(1), (double)N1_vel(2)); + // PX4_INFO("Pos Control P0:\t%8.6f\t%8.6f", (double)_param_mpc_rcac_pos_p0.get(), (double)P_Pr_R(0,0) ); + } + //vel_sp_position = alpha_PID*vel_sp_position + u_k_Pr_R; + vel_sp_position = alpha_PID_pos*vel_sp_position + u_k_Pr_R; + // Position and feed-forward velocity setpoints or position states being NAN results in them not having an influence ControlMath::addIfNotNanVector3f(_vel_sp, vel_sp_position); // make sure there are no NAN elements for further reference while constraining @@ -147,6 +175,61 @@ void PositionControl::_velocityControl(const float dt) Vector3f vel_error = _vel_sp - _vel; Vector3f acc_sp_velocity = vel_error.emult(_gain_vel_p) + _vel_int - _vel_dot.emult(_gain_vel_d); + z_k_vel = vel_error; + u_k_vel.setZero(); + if (RCAC_Pv_ON) + { + ii_Pv_R += 1; + // Ankit 01 30 2020:New SISO implementation + z_k_vel = vel_error; + + phi_k_vel_x(0,0) = vel_error(0); + phi_k_vel_x(0,1) = _vel_int(0); + phi_k_vel_x(0,2) = _vel_dot(0) * 0; + + phi_k_vel_y(0,0) = vel_error(1); + phi_k_vel_y(0,1) = _vel_int(1); + phi_k_vel_y(0,2) = _vel_dot(1) * 0; + + phi_k_vel_z(0,0) = vel_error(2); + phi_k_vel_z(0,1) = _vel_int(2); + phi_k_vel_z(0,2) = _vel_dot(2) * 0; + + dummy1 = phi_km1_vel_x * P_vel_x * phi_km1_vel_x.T() + 1.0f; + dummy2 = phi_km1_vel_y * P_vel_y * phi_km1_vel_y.T() + 1.0f; + dummy3 = phi_km1_vel_z * P_vel_z * phi_km1_vel_z.T() + 1.0f; + Gamma_vel(0) = dummy1(0,0); + Gamma_vel(1) = dummy2(0,0); + Gamma_vel(2) = dummy3(0,0); + + P_vel_x = P_vel_x - (P_vel_x * phi_km1_vel_x.T()) * (phi_km1_vel_x * P_vel_x) / Gamma_vel(0); + P_vel_y = P_vel_y - (P_vel_y * phi_km1_vel_y.T()) * (phi_km1_vel_y * P_vel_y) / Gamma_vel(1); + P_vel_z = P_vel_z - (P_vel_z * phi_km1_vel_z.T()) * (phi_km1_vel_z * P_vel_z) / Gamma_vel(2); + + dummy1 = N1_vel(0)*(phi_km1_vel_x * theta_k_vel_x - u_km1_vel(0)); + dummy2 = N1_vel(1)*(phi_km1_vel_y * theta_k_vel_y - u_km1_vel(1)); + dummy3 = N1_vel(2)*(phi_km1_vel_z * theta_k_vel_z - u_km1_vel(2)); + theta_k_vel_x = theta_k_vel_x + (P_vel_x * phi_km1_vel_x.T()) * N1_vel(0) *(z_k_vel(0) + dummy1(0,0)); + theta_k_vel_y = theta_k_vel_y + (P_vel_y * phi_km1_vel_y.T()) * N1_vel(1) *(z_k_vel(1) + dummy2(0,0)); + theta_k_vel_z = theta_k_vel_z + (P_vel_z * phi_km1_vel_z.T()) * N1_vel(2) *(z_k_vel(2) + dummy3(0,0)); + + dummy1 = phi_k_vel_x * theta_k_vel_x; + dummy2 = phi_k_vel_y * theta_k_vel_y; + dummy3 = phi_k_vel_z * theta_k_vel_z; + u_k_vel(0) = dummy1(0,0); + u_k_vel(1) = dummy2(0,0); + u_k_vel(2) = dummy3(0,0); + + u_km1_vel = u_k_vel; + + phi_km1_vel_x = phi_k_vel_x; + phi_km1_vel_y = phi_k_vel_y; + phi_km1_vel_z = phi_k_vel_z; + + } + + acc_sp_velocity = alpha_PID_vel*acc_sp_velocity + u_k_vel; + // No control input from setpoints or corresponding states which are NAN ControlMath::addIfNotNanVector3f(_acc_sp, acc_sp_velocity); @@ -248,3 +331,215 @@ void PositionControl::getAttitudeSetpoint(vehicle_attitude_setpoint_s &attitude_ ControlMath::thrustToAttitude(_thr_sp, _yaw_sp, attitude_setpoint); attitude_setpoint.yaw_sp_move_rate = _yawspeed_sp; } + + const matrix::Vector3f PositionControl::get_RCAC_pos_z() + { + matrix::Vector3f RCAC_z{}; + + for (int i = 0; i <= 2; i++) { + RCAC_z(i) = z_k_Pr_R(i,0); + } + + return RCAC_z; + } + + const matrix::Vector3f PositionControl::get_RCAC_pos_u() + { + matrix::Vector3f RCAC_u{}; + + for (int i = 0; i <= 2; i++) { + RCAC_u(i) = u_k_Pr_R(i,0); + } + + return RCAC_u; + } + + const matrix::Vector3f PositionControl::get_RCAC_pos_theta() + { + matrix::Vector3f RCAC_theta{}; + + for (int i = 0; i <= 2; i++) { + RCAC_theta(i) = theta_k_Pr_R(i,0); + } + + return RCAC_theta; + } + + const matrix::Vector3f PositionControl::get_PX4_pos_theta() + { + matrix::Vector3f PX4_theta{}; + PX4_theta(0) = _gain_pos_p(0); + PX4_theta(1) = _gain_pos_p(1); + PX4_theta(2) = _gain_pos_p(2); + return PX4_theta; + } + + const matrix::Matrix PositionControl::get_PX4_ol_theta() + { + matrix::Matrix PX4_theta{}; + PX4_theta(0,0) = _gain_pos_p(0); + PX4_theta(1,0) = _gain_pos_p(1); + PX4_theta(2,0) = _gain_pos_p(2); + + PX4_theta(3,0) = _gain_vel_p(0); + PX4_theta(4,0) = _gain_vel_i(0); + PX4_theta(5,0) = _gain_vel_d(0); + + PX4_theta(6,0) = _gain_vel_p(2); + PX4_theta(7,0) = _gain_vel_i(2); + PX4_theta(8,0) = _gain_vel_d(2); + + return PX4_theta; + } + + const matrix::Vector3f PositionControl::get_RCAC_vel_z() + { + matrix::Vector3f RCAC_z{}; + + for (int i = 0; i <= 2; i++) { + RCAC_z(i) = z_k_vel(i); //z_k_Pv_R(i,0); + } + + return RCAC_z; + } + + const matrix::Vector3f PositionControl::get_RCAC_vel_u() + { + matrix::Vector3f RCAC_u{}; + + for (int i = 0; i <= 2; i++) { + RCAC_u(i) = u_k_vel(i); //u_k_Pv_R(i,0); + } + + return RCAC_u; + } + + const matrix::Matrix PositionControl::get_RCAC_vel_theta() + { + matrix::Matrix RCAC_theta{}; + + for (int i = 0; i <= 2; i++) { + //RCAC_theta(i,0) = theta_k_Pv_R(i,0); + RCAC_theta(i,0) = theta_k_vel_x(i); + RCAC_theta(i+3,0) = theta_k_vel_y(i); + RCAC_theta(i+6,0) = theta_k_vel_z(i); + } + + return RCAC_theta; + } + + void PositionControl::set_RCAC_pos_switch(float switch_RCAC) + { + RCAC_Pr_ON = 1; + if (switch_RCAC<0.0f) { + RCAC_Pr_ON = 0; + } + } + + void PositionControl::set_RCAC_vel_switch(float switch_RCAC) + { + RCAC_Pv_ON = 1; + if (switch_RCAC<0.0f) { + RCAC_Pv_ON = 0; + } + } + + void PositionControl::set_PID_pv_factor(float PID_factor, float pos_alpha, float vel_alpha) + { + alpha_PID_pos = 1.0f; + alpha_PID_vel = 1.0f; + //alpha_PID = 1.0f; + + if (PID_factor<0.0f) { + //alpha_PID = 0.5; + alpha_PID_pos = pos_alpha; + alpha_PID_vel = vel_alpha; + } + + } + +void PositionControl::init_RCAC(float rcac_pos_p0, float rcac_vel_p0) { + I3 = eye(); + N1_Pr = eye() * (1.0f); + + for (int i = 0; i <= 2; i++) { + N1_vel(i) = 1; + } + P_Pr_R = eye() * 0.010 ; + + phi_k_Pr_R.setZero(); + phi_km1_Pr_R.setZero(); + theta_k_Pr_R.setZero(); + z_k_Pr_R.setZero(); + z_km1_Pr_R.setZero(); + u_k_Pr_R.setZero(); + u_km1_Pr_R.setZero(); + Gamma_Pr_R.setZero(); + + P_vel_x = eye() * 0.0010; + P_vel_y = eye() * 0.0010; + P_vel_z = eye() * 0.0010; + phi_k_vel_x.setZero(); + phi_k_vel_y.setZero(); + phi_k_vel_z.setZero(); + phi_km1_vel_x.setZero(); + phi_km1_vel_y.setZero(); + phi_km1_vel_z.setZero(); + theta_k_vel_x.setZero(); + theta_k_vel_y.setZero(); + theta_k_vel_z.setZero(); + u_k_vel.setZero(); + z_k_vel.setZero(); + + P_Pr_R = eye() * rcac_pos_p0; + P_vel_x = eye() * rcac_vel_p0; + P_vel_y = eye() * rcac_vel_p0; + P_vel_z = eye() * rcac_vel_p0; + + PX4_INFO("Pos Control P0:\t%8.6f", (double)rcac_pos_p0); + PX4_INFO("Vel Control P0:\t%8.6f", (double)rcac_vel_p0); +} + + void PositionControl::resetRCAC(float rcac_pos_p0, float rcac_vel_p0) + { + // P_vel_x = eye() * 0.0010; + // P_vel_y = eye() * 0.0010; + // P_vel_z = eye() * 0.0010; + // P_Pr_R = eye() * 0.010 * alpha_P; + + P_vel_x.setZero(); + P_vel_y.setZero(); + P_vel_z.setZero(); + P_Pr_R.setZero(); + for (int i = 0; i <= 2; i++) { + P_vel_x(i,i) = 0.001; + P_vel_y(i,i) = 0.001; + P_vel_z(i,i) = 0.001; + P_Pr_R(i,i) = 0.01; + P_vel_x(i,i) = rcac_vel_p0; + P_vel_y(i,i) = rcac_vel_p0; + P_vel_z(i,i) = rcac_vel_p0; + P_Pr_R(i,i) = rcac_pos_p0; + } + phi_k_vel_x.setZero(); + phi_k_vel_y.setZero(); + phi_k_vel_z.setZero(); + phi_km1_vel_x.setZero(); + phi_km1_vel_y.setZero(); + phi_km1_vel_z.setZero(); + theta_k_vel_x.setZero(); + theta_k_vel_y.setZero(); + theta_k_vel_z.setZero(); + u_k_vel.setZero(); + z_k_vel.setZero(); + + phi_km1_Pr_R.setZero(); + theta_k_Pr_R.setZero(); + z_k_Pr_R.setZero(); + z_km1_Pr_R.setZero(); + u_k_Pr_R.setZero(); + u_km1_Pr_R.setZero(); + + ii_Pr_R = 0; + ii_Pv_R = 0; + } diff --git a/src/modules/mc_pos_control/PositionControl/PositionControl.hpp b/src/modules/mc_pos_control/PositionControl/PositionControl.hpp index 4986c93a86f8..a3f1ac24ab0a 100644 --- a/src/modules/mc_pos_control/PositionControl/PositionControl.hpp +++ b/src/modules/mc_pos_control/PositionControl/PositionControl.hpp @@ -179,6 +179,141 @@ class PositionControl */ void getAttitudeSetpoint(vehicle_attitude_setpoint_s &attitude_setpoint) const; + /** + * Get the + * @see z_k_Pr_R + * @return The z variable used by RCAC in the P controller + */ + const matrix::Vector3f get_RCAC_pos_z(); + + /** + * Get the + * @see u_k_Pr_R + * @return The u variable computed by RCAC in the P controller + */ + const matrix::Vector3f get_RCAC_pos_u(); + /** + * Get the + * @see theta_k_Pr_R + * @return The theta variable computed by RCAC in the P controller + */ + const matrix::Vector3f get_RCAC_pos_theta(); + + /** + * Get the + * @see Pos P gains + * @return The P gains + */ + const matrix::Vector3f get_PX4_pos_theta(); + + /** + * Get the + * @see PID gains + * @return PX4's PID gains in the outer loop + */ + const matrix::Matrix get_PX4_ol_theta(); + + /** + * Get the + * @see z_k_Pv_R + * @return The z variable used by RCAC in the PID velocity controller + */ + const matrix::Vector3f get_RCAC_vel_z(); + + /** + * Get the + * @see u_k_Pr_R + * @return The u variable computed by RCAC in the P controller + */ + const matrix::Vector3f get_RCAC_vel_u(); + + /** + * Get the + * @see theta_k_Pr_R + * @return The theta variable computed by RCAC in the P controller + */ + const matrix::Matrix get_RCAC_vel_theta(); + + /** + * Get the + * @see ii + * @return Iteration step of the RCAC position controller + */ + const int &get_RCAC_pos_ii() { return ii_Pr_R; } + + /** + * Get the + * @see ii + * @return Iteration step of the RCAC velocity controller + */ + const int &get_RCAC_vel_ii() { return ii_Pv_R; } + + /** + * Set the RCAC position switch. + * @see _thr_int + */ + void set_RCAC_pos_switch(float switch_RCAC); + + /** + * Set the RCAC velocity switch. + * @see _thr_int + */ + void set_RCAC_vel_switch(float switch_RCAC); + + /** + * Set the PID scaling factor. + * @see _thr_int + */ + void set_PID_pv_factor(float PID_factor, float pos_alpha, float vel_alpha); + /** + * Get the + * @see RCAC_Pr_ON + * @return Get RCAC pos controller switch + */ + const bool &get_RCAC_pos_switch() {return RCAC_Pr_ON;} + + /** + * Get the + * @see RCAC_Pr_ON + * @return Get RCAC vel controller switch + */ + const bool &get_RCAC_vel_switch() {return RCAC_Pv_ON;} + + /** + * Get the + * @see alpha_PID_pos + * @return Get gain that multiplies the position PID gains + */ + const float &get_pid_pos_alpha() {return alpha_PID_pos;} + + /** + * Get the + * @see alpha_PID_vel + * @return Get gain that multiplies the velocity PID gains + */ + const float &get_pid_vel_alpha() {return alpha_PID_vel;} + + /** + * Get the + * @see P_Pr_R + * @return RCAC P(1,1) of the Position controller + */ + const float &get_RCAC_P11_Pos() { return P_Pr_R(0,0); } + + /** + * Get the + * @see P_vel_x + * @return RCAC P(1,1) of the Velcity x controller + */ + const float &get_RCAC_P11_Velx() { return P_vel_x(0,0); } + + /** + * Reset RCAC variables + * @see _thr_int + */ + void resetRCAC(float rcac_pos_p0, float rcac_vel_p0); + void init_RCAC(float rcac_pos_p0, float rcac_vel_p0); + private: bool _updateSuccessful(); @@ -218,4 +353,41 @@ class PositionControl matrix::Vector3f _thr_sp; /**< desired thrust */ float _yaw_sp{}; /**< desired heading */ float _yawspeed_sp{}; /** desired yaw-speed */ + + // RCAC + int ii_Pr_R = 0; + bool RCAC_Pr_ON=1; + matrix::SquareMatrix P_Pr_R; + matrix::Matrix phi_k_Pr_R, phi_km1_Pr_R; + matrix::Matrix theta_k_Pr_R; + matrix::Matrix z_k_Pr_R, z_km1_Pr_R,u_k_Pr_R, u_km1_Pr_R; + matrix::SquareMatrix Gamma_Pr_R; + + // const TODO: make really const. + matrix::SquareMatrix I3, N1_Pr; + + int ii_Pv_R = 0; + bool RCAC_Pv_ON=1; + bool _rcac_logging = true; /**< True if logging the aircraft state variable */ //TODO: MAV integration + + // matrix::SquareMatrix P_Pv_R; + // matrix::Matrix phi_k_Pv_R, phi_km1_Pv_R; + // matrix::Matrix theta_k_Pv_R,theta_k_Pv_PID; + // matrix::Matrix z_k_Pv_R, z_km1_Pv_R,u_k_Pv_R, u_km1_Pv_R; + // matrix::SquareMatrix Gamma_Pv_R, N1_Pv; + + matrix::SquareMatrix P_vel_x,P_vel_y,P_vel_z; + matrix::Matrix phi_k_vel_x, phi_km1_vel_x; + matrix::Matrix phi_k_vel_y, phi_km1_vel_y; + matrix::Matrix phi_k_vel_z, phi_km1_vel_z; + matrix::Vector3f theta_k_vel_x, theta_k_vel_y, theta_k_vel_z; + matrix::Vector3f z_k_vel, z_km1_vel, u_k_vel, u_km1_vel; + matrix::Vector3f N1_vel, Gamma_vel; + matrix::Matrix dummy1,dummy2,dummy3; + + //float alpha_PID = 1.0f; + float alpha_PID_pos = 1.0f; + float alpha_PID_vel = 1.0f; + + matrix::Vector3f Pv_intg; }; diff --git a/src/modules/mc_pos_control/mc_pos_control_params.c b/src/modules/mc_pos_control/mc_pos_control_params.c index 332490345aba..7b7df9148966 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -789,3 +789,67 @@ PARAM_DEFINE_FLOAT(MPC_SPOOLUP_TIME, 1.0f); * @group Mission */ PARAM_DEFINE_INT32(MPC_YAW_MODE, 0); + +/** + * RCAC Position switches. + * + * Specifies RCAC state. + * + * @min -10 + * @max 10 + * @group Multicopter RCAC Control + */ +PARAM_DEFINE_FLOAT(MPC_RCAC_POS_SW, 1.0f); + +/** + * RCAC Velocity switches. + * + * Specifies RCAC state. + * + * @min -10 + * @max 10 + * @group Multicopter RCAC Control + */ +PARAM_DEFINE_FLOAT(MPC_RCAC_VEL_SW, 1.0f); + +/** + * PID Position gain. + * + * Specifies gain applied to the PID position gains. + * + * @min -10 + * @max 10 + * @group Multicopter RCAC Control + */ +PARAM_DEFINE_FLOAT(MPC_POS_ALPHA, 1.0f); + +/** + * PID Velocity gain. + * + * Specifies gain applied to the PID velocity gains. + * + * @min -10 + * @max 10 + * @group Multicopter RCAC Control + */ +PARAM_DEFINE_FLOAT(MPC_VEL_ALPHA, 1.0f); + +/** + * P0 for the position controller + * + * @min 0.0 + * @max 10.0 + * @decimal 8 + * @group Multicopter RCAC Control + */ +PARAM_DEFINE_FLOAT(MPC_RCAC_POS_P0, 0.005f); + +/** + * P0 for the velocity controller + * + * @min 0.0 + * @max 10.0 + * @decimal 8 + * @group Multicopter RCAC Control + */ +PARAM_DEFINE_FLOAT(MPC_RCAC_VEL_P0, 0.001f); diff --git a/src/modules/mc_rate_control/MulticopterRateControl.cpp b/src/modules/mc_rate_control/MulticopterRateControl.cpp index dc97d17dbb48..f8c69dd27642 100644 --- a/src/modules/mc_rate_control/MulticopterRateControl.cpp +++ b/src/modules/mc_rate_control/MulticopterRateControl.cpp @@ -94,6 +94,39 @@ MulticopterRateControl::parameters_updated() radians(_param_mc_acro_y_max.get())); _actuators_0_circuit_breaker_enabled = circuit_breaker_enabled_by_val(_param_cbrk_rate_ctrl.get(), CBRK_RATE_CTRL_KEY); + + + //Set P0 for RCAC attitude and Rate controller + + _rate_control.set_RCAC_rate_P0(_param_mpc_rcac_rate_P0.get()); + _rate_control.init_RCAC_rate(); + PX4_INFO("Rate Control P0:\t%8.6f", (double)_param_mpc_rcac_rate_P0.get()); + +} + +void +MulticopterRateControl::publish_rcac_rate_variables() +{ + _rcac_rate_variables.timestamp = hrt_absolute_time(); + _rcac_rate_variables.ii_rate = _rate_control.get_RCAC_rate_ii(); + + _rcac_rate_variables.switch_rate = _rate_control.get_RCAC_rate_switch(); + + _rcac_rate_variables.alpha_pid_rate = _rate_control.get_alpha_PID_rate(); + + _rcac_rate_variables.p11_ratex = _rate_control.get_RCAC_P11_Ratex(); + + for (int i = 0; i <= 2; i++) { + + _rcac_rate_variables.rcac_rate_z[i] = _rate_control.get_RCAC_rate_z()(i); + _rcac_rate_variables.rcac_rate_u[i] = _rate_control.get_RCAC_rate_u()(i); + + } + for (int i = 0; i <= 11; i++) { + _rcac_rate_variables.rcac_rate_theta[i] = _rate_control.get_RCAC_rate_theta()(i,0); + _rcac_rate_variables.px4_rate_theta[i] = _rate_control.get_PX4_rate_theta()(i,0); + } + _rcac_rate_variables_pub.publish(_rcac_rate_variables); } void @@ -135,6 +168,34 @@ MulticopterRateControl::Run() const Vector3f angular_accel{v_angular_acceleration.xyz}; const Vector3f rates{angular_velocity.xyz}; + //RCtopic + // Ankit: RCAC switches and the PID scaling factor controlled by the RC transmitter + _rc_channels_sub.update(&_rc_channels_switch); + float RCAC_switch = _rc_channels_switch.channels[14]; + float PID_scale_f = _rc_channels_switch.channels[13]; + // SITL + //RCAC_switch = -1.0f; + //PID_scale_f = 1.0f; + if (RCAC_switch>0.0f) + { + _rate_control.set_RCAC_rate_switch(_param_mpc_rcac_rate_sw.get()); + } + else + { + _rate_control.set_RCAC_rate_switch(RCAC_switch); + } + //_rate_control.set_RCAC_rate_switch(RCAC_switch); + _rate_control.set_PID_rate_factor(PID_scale_f, _param_mpc_rate_alpha.get()); + + // _rate_control.set_RCAC_rate_switch(_rc_channels_switch.channels[14]); + // _rate_control.set_PID_rate_factor(_rc_channels_switch.channels[13]); + + // // Ankit: RCAC switches and the PID scaling factor for SITL testing + // _rate_control.set_RCAC_rate_switch(-1.0f); + // _rate_control.set_PID_rate_factor(1.0f); + + + /* check for updates in other topics */ _v_control_mode_sub.update(&_v_control_mode); @@ -276,6 +337,7 @@ MulticopterRateControl::Run() actuators.timestamp = hrt_absolute_time(); _actuators_0_pub.publish(actuators); + publish_rcac_rate_variables(); } else if (_v_control_mode.flag_control_termination_enabled) { if (!_vehicle_status.is_vtol) { @@ -283,6 +345,7 @@ MulticopterRateControl::Run() actuator_controls_s actuators{}; actuators.timestamp = hrt_absolute_time(); _actuators_0_pub.publish(actuators); + publish_rcac_rate_variables(); } } } diff --git a/src/modules/mc_rate_control/MulticopterRateControl.hpp b/src/modules/mc_rate_control/MulticopterRateControl.hpp index 68168480136f..221ee1445ddb 100644 --- a/src/modules/mc_rate_control/MulticopterRateControl.hpp +++ b/src/modules/mc_rate_control/MulticopterRateControl.hpp @@ -59,6 +59,8 @@ #include #include #include +#include +#include using namespace time_literals; @@ -86,6 +88,8 @@ class MulticopterRateControl : public ModuleBase, public * initialize some vectors/matrices from parameters */ void parameters_updated(); + + void publish_rcac_rate_variables(); RateControl _rate_control; ///< class for rate control calculations @@ -98,6 +102,7 @@ class MulticopterRateControl : public ModuleBase, public uORB::Subscription _vehicle_angular_acceleration_sub{ORB_ID(vehicle_angular_acceleration)}; uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + uORB::Subscription _rc_channels_sub{ORB_ID(rc_channels)}; /**< RC switch>*/ uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; @@ -107,10 +112,14 @@ class MulticopterRateControl : public ModuleBase, public uORB::PublicationMulti _controller_status_pub{ORB_ID(rate_ctrl_status)}; /**< controller status publication */ uORB::Publication _landing_gear_pub{ORB_ID(landing_gear)}; uORB::Publication _v_rates_sp_pub{ORB_ID(vehicle_rates_setpoint)}; /**< rate setpoint publication */ + uORB::Publication _rcac_rate_variables_pub{ORB_ID(rcac_rate_variables)}; /**< RCAC variables log */ + manual_control_setpoint_s _manual_control_setpoint{}; vehicle_control_mode_s _v_control_mode{}; vehicle_status_s _vehicle_status{}; + rc_channels_s _rc_channels_switch{}; /**< Switch from the RC channel */ + rcac_rate_variables_s _rcac_rate_variables{}; bool _actuators_0_circuit_breaker_enabled{false}; /**< circuit breaker to suppress output */ bool _landed{true}; @@ -164,7 +173,11 @@ class MulticopterRateControl : public ModuleBase, public (ParamBool) _param_mc_bat_scale_en, - (ParamInt) _param_cbrk_rate_ctrl + (ParamInt) _param_cbrk_rate_ctrl, + + (ParamFloat) _param_mpc_rcac_rate_sw, + (ParamFloat) _param_mpc_rate_alpha, + (ParamFloat) _param_mpc_rcac_rate_P0 ) matrix::Vector3f _acro_rate_max; /**< max attitude rates in acro mode */ diff --git a/src/modules/mc_rate_control/RateControl/RateControl.cpp b/src/modules/mc_rate_control/RateControl/RateControl.cpp index 37a252550619..7b6b54e63178 100644 --- a/src/modules/mc_rate_control/RateControl/RateControl.cpp +++ b/src/modules/mc_rate_control/RateControl/RateControl.cpp @@ -64,7 +64,89 @@ Vector3f RateControl::update(const Vector3f &rate, const Vector3f &rate_sp, cons Vector3f rate_error = rate_sp - rate; // PID control with feed forward - const Vector3f torque = _gain_p.emult(rate_error) + _rate_int - _gain_d.emult(angular_accel) + _gain_ff.emult(rate_sp); + //const Vector3f torque = _gain_p.emult(rate_error) + _rate_int - _gain_d.emult(angular_accel) + _gain_ff.emult(rate_sp); + Vector3f torque = _gain_p.emult(rate_error) + _rate_int - _gain_d.emult(angular_accel) + _gain_ff.emult(rate_sp); + + //PX4_INFO("Rate Controller:\t%8.4f", (double)dt); + if (landed) + { + ii_AC_R = 0; + // ii_Pq_R = 0; + } + z_k_rate = rate_error; + u_k_rate.setZero(); + if (!landed && RCAC_Aw_ON) + { + ii_AC_R = ii_AC_R + 1; + if (ii_AC_R == 1) + { + init_RCAC_rate(); + /*theta_k_Ac_PID(0,0) = _gain_p(0); + theta_k_Ac_PID(1,0) = _gain_i(0); + theta_k_Ac_PID(2,0) = _gain_d(0); + theta_k_Ac_PID(3,0) = _gain_ff(0); + theta_k_Ac_PID(4,0) = _gain_p(1); + theta_k_Ac_PID(5,0) = _gain_i(1); + theta_k_Ac_PID(6,0) = _gain_d(1); + theta_k_Ac_PID(7,0) = _gain_ff(1); + theta_k_Ac_PID(8,0) = _gain_p(2); + theta_k_Ac_PID(9,0) = _gain_i(2); + theta_k_Ac_PID(10,0) = _gain_d(2); + theta_k_Ac_PID(11,0) = _gain_ff(2);*/ + + } + + // Ankit 01 30 2020:New SISO implementation + z_k_rate = rate_error; + + phi_k_rate_x(0,0) = rate_error(0)*0; //Ankit: Disable P rate + phi_k_rate_x(0,1) = _rate_int(0); + phi_k_rate_x(0,2) = angular_accel(0) * 0; + phi_k_rate_x(0,3) = rate_sp(0) * 0; + + phi_k_rate_y(0,0) = rate_error(1); + phi_k_rate_y(0,1) = _rate_int(1); + phi_k_rate_y(0,2) = angular_accel(1) * 0; + phi_k_rate_y(0,3) = rate_sp(1) * 0; + + phi_k_rate_z(0,0) = rate_error(2); + phi_k_rate_z(0,1) = _rate_int(2); + phi_k_rate_z(0,2) = angular_accel(2) * 0; + phi_k_rate_z(0,3) = rate_sp(2) * 0; + + dummy1 = phi_km1_rate_x * P_rate_x * phi_km1_rate_x.T() + 1.0f; + dummy2 = phi_km1_rate_y * P_rate_y * phi_km1_rate_y.T() + 1.0f; + dummy3 = phi_km1_rate_z * P_rate_z * phi_km1_rate_z.T() + 1.0f; + Gamma_rate(0) = dummy1(0,0); + Gamma_rate(1) = dummy2(0,0); + Gamma_rate(2) = dummy3(0,0); + + P_rate_x = P_rate_x - (P_rate_x * phi_km1_rate_x.T()) * (phi_km1_rate_x * P_rate_x) / Gamma_rate(0); + P_rate_y = P_rate_y - (P_rate_y * phi_km1_rate_y.T()) * (phi_km1_rate_y * P_rate_y) / Gamma_rate(1); + P_rate_z = P_rate_z - (P_rate_z * phi_km1_rate_z.T()) * (phi_km1_rate_z * P_rate_z) / Gamma_rate(2); + + dummy1 = N1_rate(0)*(phi_km1_rate_x * theta_k_rate_x - u_km1_rate(0)); + dummy2 = N1_rate(1)*(phi_km1_rate_y * theta_k_rate_y - u_km1_rate(1)); + dummy3 = N1_rate(2)*(phi_km1_rate_z * theta_k_rate_z - u_km1_rate(2)); + theta_k_rate_x = theta_k_rate_x + (P_rate_x * phi_km1_rate_x.T()) * N1_rate(0) *(z_k_rate(0) + dummy1(0,0)); + theta_k_rate_y = theta_k_rate_y + (P_rate_y * phi_km1_rate_y.T()) * N1_rate(1) *(z_k_rate(1) + dummy2(0,0)); + theta_k_rate_z = theta_k_rate_z + (P_rate_z * phi_km1_rate_z.T()) * N1_rate(2) *(z_k_rate(2) + dummy3(0,0)); + + dummy1 = phi_k_rate_x * theta_k_rate_x; + dummy2 = phi_k_rate_y * theta_k_rate_y; + dummy3 = phi_k_rate_z * theta_k_rate_z; + u_k_rate(0) = dummy1(0,0); + u_k_rate(1) = dummy2(0,0); + u_k_rate(2) = dummy3(0,0); + + u_km1_rate = u_k_rate; + + phi_km1_rate_x = phi_k_rate_x; + phi_km1_rate_y = phi_k_rate_y; + phi_km1_rate_z = phi_k_rate_z; + } + //torque = alpha_PID*torque+u_k_rate; + torque = alpha_PID_rate*torque+u_k_rate; // update integral only if we are not landed if (!landed) { diff --git a/src/modules/mc_rate_control/RateControl/RateControl.hpp b/src/modules/mc_rate_control/RateControl/RateControl.hpp index 6f0783a34953..cbcd60050acb 100644 --- a/src/modules/mc_rate_control/RateControl/RateControl.hpp +++ b/src/modules/mc_rate_control/RateControl/RateControl.hpp @@ -100,6 +100,181 @@ class RateControl */ void getRateControlStatus(rate_ctrl_status_s &rate_ctrl_status); + /** + * Get the + * @see z_k_Pr_R + * @return The z variable used by RCAC in the PID+FF controller + */ + const matrix::Vector3f get_RCAC_rate_z() + { + matrix::Vector3f RCAC_z{}; + + for (int i = 0; i <= 2; i++) { + RCAC_z(i) = z_k_rate(i); //z_k_AC_R(i,0); + } + + return RCAC_z; + } + + /** + * Get the + * @see u_k_Pr_R + * @return The u variable computed by RCAC in the PID+FF controller + */ + const matrix::Vector3f get_RCAC_rate_u() + { + matrix::Vector3f RCAC_u{}; + + for (int i = 0; i <= 2; i++) { + RCAC_u(i) = u_k_rate(i); //u_k_AC_R(i,0); + } + + return RCAC_u; + } + + /** + * Get the + * @see theta_k_Pr_R + * @return The theta variable computed by RCAC in the PID+FF controller + */ + const matrix::Matrix get_RCAC_rate_theta() + { + matrix::Matrix RCAC_theta{}; + + for (int i = 0; i <= 3; i++) { + // RCAC_theta(i,0) = theta_k_AC_R(i,0); + RCAC_theta(i,0) = theta_k_rate_x(i,0); + RCAC_theta(i+4,0) = theta_k_rate_y(i,0); + RCAC_theta(i+8,0) = theta_k_rate_z(i,0); + } + + return RCAC_theta; + } + + /** + * Get the + * @see gains + * @return PX4 PID gains for the rate controller + */ + const matrix::Matrix get_PX4_rate_theta() + { + matrix::Matrix PX4_rate_theta{}; + + PX4_rate_theta(0,0) = _gain_p(0); + PX4_rate_theta(1,0) = _gain_i(0); + PX4_rate_theta(2,0) = _gain_d(0); + PX4_rate_theta(3,0) = _gain_ff(0); + PX4_rate_theta(4,0) = _gain_p(1); + PX4_rate_theta(5,0) = _gain_i(1); + PX4_rate_theta(6,0) = _gain_d(1); + PX4_rate_theta(7,0) = _gain_ff(1); + PX4_rate_theta(8,0) = _gain_p(2); + PX4_rate_theta(9,0) = _gain_i(2); + PX4_rate_theta(10,0) = _gain_d(2); + PX4_rate_theta(11,0) = _gain_ff(2); + + return PX4_rate_theta; + } + + /** + * Get the + * @see ii + * @return Iteration step of the RCAC rate controller + */ + const int &get_RCAC_rate_ii() { return ii_AC_R; } + + /** + * Get the + * @see RCAC_Aq_ON + * @return Get RCAC rate controller switch + */ + const bool &get_RCAC_rate_switch() {return RCAC_Aw_ON;} + + /** + * Get the + * @see alpha_PID_rate + * @return Get the gain that multiplies the rate PID gains. + */ + const float &get_alpha_PID_rate() {return alpha_PID_rate;} + + + /** + * Set the RCAC Rate switch. + * @see _thr_int + */ + void set_RCAC_rate_switch(float switch_RCAC) + { + RCAC_Aw_ON = 1; + if (switch_RCAC<0.0f) { + RCAC_Aw_ON = 0; + } + } + + /** + * Set the PID scaling factor. + * @see _thr_int + */ + void set_PID_rate_factor(float PID_factor, float PID_val) + { + //alpha_PID = 1; + alpha_PID_rate = 1.0f; + if (PID_factor<0.0f) { + //alpha_PID = 0.5; + alpha_PID_rate = PID_val; + } + } + + /** + * Get the + * @see P_rate_x + * @return RCAC P(1,1) of the Rate controller + */ + const float &get_RCAC_P11_Ratex() { return P_rate_x(0,0); } + + /** + * Set RCAC variables. + * @see all RCAC variables + */ + void init_RCAC_rate() + { + P_rate_x.setZero(); //= eye() * 0.00010; + P_rate_y.setZero(); //= eye() * 0.00010; + P_rate_z.setZero(); //= eye() * 0.00010; + for (int i = 0; i <= 3; i++) { + P_rate_x(i,i) = 0.001f; + P_rate_y(i,i) = 0.001f; + P_rate_z(i,i) = 0.001f; + P_rate_x(i,i) = rcac_rate_P0; + P_rate_y(i,i) = rcac_rate_P0; + P_rate_z(i,i) = rcac_rate_P0; + } + for (int i = 0; i <= 2; i++) { + N1_rate(i) = 1; + } + phi_k_rate_x.setZero(); + phi_k_rate_y.setZero(); + phi_k_rate_z.setZero(); + phi_km1_rate_x.setZero(); + phi_km1_rate_y.setZero(); + phi_km1_rate_z.setZero(); + theta_k_rate_x.setZero(); + theta_k_rate_y.setZero(); + theta_k_rate_z.setZero(); + u_k_rate.setZero(); + z_k_rate.setZero(); + u_km1_rate.setZero(); + z_km1_rate.setZero(); + } + + /** + * Set the RCAC Rate Controller P0. + * @see rcac_rate_P0 + */ + void set_RCAC_rate_P0(float rate_P0) + { + rcac_rate_P0 = rate_P0; + } + private: void updateIntegral(matrix::Vector3f &rate_error, const float dt); @@ -115,4 +290,25 @@ class RateControl bool _mixer_saturation_positive[3] {}; bool _mixer_saturation_negative[3] {}; + + int ii_AC_R = 0; + bool RCAC_Aw_ON=1; + // matrix::SquareMatrix P_AC_R; + // matrix::Matrix phi_k_AC_R, phi_km1_AC_R; + // matrix::Matrix theta_k_AC_R,theta_k_Ac_PID; + // matrix::Matrix z_k_AC_R, z_km1_AC_R,u_k_AC_R, u_km1_AC_R; + // matrix::SquareMatrix Gamma_AC_R, I3, N1_Aw; + + matrix::SquareMatrix P_rate_x,P_rate_y,P_rate_z; + matrix::Matrix phi_k_rate_x, phi_km1_rate_x; + matrix::Matrix phi_k_rate_y, phi_km1_rate_y; + matrix::Matrix phi_k_rate_z, phi_km1_rate_z; + matrix::Matrix theta_k_rate_x, theta_k_rate_y, theta_k_rate_z; + matrix::Vector3f z_k_rate, z_km1_rate, u_k_rate, u_km1_rate; + matrix::Vector3f N1_rate, Gamma_rate; + matrix::Matrix dummy1,dummy2,dummy3; + + //float alpha_PID = 1.0f; + float alpha_PID_rate = 1.0f; + float rcac_rate_P0 = 0.0011f; }; diff --git a/src/modules/mc_rate_control/mc_rate_control_params.c b/src/modules/mc_rate_control/mc_rate_control_params.c index df6f98e1cdb6..9d955aa89b67 100644 --- a/src/modules/mc_rate_control/mc_rate_control_params.c +++ b/src/modules/mc_rate_control/mc_rate_control_params.c @@ -395,3 +395,35 @@ PARAM_DEFINE_FLOAT(MC_ACRO_SUPEXPOY, 0.7f); * @group Multicopter Rate Control */ PARAM_DEFINE_INT32(MC_BAT_SCALE_EN, 0); + +/** + * RCAC Rate switches. + * + * Specifies RCAC state. + * + * @min -10 + * @max 10 + * @group Multicopter Attitude Control + */ +PARAM_DEFINE_FLOAT(MPC_RCAC_RATE_SW, -1.0f); + +/** + * PID Rate gain. + * + * Specifies gain applied to the PID rate gains. + * + * @min -10 + * @max 10 + * @group Multicopter RCAC Control + */ +PARAM_DEFINE_FLOAT(MPC_RATE_ALPHA, 1.0f); + +/** + * P0 for the Rate controller + * + * @min 0.0 + * @max 10.0 + * @decimal 8 + * @group Multicopter Attitude Control + */ +PARAM_DEFINE_FLOAT(MPC_RCAC_RATE_P0, 0.001f); From 806472d85ee683080075334551f89a841095dc8a Mon Sep 17 00:00:00 2001 From: Juan Paredes Date: Fri, 19 Feb 2021 01:08:58 -0500 Subject: [PATCH 5/9] Solved RCAC issues... --- ROMFS/px4fmu_common/init.d-posix/rcS | 13 ++++- src/modules/mavlink/mavlink_main.cpp | 3 + src/modules/mavlink/mavlink_messages.cpp | 57 +++++++++++-------- .../mc_att_control/mc_att_control_main.cpp | 4 +- .../mc_att_control/mc_att_control_params.c | 2 +- .../MulticopterPositionControl.cpp | 4 +- .../MulticopterRateControl.cpp | 4 +- .../mc_rate_control/mc_rate_control_params.c | 2 +- 8 files changed, 53 insertions(+), 36 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d-posix/rcS b/ROMFS/px4fmu_common/init.d-posix/rcS index e573050d7163..d74925497ca1 100644 --- a/ROMFS/px4fmu_common/init.d-posix/rcS +++ b/ROMFS/px4fmu_common/init.d-posix/rcS @@ -242,6 +242,9 @@ if [ -e etc/init.d-posix/rc.mavlink_override ] then echo "Running non-default mavlink config rc.mavlink_override" sh etc/init.d-posix/rc.mavlink_override + #mavlink stream -r 2 -s RCAC_ATT_VARIABLES -u $udp_gcs_port_local + #mavlink stream -r 2 -s RCAC_RATE_VARIABLES -u $udp_gcs_port_local + #mavlink stream -r 2 -s RCAC_POS_VEL_VARIABLES -u $udp_gcs_port_local else # GCS link mavlink start -x -u $udp_gcs_port_local -r 4000000 @@ -254,9 +257,9 @@ else mavlink stream -r 50 -s SERVO_OUTPUT_RAW_0 -u $udp_gcs_port_local mavlink stream -r 20 -s RC_CHANNELS -u $udp_gcs_port_local mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u $udp_gcs_port_local - mavlink stream -r 2 -s RCAC_ATT_VARIABLES -u $udp_gcs_port_local - mavlink stream -r 2 -s RCAC_RATE_VARIABLES -u $udp_gcs_port_local - mavlink stream -r 2 -s RCAC_POS_VEL_VARIABLES -u $udp_gcs_port_local + #mavlink stream -r 2 -s RCAC_ATT_VARIABLES -u $udp_gcs_port_local + #mavlink stream -r 2 -s RCAC_RATE_VARIABLES -u $udp_gcs_port_local + #mavlink stream -r 2 -s RCAC_POS_VEL_VARIABLES -u $udp_gcs_port_local @@ -282,3 +285,7 @@ fi mavlink boot_complete replay trystart + +mavlink stream -r 2 -s RCAC_ATT_VARIABLES -u $udp_gcs_port_local +mavlink stream -r 2 -s RCAC_RATE_VARIABLES -u $udp_gcs_port_local +mavlink stream -r 2 -s RCAC_POS_VEL_VARIABLES -u $udp_gcs_port_local diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 228dea8d10a9..577ec7314f23 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1840,6 +1840,9 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("RC_CHANNELS", 0.5f); configure_stream_local("SYS_STATUS", 0.1f); configure_stream_local("VFR_HUD", 1.0f); + configure_stream_local("RCAC_ATT_VARIABLES", 2.0f); + configure_stream_local("RCAC_RATE_VARIABLES", 2.0f); + configure_stream_local("RCAC_POS_VEL_VARIABLES", 2.0f); #if !defined(CONSTRAINED_FLASH) configure_stream_local("LINK_NODE_STATUS", 1.0f); diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index c8521ffacb1d..c0f8a6aa10a1 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -3019,7 +3019,7 @@ class MavlinkStreamCameraCapture : public MavlinkStream class MavlinkStreamRcacAttVariables : public MavlinkStream { public: - const char *get_name() const override + const char *get_name() const { return MavlinkStreamRcacAttVariables::get_name_static(); } @@ -3031,7 +3031,7 @@ class MavlinkStreamRcacAttVariables : public MavlinkStream { return MAVLINK_MSG_ID_RCAC_ATT_VARIABLES; } - uint16_t get_id() override + uint16_t get_id() { return get_id_static(); } @@ -3039,7 +3039,7 @@ class MavlinkStreamRcacAttVariables : public MavlinkStream { return new MavlinkStreamRcacAttVariables(mavlink); } - unsigned get_size() override + unsigned get_size() { return MAVLINK_MSG_ID_RCAC_ATT_VARIABLES_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } @@ -3051,8 +3051,8 @@ class MavlinkStreamRcacAttVariables : public MavlinkStream uORB::Subscription _rcac_att_sub{ORB_ID(rcac_att_variables)}; /* do not allow top copying this class */ - MavlinkStreamRcacAttVariables(MavlinkStreamRcacAttVariables &) = delete; - MavlinkStreamRcacAttVariables& operator = (const MavlinkStreamRcacAttVariables &) = delete; + MavlinkStreamRcacAttVariables(MavlinkStreamRcacAttVariables &); + MavlinkStreamRcacAttVariables& operator = (const MavlinkStreamRcacAttVariables &); protected: /*explicit MavlinkStreamRcacAttVariables(Mavlink *mavlink) : MavlinkStream(mavlink), @@ -3066,10 +3066,11 @@ class MavlinkStreamRcacAttVariables : public MavlinkStream bool send() override { //struct rcac_att_variables_struct_s _rcac_att_variables; - rcac_att_variables_s _rcac_att_variables; + struct rcac_att_variables_s _rcac_att_variables; + //rcac_att_variables_s _rcac_att_variables; if (_rcac_att_sub.update(&_rcac_att_variables)) { - mavlink_rcac_att_variables_t _msg_rcac_att_variables {}; + mavlink_rcac_att_variables_t _msg_rcac_att_variables; _msg_rcac_att_variables.timestamp = _rcac_att_variables.timestamp; _msg_rcac_att_variables.switch_att = _rcac_att_variables.switch_att; @@ -3080,16 +3081,18 @@ class MavlinkStreamRcacAttVariables : public MavlinkStream _msg_rcac_att_variables.rcac_att_theta[2] = _rcac_att_variables.rcac_att_theta[2]; mavlink_msg_rcac_att_variables_send_struct(_mavlink->get_channel(), &_msg_rcac_att_variables); + + return true; } - return true; + return false; } }; /******************************************/ class MavlinkStreamRcacRateVariables : public MavlinkStream { public: - const char *get_name() const override + const char *get_name() const { return MavlinkStreamRcacRateVariables::get_name_static(); } @@ -3101,7 +3104,7 @@ class MavlinkStreamRcacRateVariables : public MavlinkStream { return MAVLINK_MSG_ID_RCAC_RATE_VARIABLES; } - uint16_t get_id() override + uint16_t get_id() { return get_id_static(); } @@ -3109,7 +3112,7 @@ class MavlinkStreamRcacRateVariables : public MavlinkStream { return new MavlinkStreamRcacRateVariables(mavlink); } - unsigned get_size() override + unsigned get_size() { return MAVLINK_MSG_ID_RCAC_RATE_VARIABLES_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } @@ -3121,8 +3124,8 @@ class MavlinkStreamRcacRateVariables : public MavlinkStream uORB::Subscription _rcac_rate_sub{ORB_ID(rcac_rate_variables)}; /* do not allow top copying this class */ - MavlinkStreamRcacRateVariables(MavlinkStreamRcacRateVariables &) = delete; - MavlinkStreamRcacRateVariables& operator = (const MavlinkStreamRcacRateVariables &) = delete; + MavlinkStreamRcacRateVariables(MavlinkStreamRcacRateVariables &); + MavlinkStreamRcacRateVariables& operator = (const MavlinkStreamRcacRateVariables &); protected: /*explicit MavlinkStreamRcacRateVariables(Mavlink *mavlink) : MavlinkStream(mavlink), @@ -3136,10 +3139,11 @@ class MavlinkStreamRcacRateVariables : public MavlinkStream bool send() override { //struct rcac_rate_variables_struct_s _rcac_rate_variables; - rcac_rate_variables_s _rcac_rate_variables; + struct rcac_rate_variables_s _rcac_rate_variables; + //rcac_rate_variables_s _rcac_rate_variables; if (_rcac_rate_sub.update(&_rcac_rate_variables)) { - mavlink_rcac_rate_variables_t _msg_rcac_rate_variables {}; + mavlink_rcac_rate_variables_t _msg_rcac_rate_variables; _msg_rcac_rate_variables.timestamp = _rcac_rate_variables.timestamp; _msg_rcac_rate_variables.switch_rate = _rcac_rate_variables.switch_rate; @@ -3159,16 +3163,18 @@ class MavlinkStreamRcacRateVariables : public MavlinkStream _msg_rcac_rate_variables.rcac_rate_theta[11] = _rcac_rate_variables.rcac_rate_theta[11]; mavlink_msg_rcac_rate_variables_send_struct(_mavlink->get_channel(), &_msg_rcac_rate_variables); + + return true; } - return true; + return false; } }; /******************************************/ class MavlinkStreamRcacPosVelVariables : public MavlinkStream { public: - const char *get_name() const override + const char *get_name() const { return MavlinkStreamRcacPosVelVariables::get_name_static(); } @@ -3180,7 +3186,7 @@ class MavlinkStreamRcacPosVelVariables : public MavlinkStream { return MAVLINK_MSG_ID_RCAC_POS_VEL_VARIABLES; } - uint16_t get_id() override + uint16_t get_id() { return get_id_static(); } @@ -3188,7 +3194,7 @@ class MavlinkStreamRcacPosVelVariables : public MavlinkStream { return new MavlinkStreamRcacPosVelVariables(mavlink); } - unsigned get_size() override + unsigned get_size() { return MAVLINK_MSG_ID_RCAC_POS_VEL_VARIABLES_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } @@ -3199,8 +3205,8 @@ class MavlinkStreamRcacPosVelVariables : public MavlinkStream uORB::Subscription _rcac_pos_vel_sub{ORB_ID(rcac_pos_vel_variables)}; /* do not allow top copying this class */ - MavlinkStreamRcacPosVelVariables(MavlinkStreamRcacPosVelVariables &) = delete; - MavlinkStreamRcacPosVelVariables& operator = (const MavlinkStreamRcacPosVelVariables &) = delete; + MavlinkStreamRcacPosVelVariables(MavlinkStreamRcacPosVelVariables &); + MavlinkStreamRcacPosVelVariables& operator = (const MavlinkStreamRcacPosVelVariables &); protected: /*explicit MavlinkStreamRcacPosVelVariables(Mavlink *mavlink) : MavlinkStream(mavlink), @@ -3214,11 +3220,12 @@ class MavlinkStreamRcacPosVelVariables : public MavlinkStream bool send() override { //struct rcac_pos_vel_variables_struct_s _rcac_pos_vel_variables; - rcac_pos_vel_variables_s _rcac_pos_vel_variables; + struct rcac_pos_vel_variables_s _rcac_pos_vel_variables; + //rcac_pos_vel_variables_s _rcac_pos_vel_variables; /*if (_sub->update(&_rcac_pvv_time, &_rcac_pos_vel_variables)) {*/ if (_rcac_pos_vel_sub.update(&_rcac_pos_vel_variables)) { - mavlink_rcac_pos_vel_variables_t _msg_rcac_pos_vel_variables{}; + mavlink_rcac_pos_vel_variables_t _msg_rcac_pos_vel_variables; _msg_rcac_pos_vel_variables.timestamp = _rcac_pos_vel_variables.timestamp; _msg_rcac_pos_vel_variables.pid_factor = _rcac_pos_vel_variables.pid_factor; @@ -3241,11 +3248,11 @@ class MavlinkStreamRcacPosVelVariables : public MavlinkStream _msg_rcac_pos_vel_variables.rcac_vel_theta[6] = _rcac_pos_vel_variables.rcac_vel_theta[6]; _msg_rcac_pos_vel_variables.rcac_vel_theta[7] = _rcac_pos_vel_variables.rcac_vel_theta[7]; _msg_rcac_pos_vel_variables.rcac_vel_theta[8] = _rcac_pos_vel_variables.rcac_vel_theta[8]; - mavlink_msg_rcac_pos_vel_variables_send_struct(_mavlink->get_channel(), &_msg_rcac_pos_vel_variables); + return true; } - return true; + return false; } }; /******************************************/ 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 e86e5f42900e..79ae5dbb3e10 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -296,8 +296,8 @@ MulticopterAttitudeControl::Run() float RCAC_switch = _rc_channels_switch.channels[14]; float PID_scale_f = _rc_channels_switch.channels[13]; // SITL - //RCAC_switch = -1.0f; - //PID_scale_f = 1.0f; + RCAC_switch = 1.0f; + PID_scale_f = 1.0f; if (RCAC_switch>0.0f) { _attitude_control.set_RCAC_att_switch(_param_mpc_rcac_att_sw.get()); diff --git a/src/modules/mc_att_control/mc_att_control_params.c b/src/modules/mc_att_control/mc_att_control_params.c index 6e6e7696d888..f86af5fe6371 100644 --- a/src/modules/mc_att_control/mc_att_control_params.c +++ b/src/modules/mc_att_control/mc_att_control_params.c @@ -185,7 +185,7 @@ PARAM_DEFINE_FLOAT(MC_MAN_TILT_TAU, 0.0f); * @max 10 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MPC_RCAC_ATT_SW, -1.0f); +PARAM_DEFINE_FLOAT(MPC_RCAC_ATT_SW, 1.0f); /** * PID Attitude gain. diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.cpp b/src/modules/mc_pos_control/MulticopterPositionControl.cpp index aebe55e3e7fc..8f341cff3bf8 100644 --- a/src/modules/mc_pos_control/MulticopterPositionControl.cpp +++ b/src/modules/mc_pos_control/MulticopterPositionControl.cpp @@ -269,7 +269,7 @@ void MulticopterPositionControl::Run() float RCAC_switch = _rc_channels_switch.channels[14]; // SITL 1 - //RCAC_switch = -1.0f; + RCAC_switch = 1.0f; if (RCAC_switch>0.0f) { _control.set_RCAC_pos_switch(_param_mpc_rcac_pos_sw.get()); @@ -283,7 +283,7 @@ void MulticopterPositionControl::Run() } float PID_scale_f = _rc_channels_switch.channels[13]; // SITL 2 - //PID_scale_f = 1.0f; + PID_scale_f = 1.0f; _control.set_PID_pv_factor(PID_scale_f, _param_mpc_pos_alpha.get(), _param_mpc_vel_alpha.get()); parameters_update(false); diff --git a/src/modules/mc_rate_control/MulticopterRateControl.cpp b/src/modules/mc_rate_control/MulticopterRateControl.cpp index f8c69dd27642..01d60c07d5c1 100644 --- a/src/modules/mc_rate_control/MulticopterRateControl.cpp +++ b/src/modules/mc_rate_control/MulticopterRateControl.cpp @@ -174,8 +174,8 @@ MulticopterRateControl::Run() float RCAC_switch = _rc_channels_switch.channels[14]; float PID_scale_f = _rc_channels_switch.channels[13]; // SITL - //RCAC_switch = -1.0f; - //PID_scale_f = 1.0f; + RCAC_switch = -1.0f; + PID_scale_f = 1.0f; if (RCAC_switch>0.0f) { _rate_control.set_RCAC_rate_switch(_param_mpc_rcac_rate_sw.get()); diff --git a/src/modules/mc_rate_control/mc_rate_control_params.c b/src/modules/mc_rate_control/mc_rate_control_params.c index 9d955aa89b67..ddccf2f482ff 100644 --- a/src/modules/mc_rate_control/mc_rate_control_params.c +++ b/src/modules/mc_rate_control/mc_rate_control_params.c @@ -405,7 +405,7 @@ PARAM_DEFINE_INT32(MC_BAT_SCALE_EN, 0); * @max 10 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MPC_RCAC_RATE_SW, -1.0f); +PARAM_DEFINE_FLOAT(MPC_RCAC_RATE_SW, 1.0f); /** * PID Rate gain. From 02ad0d95cd2f7a11c63f717db71b222c919dd680 Mon Sep 17 00:00:00 2001 From: Juan Paredes Date: Mon, 17 May 2021 13:40:22 -0400 Subject: [PATCH 6/9] Added repo update script and SITL scripts. --- repo_update.sh | 4 ++++ run_gazebo.sh | 4 ++++ run_jmavsim.sh | 4 ++++ 3 files changed, 12 insertions(+) create mode 100755 repo_update.sh create mode 100755 run_gazebo.sh create mode 100755 run_jmavsim.sh diff --git a/repo_update.sh b/repo_update.sh new file mode 100755 index 000000000000..be33a81b7619 --- /dev/null +++ b/repo_update.sh @@ -0,0 +1,4 @@ +make distclean +git submodule sync +git submodule update --init --recursive +git submodule update --remote --recursive mavlink diff --git a/run_gazebo.sh b/run_gazebo.sh new file mode 100755 index 000000000000..f654827edc38 --- /dev/null +++ b/run_gazebo.sh @@ -0,0 +1,4 @@ +export PX4_HOME_LAT=42.29454 +export PX4_HOME_LON=-83.710547 +export PX4_HOME_ALT=274.0 +make px4_sitl gazebo diff --git a/run_jmavsim.sh b/run_jmavsim.sh new file mode 100755 index 000000000000..ff54d53f2c06 --- /dev/null +++ b/run_jmavsim.sh @@ -0,0 +1,4 @@ +export PX4_HOME_LAT=42.29454 +export PX4_HOME_LON=-83.710547 +export PX4_HOME_ALT=274.0 +make px4_sitl_default jmavsim From a515c2fade6c7909128f73de4663be0c11f9b4de Mon Sep 17 00:00:00 2001 From: Juan Paredes Date: Mon, 17 May 2021 16:57:05 -0400 Subject: [PATCH 7/9] Solved rate controller SITL activation issue --- src/modules/mc_rate_control/MulticopterRateControl.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mc_rate_control/MulticopterRateControl.cpp b/src/modules/mc_rate_control/MulticopterRateControl.cpp index 01d60c07d5c1..08ccd400c1d0 100644 --- a/src/modules/mc_rate_control/MulticopterRateControl.cpp +++ b/src/modules/mc_rate_control/MulticopterRateControl.cpp @@ -174,7 +174,7 @@ MulticopterRateControl::Run() float RCAC_switch = _rc_channels_switch.channels[14]; float PID_scale_f = _rc_channels_switch.channels[13]; // SITL - RCAC_switch = -1.0f; + RCAC_switch = 1.0f; PID_scale_f = 1.0f; if (RCAC_switch>0.0f) { From 25a200a0e034350661b551b2821908ffce0d1f36 Mon Sep 17 00:00:00 2001 From: John Spencer Date: Wed, 19 May 2021 12:16:45 -0400 Subject: [PATCH 8/9] Message(commit on 'add_RCAC') --- .vscode/c_cpp_properties.json | 5 +- src/lib/mathlib/CMakeLists.txt | 4 + src/lib/mathlib/math/RCAC.cpp | 132 ++++++++++++++++++ src/lib/mathlib/math/RCAC.h | 72 ++++++++++ .../PositionControl/PositionControl.cpp | 14 +- .../PositionControl/PositionControl.hpp | 1 + 6 files changed, 225 insertions(+), 3 deletions(-) create mode 100644 src/lib/mathlib/math/RCAC.cpp create mode 100644 src/lib/mathlib/math/RCAC.h diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json index 9292c68182b4..05f023ad58f3 100644 --- a/.vscode/c_cpp_properties.json +++ b/.vscode/c_cpp_properties.json @@ -3,7 +3,10 @@ { "name": "PX4", "includePath": [ - "${workspaceFolder}/**" + "${workspaceFolder}/**", + "${workspaceFolder}/src/lib", + "${workspaceFolder}/src", + "${workspaceFolder}/src/lib" ], "defines": [], "macFrameworkPath": [], diff --git a/src/lib/mathlib/CMakeLists.txt b/src/lib/mathlib/CMakeLists.txt index 629a6a5cca59..b0bf801584b7 100644 --- a/src/lib/mathlib/CMakeLists.txt +++ b/src/lib/mathlib/CMakeLists.txt @@ -35,8 +35,12 @@ px4_add_library(mathlib math/test/test.cpp math/filter/LowPassFilter2p.cpp math/filter/LowPassFilter2pVector3f.cpp + math/RCAC.cpp + math/RCAC.h ) px4_add_unit_gtest(SRC math/filter/MedianFilterTest.cpp) px4_add_unit_gtest(SRC math/filter/NotchFilterTest.cpp) px4_add_unit_gtest(SRC math/FunctionsTest.cpp) +px4_add_unit_gtest(math/RCAC.cpp) +px4_add_unit_gtest(math/RCAC.h) diff --git a/src/lib/mathlib/math/RCAC.cpp b/src/lib/mathlib/math/RCAC.cpp new file mode 100644 index 000000000000..eccf8a56498e --- /dev/null +++ b/src/lib/mathlib/math/RCAC.cpp @@ -0,0 +1,132 @@ +#include "RCAC.h" +#include +//#include +#include + +// using namespace std; + +RCAC::RCAC(float P0_val, float lambda_val, float N_nf_val){ + + P0 = P0_val; + lambda = lambda_val; + N_nf = N_nf_val; + + // Initialize interal RCAC v ariables + P = eye() * P0; + theta.setZero(); + filtNu.setZero(); + filtNu(0,nf-1)=N_nf; + u_km1 = 0; + z_km1 = 0; + + Phi_k.setZero(); + + ubar.setZero(); + Phibar.setZero(); + + Phi_filt.setZero(); + + one_matrix = eye(); +} + +void RCAC::init_test(){ + std::cout << "init success!" << std::endl; +} + +void RCAC::set_RCAC_data(float z_km1_val, float u_km1_val) +{ + z_km1 = z_km1_val; + u_km1 = u_km1_val; +} + +void RCAC::buildRegressor(float z, float z_int, float z_diff) +{ + Phi_k(0, 0) = z; + Phi_k(0, 1) = z_int; + Phi_k(0, 2) = z_diff; +} + +void RCAC::filter_data() +{ + for (int ii = nf - 1; ii > 0; ii--) + { + ubar(ii, 0) = ubar(ii - 1, 0); + } + ubar(0, 0) = u_km1; + + for (int ii = nf; ii > 0; ii--) + { + for (int jj = 0; jj < 3; jj++) + { + Phibar(ii, jj) = Phibar(ii - 1, jj); + } + } + for (int jj = 0; jj < 3; jj++) + { + Phibar(0, jj) = Phi_k(0, jj); + } + + z_filt = z_km1; + + dummy = filtNu * ubar; + u_filt = dummy(0, 0); + + for (int ii = 0; ii < nf; ii++) + { + for (int jj = 0; jj < 3; jj++) + { + PhibarBlock(ii, jj) = Phibar(ii + 1, jj); + } + } + Phi_filt = filtNu * PhibarBlock; +} + +void RCAC::update_theta() +{ + if (kk > 3) + { + dummy = Phi_filt * P * Phi_filt.transpose(); + Gamma = lambda + dummy(0, 0); + P = P - P * Phi_filt.transpose() * 1 / Gamma * Phi_filt * P; + P = P / lambda; + + theta = theta - P * Phi_filt.transpose() * (z_filt * one_matrix + Phi_filt * theta - u_filt); + } + std::cout << kk << "\t" << z_filt << "\t" << u_filt << "\t" << Phi_filt(0 ,0) << "\t" << theta(0 ,0) << std::endl; +} + +float RCAC::compute_uk(float z, float z_int, float z_diff, float u) +{ + set_RCAC_data(z, u); + buildRegressor(z, z_int, z_diff); + filter_data(); + update_theta(); + dummy = Phi_k * theta; + u_k = dummy(0, 0); + kk = kk + 1; + return u_k; +} +/* +void RCAC::populate_states(int index) +{ + _rcac_pos_vel_states.timestamp = hrt_absolute_time(); + _rcac_pos_vel_states.id_xyz = index; + + //Add an if else statement depending on if position on velocity controller component + _rcac_pos_vel_states.u_pos[index] = get_rcac_uk(); + _rcac_pos_vel_states.theta_pos[index] = get_rcac_theta(); + _rcac_pos_vel_states.z_pos[index] = u_km1; + //_rcac_pos_vel_states_pub.publish(_rcac_pos_vel_states); + std::cout << "publisher values "<< _rcac_pos_vel_states.u << std::endl; +} + +void RCAC::publish_states(){ + _rcac_pos_vel_states_pub.publish(_rcac_pos_vel_states); +} +*/ +// [ 0 0 0] + +//pos_mes +//vel_mes +// if (1) +// pos_mes diff --git a/src/lib/mathlib/math/RCAC.h b/src/lib/mathlib/math/RCAC.h new file mode 100644 index 000000000000..de1c99afcc8c --- /dev/null +++ b/src/lib/mathlib/math/RCAC.h @@ -0,0 +1,72 @@ +#pragma once + +#include +#include +//#include + +using namespace matrix; +// using namespace std; + +/** + * The parent RCAC class. This class handles all the low level computation of RCAC + * such as the filtering, coefficient updates, and keeping track of the regressors. + * + * The notation here follows the JGCD 2019 implementation + */ +class RCAC +{ + float P0; + float lambda; + float N_nf; +/* +private: + uORB::Publication _rcac_pos_vel_states_pub{ORB_ID(rcac_pos_vel_states)}; + rcac_pos_vel_states_s _rcac_pos_vel_states{}; + //uORB::Publication _rcac_pos_vel_variables_pub{ORB_ID(rcac_pos_vel_variables)}; +*/ + +public: + RCAC(float P0_val, float lambda_val, float N_nf_val); + ///void init_RCAC(float, float, float); + + int getkk() {return kk;}; + float get_rcac_uk() {return u_k;}; + float get_rcac_theta(int i) {return theta(i,0);} + float get_rcac_P(int i, int j){return P(i, j);}; + float get_rcac_Phi(int i) {return Phi_k(i,0);} + float get_rcac_ukm1() {return u_km1;}; + + void set_RCAC_data(float, float); + void buildRegressor(float zkm1, float zkm1_int, float zkm1_diff); + void filter_data(); + void update_theta(); + float compute_uk(float z, float z_int, float z_diff, float u); + void init_test(); + void publish_states(int index); + +protected: + const int nf = 2; + matrix::Matrix filtNu; // 1st order filter. Gf = filtNu(0) + filtNu(1)/q + // In most cases, filtNu(0) = 0 and filtNu(1) = +-1 + + //RCAC internal variables + matrix::Matrix P; + matrix::Matrix theta; // Kp = theta(0,0) + // Ki = theta(1,0) + // Kd = theta(2,0) + + float u_k, u_km1, u_filt; + float z_km1, z_filt; + matrix::Matrix Phi_k, Phi_filt; + + matrix::Matrix ubar; // Size nf by 1 + matrix::Matrix Phibar; // Size nf+1 by 1 + matrix::Matrix PhibarBlock; // Size nf by 1 + + float Gamma; + float Idty_lz; + matrix::Matrix one_matrix; + matrix::Matrix dummy; + + int kk = 0; +}; diff --git a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp index eadd95c2272a..a26f81695395 100644 --- a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp +++ b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp @@ -44,6 +44,7 @@ using namespace matrix; + void PositionControl::setVelocityGains(const Vector3f &P, const Vector3f &I, const Vector3f &D) { _gain_vel_p = P; @@ -127,12 +128,18 @@ bool PositionControl::update(const float dt) void PositionControl::_positionControl() { + RCAC _rcac_pos_x(10.0,1.0,1.0); + RCAC _rcac_pos_y(10.0,1.0,1.0); + RCAC _rcac_pos_z(10.0,1.0,1.0); + + _rcac_pos_x.compute_uk(z_k_Pr_R(1,0),z_k_Pr_R(1,0)-z_km1_Pr_R(1,0),z_k_Pr_R(1,0)+z_km1_Pr_R(1,0),u_km1_Pr_R(1,0)); + _rcac_pos_y.compute_uk(z_k_Pr_R(2,0),z_k_Pr_R(2,0)-z_km1_Pr_R(2,0),z_k_Pr_R(2,0)+z_km1_Pr_R(2,0),u_km1_Pr_R(2,0)); + _rcac_pos_z.compute_uk(z_k_Pr_R(3,0),z_k_Pr_R(3,0)-z_km1_Pr_R(3,0),z_k_Pr_R(3,0)+z_km1_Pr_R(3,0),u_km1_Pr_R(3,0)); + // P-position controller Vector3f vel_sp_position = (_pos_sp - _pos).emult(_gain_pos_p); - z_k_Pr_R = (_pos_sp - _pos); u_k_Pr_R.setZero(); - if (RCAC_Pr_ON) { ii_Pr_R += 1; @@ -543,3 +550,6 @@ void PositionControl::init_RCAC(float rcac_pos_p0, float rcac_vel_p0) { ii_Pr_R = 0; ii_Pv_R = 0; } + + + diff --git a/src/modules/mc_pos_control/PositionControl/PositionControl.hpp b/src/modules/mc_pos_control/PositionControl/PositionControl.hpp index a3f1ac24ab0a..5002adea0d87 100644 --- a/src/modules/mc_pos_control/PositionControl/PositionControl.hpp +++ b/src/modules/mc_pos_control/PositionControl/PositionControl.hpp @@ -43,6 +43,7 @@ #include #include #include +#include struct PositionControlStates { matrix::Vector3f position; From 33c0db5acd4664ac04caf0447b03e0a1d4fde2b7 Mon Sep 17 00:00:00 2001 From: John Spencer Date: Wed, 19 May 2021 15:28:08 -0400 Subject: [PATCH 9/9] Message(commit on 'add_RCAC') --- .vscode/c_cpp_properties.json | 5 +- src/lib/mathlib/CMakeLists.txt | 4 + src/lib/mathlib/math/RCAC.cpp | 131 ++++++++++++++++++ src/lib/mathlib/math/RCAC.h | 72 ++++++++++ .../PositionControl/PositionControl.cpp | 14 +- .../PositionControl/PositionControl.hpp | 1 + 6 files changed, 224 insertions(+), 3 deletions(-) create mode 100644 src/lib/mathlib/math/RCAC.cpp create mode 100644 src/lib/mathlib/math/RCAC.h diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json index 9292c68182b4..05f023ad58f3 100644 --- a/.vscode/c_cpp_properties.json +++ b/.vscode/c_cpp_properties.json @@ -3,7 +3,10 @@ { "name": "PX4", "includePath": [ - "${workspaceFolder}/**" + "${workspaceFolder}/**", + "${workspaceFolder}/src/lib", + "${workspaceFolder}/src", + "${workspaceFolder}/src/lib" ], "defines": [], "macFrameworkPath": [], diff --git a/src/lib/mathlib/CMakeLists.txt b/src/lib/mathlib/CMakeLists.txt index 629a6a5cca59..b0bf801584b7 100644 --- a/src/lib/mathlib/CMakeLists.txt +++ b/src/lib/mathlib/CMakeLists.txt @@ -35,8 +35,12 @@ px4_add_library(mathlib math/test/test.cpp math/filter/LowPassFilter2p.cpp math/filter/LowPassFilter2pVector3f.cpp + math/RCAC.cpp + math/RCAC.h ) px4_add_unit_gtest(SRC math/filter/MedianFilterTest.cpp) px4_add_unit_gtest(SRC math/filter/NotchFilterTest.cpp) px4_add_unit_gtest(SRC math/FunctionsTest.cpp) +px4_add_unit_gtest(math/RCAC.cpp) +px4_add_unit_gtest(math/RCAC.h) diff --git a/src/lib/mathlib/math/RCAC.cpp b/src/lib/mathlib/math/RCAC.cpp new file mode 100644 index 000000000000..576dd46a9f4f --- /dev/null +++ b/src/lib/mathlib/math/RCAC.cpp @@ -0,0 +1,131 @@ +#include "RCAC.h" +#include +#include + +// using namespace std; + +RCAC::RCAC(float P0_val, float lambda_val, float N_nf_val){ + + P0 = P0_val; + lambda = lambda_val; + N_nf = N_nf_val; + + // Initialize interal RCAC v ariables + P = eye() * P0; + theta.setZero(); + filtNu.setZero(); + filtNu(0,nf-1)=N_nf; + u_km1 = 0; + z_km1 = 0; + + Phi_k.setZero(); + + ubar.setZero(); + Phibar.setZero(); + + Phi_filt.setZero(); + + one_matrix = eye(); +} + +void RCAC::init_test(){ + std::cout << "init success!" << std::endl; +} + +void RCAC::set_RCAC_data(float z_km1_val, float u_km1_val) +{ + z_km1 = z_km1_val; + u_km1 = u_km1_val; +} + +void RCAC::buildRegressor(float z, float z_int, float z_diff) +{ + Phi_k(0, 0) = z; + Phi_k(0, 1) = z_int; + Phi_k(0, 2) = z_diff; +} + +void RCAC::filter_data() +{ + for (int ii = nf - 1; ii > 0; ii--) + { + ubar(ii, 0) = ubar(ii - 1, 0); + } + ubar(0, 0) = u_km1; + + for (int ii = nf; ii > 0; ii--) + { + for (int jj = 0; jj < 3; jj++) + { + Phibar(ii, jj) = Phibar(ii - 1, jj); + } + } + for (int jj = 0; jj < 3; jj++) + { + Phibar(0, jj) = Phi_k(0, jj); + } + + z_filt = z_km1; + + dummy = filtNu * ubar; + u_filt = dummy(0, 0); + + for (int ii = 0; ii < nf; ii++) + { + for (int jj = 0; jj < 3; jj++) + { + PhibarBlock(ii, jj) = Phibar(ii + 1, jj); + } + } + Phi_filt = filtNu * PhibarBlock; +} + +void RCAC::update_theta() +{ + if (kk > 3) + { + dummy = Phi_filt * P * Phi_filt.transpose(); + Gamma = lambda + dummy(0, 0); + P = P - P * Phi_filt.transpose() * 1 / Gamma * Phi_filt * P; + P = P / lambda; + + theta = theta - P * Phi_filt.transpose() * (z_filt * one_matrix + Phi_filt * theta - u_filt); + } + //std::cout << kk << "\t" << z_filt << "\t" << u_filt << "\t" << Phi_filt(0 ,0) << "\t" << theta(0 ,0) << std::endl; +} + +float RCAC::compute_uk(float z, float z_int, float z_diff, float u) +{ + set_RCAC_data(z, u); + buildRegressor(z, z_int, z_diff); + filter_data(); + update_theta(); + dummy = Phi_k * theta; + u_k = dummy(0, 0); + kk = kk + 1; + return u_k; +} +/* +void RCAC::populate_states(int index) +{ + _rcac_pos_vel_states.timestamp = hrt_absolute_time(); + _rcac_pos_vel_states.id_xyz = index; + + //Add an if else statement depending on if position on velocity controller component + _rcac_pos_vel_states.u_pos[index] = get_rcac_uk(); + _rcac_pos_vel_states.theta_pos[index] = get_rcac_theta(); + _rcac_pos_vel_states.z_pos[index] = u_km1; + //_rcac_pos_vel_states_pub.publish(_rcac_pos_vel_states); + std::cout << "publisher values "<< _rcac_pos_vel_states.u << std::endl; +} + +void RCAC::publish_states(){ + _rcac_pos_vel_states_pub.publish(_rcac_pos_vel_states); +} +*/ +// [ 0 0 0] + +//pos_mes +//vel_mes +// if (1) +// pos_mes diff --git a/src/lib/mathlib/math/RCAC.h b/src/lib/mathlib/math/RCAC.h new file mode 100644 index 000000000000..de1c99afcc8c --- /dev/null +++ b/src/lib/mathlib/math/RCAC.h @@ -0,0 +1,72 @@ +#pragma once + +#include +#include +//#include + +using namespace matrix; +// using namespace std; + +/** + * The parent RCAC class. This class handles all the low level computation of RCAC + * such as the filtering, coefficient updates, and keeping track of the regressors. + * + * The notation here follows the JGCD 2019 implementation + */ +class RCAC +{ + float P0; + float lambda; + float N_nf; +/* +private: + uORB::Publication _rcac_pos_vel_states_pub{ORB_ID(rcac_pos_vel_states)}; + rcac_pos_vel_states_s _rcac_pos_vel_states{}; + //uORB::Publication _rcac_pos_vel_variables_pub{ORB_ID(rcac_pos_vel_variables)}; +*/ + +public: + RCAC(float P0_val, float lambda_val, float N_nf_val); + ///void init_RCAC(float, float, float); + + int getkk() {return kk;}; + float get_rcac_uk() {return u_k;}; + float get_rcac_theta(int i) {return theta(i,0);} + float get_rcac_P(int i, int j){return P(i, j);}; + float get_rcac_Phi(int i) {return Phi_k(i,0);} + float get_rcac_ukm1() {return u_km1;}; + + void set_RCAC_data(float, float); + void buildRegressor(float zkm1, float zkm1_int, float zkm1_diff); + void filter_data(); + void update_theta(); + float compute_uk(float z, float z_int, float z_diff, float u); + void init_test(); + void publish_states(int index); + +protected: + const int nf = 2; + matrix::Matrix filtNu; // 1st order filter. Gf = filtNu(0) + filtNu(1)/q + // In most cases, filtNu(0) = 0 and filtNu(1) = +-1 + + //RCAC internal variables + matrix::Matrix P; + matrix::Matrix theta; // Kp = theta(0,0) + // Ki = theta(1,0) + // Kd = theta(2,0) + + float u_k, u_km1, u_filt; + float z_km1, z_filt; + matrix::Matrix Phi_k, Phi_filt; + + matrix::Matrix ubar; // Size nf by 1 + matrix::Matrix Phibar; // Size nf+1 by 1 + matrix::Matrix PhibarBlock; // Size nf by 1 + + float Gamma; + float Idty_lz; + matrix::Matrix one_matrix; + matrix::Matrix dummy; + + int kk = 0; +}; diff --git a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp index eadd95c2272a..a26f81695395 100644 --- a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp +++ b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp @@ -44,6 +44,7 @@ using namespace matrix; + void PositionControl::setVelocityGains(const Vector3f &P, const Vector3f &I, const Vector3f &D) { _gain_vel_p = P; @@ -127,12 +128,18 @@ bool PositionControl::update(const float dt) void PositionControl::_positionControl() { + RCAC _rcac_pos_x(10.0,1.0,1.0); + RCAC _rcac_pos_y(10.0,1.0,1.0); + RCAC _rcac_pos_z(10.0,1.0,1.0); + + _rcac_pos_x.compute_uk(z_k_Pr_R(1,0),z_k_Pr_R(1,0)-z_km1_Pr_R(1,0),z_k_Pr_R(1,0)+z_km1_Pr_R(1,0),u_km1_Pr_R(1,0)); + _rcac_pos_y.compute_uk(z_k_Pr_R(2,0),z_k_Pr_R(2,0)-z_km1_Pr_R(2,0),z_k_Pr_R(2,0)+z_km1_Pr_R(2,0),u_km1_Pr_R(2,0)); + _rcac_pos_z.compute_uk(z_k_Pr_R(3,0),z_k_Pr_R(3,0)-z_km1_Pr_R(3,0),z_k_Pr_R(3,0)+z_km1_Pr_R(3,0),u_km1_Pr_R(3,0)); + // P-position controller Vector3f vel_sp_position = (_pos_sp - _pos).emult(_gain_pos_p); - z_k_Pr_R = (_pos_sp - _pos); u_k_Pr_R.setZero(); - if (RCAC_Pr_ON) { ii_Pr_R += 1; @@ -543,3 +550,6 @@ void PositionControl::init_RCAC(float rcac_pos_p0, float rcac_vel_p0) { ii_Pr_R = 0; ii_Pv_R = 0; } + + + diff --git a/src/modules/mc_pos_control/PositionControl/PositionControl.hpp b/src/modules/mc_pos_control/PositionControl/PositionControl.hpp index a3f1ac24ab0a..5002adea0d87 100644 --- a/src/modules/mc_pos_control/PositionControl/PositionControl.hpp +++ b/src/modules/mc_pos_control/PositionControl/PositionControl.hpp @@ -43,6 +43,7 @@ #include #include #include +#include struct PositionControlStates { matrix::Vector3f position;