diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 80af177914..4261040fa2 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -142,6 +142,7 @@ set(msg_files sensors_status_imu.msg soaring_controller_heartbeat.msg soaring_controller_position_setpoint.msg + soaring_controller_position.msg soaring_controller_status.msg system_power.msg takeoff_status.msg diff --git a/msg/soaring_controller_position.msg b/msg/soaring_controller_position.msg new file mode 100644 index 0000000000..33ab052e02 --- /dev/null +++ b/msg/soaring_controller_position.msg @@ -0,0 +1,7 @@ +# SOARING CONTROLLER POSITION IN ENU SOARING FRAME + +uint64 timestamp # time since system start (microseconds) + +float32[3] pos # POSITION VECTOR IN SOARING ENU FRAME +float32[3] vel # VELOCITY VECTOR IN SOARING ENU FRAME +float32[3] acc # ACCELERATION VECTOR IN SOARING ENU FRAME \ No newline at end of file diff --git a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp index f99672a68b..990a3f898e 100644 --- a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp +++ b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp @@ -576,8 +576,19 @@ FixedwingPositionINDIControl::Run() // Publish actuator controls only once in OFFBOARD if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_OFFBOARD) { + // ======================================== + // publish controller position in ENU frame + // ======================================== + _soaring_controller_position.timestamp = hrt_absolute_time(); + for (int i=0; i<3; i++){ + _soaring_controller_position.pos[i] = _pos(i); + _soaring_controller_position.vel[i] = _vel(i); + _soaring_controller_position.acc[i] = _acc(i); + } + _soaring_controller_position_pub.publish(_soaring_controller_position); + // ==================================== - // publish high level control variables + // publish controller position setpoint // ==================================== _soaring_controller_position_setpoint.timestamp = hrt_absolute_time(); for (int i=0; i<3; i++){ @@ -636,7 +647,7 @@ FixedwingPositionINDIControl::Run() if (_counter==100) { _counter = 0; - PX4_INFO("frequency: \t%.3f", (double)(1000000*100)/(hrt_absolute_time()-_last_time)); + //PX4_INFO("frequency: \t%.3f", (double)(1000000*100)/(hrt_absolute_time()-_last_time)); _last_time = hrt_absolute_time(); } else { @@ -997,7 +1008,19 @@ FixedwingPositionINDIControl::_compute_NDI_stage_1(Vector3f pos_ref, Vector3f ve Dcmf R_ref_true(R_ref.transpose()*R_ib); // get required rotation vector (in body frame) AxisAnglef q_err(R_ref_true); - Vector3f w_err = -q_err.angle()*q_err.axis(); + Vector3f w_err; + if (abs(q_err.angle())0.f){ + w_err = (2*M_PI_F-q_err.angle())*q_err.axis(); + } + else{ + w_err = (-2*M_PI_F-q_err.angle())*q_err.axis(); + } + } + // ========================================= // apply PD control law on the body attitude @@ -1024,6 +1047,9 @@ FixedwingPositionINDIControl::_compute_NDI_stage_1(Vector3f pos_ref, Vector3f ve //PX4_INFO("force command: \t%.2f\t%.2f\t%.2f", (double)f_command(0), (double)f_command(1), (double)f_command(2)); //PX4_INFO("FRD body frame rotation vec: \t%.2f\t%.2f\t%.2f", (double)w_err(0), (double)w_err(1), (double)w_err(2)); + if (sqrtf(w_err(0)*w_err(0) + w_err(1)*w_err(1) + w_err(2)*w_err(2))>M_PI_F){ + PX4_ERR("rotation angle larger than pi: \t%.2f", (double)sqrtf(w_err(0)*w_err(0) + w_err(1)*w_err(1) + w_err(2)*w_err(2))); + } return rot_acc_command; } diff --git a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp index 471626b6eb..fcdb7eb297 100644 --- a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp +++ b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp @@ -64,6 +64,7 @@ #include #include #include +#include #include #include #include @@ -130,6 +131,7 @@ private: uORB::PublicationMulti _rate_ctrl_status_pub{ORB_ID(rate_ctrl_status)}; uORB::Publication _soaring_controller_heartbeat_pub{ORB_ID(soaring_controller_status)}; uORB::Publication _soaring_controller_position_setpoint_pub{ORB_ID(soaring_controller_position_setpoint)}; + uORB::Publication _soaring_controller_position_pub{ORB_ID(soaring_controller_position)}; uORB::Publication _offboard_control_mode_pub{ORB_ID(offboard_control_mode)}; // Message structs @@ -150,6 +152,7 @@ private: soaring_controller_status_s _soaring_controller_status {}; ///< soaring controller status soaring_controller_heartbeat_s _soaring_controller_heartbeat{}; ///< soaring controller hrt soaring_controller_position_setpoint_s _soaring_controller_position_setpoint{}; ///< soaring controller pos setpoint + soaring_controller_position_s _soaring_controller_position{}; ///< soaring controller pos // parameter struct DEFINE_PARAMETERS( diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 79ec57cabd..725abd019b 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -122,6 +122,7 @@ void LoggedTopics::add_default_topics() add_topic("vehicle_thrust_setpoint", 20); add_topic("vehicle_torque_setpoint", 20); add_topic("vehicle_actuator_setpoint", 20); + add_topic("soaring_controller_position", 50); add_topic("soaring_controller_position_setpoint", 50); // multi topics