From 98441ac1004c937eb9c065f710b004ba9e8378e5 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Sun, 26 Nov 2017 15:24:00 +0100 Subject: [PATCH] Log rate controller integrators (FW + MC) --- msg/CMakeLists.txt | 2 +- msg/mc_att_ctrl_status.msg | 4 -- msg/rate_ctrl_status.msg | 5 ++ .../fw_att_control/fw_att_control_main.cpp | 23 +++++--- src/modules/logger/logger.cpp | 1 + .../mc_att_control/mc_att_control_main.cpp | 53 ++++--------------- src/modules/sdlog2/sdlog2.c | 16 +++--- 7 files changed, 41 insertions(+), 63 deletions(-) delete mode 100644 msg/mc_att_ctrl_status.msg create mode 100644 msg/rate_ctrl_status.msg diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 1d5ab1fc5d..76bb57cd00 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -69,7 +69,6 @@ set(msg_files log_message.msg manual_control_setpoint.msg mavlink_log.msg - mc_att_ctrl_status.msg mission.msg mission_result.msg mount_orientation.msg @@ -82,6 +81,7 @@ set(msg_files power_button_state.msg pwm_input.msg qshell_req.msg + rate_ctrl_status.msg rc_channels.msg rc_parameter_map.msg safety.msg diff --git a/msg/mc_att_ctrl_status.msg b/msg/mc_att_ctrl_status.msg deleted file mode 100644 index c2979c3f00..0000000000 --- a/msg/mc_att_ctrl_status.msg +++ /dev/null @@ -1,4 +0,0 @@ - -float32 roll_rate_integ # roll rate inegrator -float32 pitch_rate_integ # pitch rate integrator -float32 yaw_rate_integ # yaw rate integrator diff --git a/msg/rate_ctrl_status.msg b/msg/rate_ctrl_status.msg new file mode 100644 index 0000000000..a3f033ec9e --- /dev/null +++ b/msg/rate_ctrl_status.msg @@ -0,0 +1,5 @@ +float32 roll_integ # roll rate integrator +float32 pitch_integ # pitch rate integrator +float32 yaw_integ # yaw rate integrator + +float32 additional_integ1 # FW: wheel rate integrator diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 35508f2f2a..17ce9eebbb 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -41,11 +41,6 @@ * */ -#include -#include -#include -#include - #include #include #include @@ -53,6 +48,10 @@ #include #include #include +#include +#include +#include +#include #include #include #include @@ -61,6 +60,7 @@ #include #include #include +#include #include #include #include @@ -68,7 +68,6 @@ #include #include #include -#include #include using matrix::Eulerf; @@ -130,6 +129,7 @@ private: orb_advert_t _attitude_sp_pub; /**< attitude setpoint point */ orb_advert_t _actuators_0_pub; /**< actuator control group 0 setpoint */ orb_advert_t _actuators_2_pub; /**< actuator control group 1 setpoint (Airframe) */ + orb_advert_t _rate_ctrl_status_pub; /**< rate controller status publication */ orb_id_t _rates_sp_id; // pointer to correct rates setpoint uORB metadata structure orb_id_t _actuators_id; // pointer to correct actuator controls0 uORB metadata structure @@ -367,6 +367,7 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _attitude_sp_pub(nullptr), _actuators_0_pub(nullptr), _actuators_2_pub(nullptr), + _rate_ctrl_status_pub(nullptr), _rates_sp_id(nullptr), _actuators_id(nullptr), @@ -1176,6 +1177,16 @@ FixedwingAttitudeControl::task_main() _rate_sp_pub = orb_advertise(_rates_sp_id, &_rates_sp); } + rate_ctrl_status_s rate_ctrl_status; + rate_ctrl_status.timestamp = hrt_absolute_time(); + rate_ctrl_status.roll_integ = _roll_ctrl.get_integrator(); + rate_ctrl_status.pitch_integ = _pitch_ctrl.get_integrator(); + rate_ctrl_status.yaw_integ = _yaw_ctrl.get_integrator(); + rate_ctrl_status.additional_integ1 = _wheel_ctrl.get_integrator(); + + int instance; + orb_publish_auto(ORB_ID(rate_ctrl_status), &_rate_ctrl_status_pub, &rate_ctrl_status, &instance, ORB_PRIO_DEFAULT); + } else { /* manual/direct control */ _actuators.control[actuator_controls_s::INDEX_ROLL] = _manual.y * _parameters.man_roll_scale + _parameters.trim_roll; diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp index d77b48e58a..03d79c00d9 100644 --- a/src/modules/logger/logger.cpp +++ b/src/modules/logger/logger.cpp @@ -599,6 +599,7 @@ void Logger::add_default_topics() add_topic("mission_result"); add_topic("optical_flow", 50); add_topic("position_setpoint_triplet", 200); + add_topic("rate_ctrl_status", 200); add_topic("safety"); add_topic("sensor_combined", 100); add_topic("sensor_preflight", 200); 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 4024dfd9c2..da330e0c3c 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -57,23 +57,21 @@ #include #include #include +#include #include #include #include #include #include #include -#include -#include #include #include -#include #include #include #include -#include #include #include +#include #include #include #include @@ -82,7 +80,6 @@ #include #include #include -#include /** * Multicopter attitude control app start / stop handling function @@ -159,7 +156,6 @@ private: struct vehicle_control_mode_s _v_control_mode; /**< vehicle control mode */ struct actuator_controls_s _actuators; /**< actuator controls */ struct vehicle_status_s _vehicle_status; /**< vehicle status */ - struct mc_att_ctrl_status_s _controller_status; /**< controller status */ struct battery_status_s _battery_status; /**< battery status */ struct sensor_gyro_s _sensor_gyro; /**< gyro data before thermal correctons and ekf bias estimates are applied */ struct sensor_correction_s _sensor_correction; /**< sensor thermal corrections */ @@ -364,7 +360,6 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _v_control_mode{}, _actuators{}, _vehicle_status{}, - _controller_status{}, _battery_status{}, _sensor_gyro{}, _sensor_correction{}, @@ -1247,11 +1242,6 @@ MulticopterAttitudeControl::task_main() } } - _controller_status.roll_rate_integ = _rates_int(0); - _controller_status.pitch_rate_integ = _rates_int(1); - _controller_status.yaw_rate_integ = _rates_int(2); - _controller_status.timestamp = hrt_absolute_time(); - if (!_actuators_0_circuit_breaker_enabled) { if (_actuators_0_pub != nullptr) { @@ -1265,12 +1255,14 @@ MulticopterAttitudeControl::task_main() } /* publish controller status */ - if (_controller_status_pub != nullptr) { - orb_publish(ORB_ID(mc_att_ctrl_status), _controller_status_pub, &_controller_status); + rate_ctrl_status_s rate_ctrl_status; + rate_ctrl_status.timestamp = hrt_absolute_time(); + rate_ctrl_status.roll_integ = _rates_int(0); + rate_ctrl_status.pitch_integ = _rates_int(1); + rate_ctrl_status.yaw_integ = _rates_int(2); - } else { - _controller_status_pub = orb_advertise(ORB_ID(mc_att_ctrl_status), &_controller_status); - } + int instance; + orb_publish_auto(ORB_ID(rate_ctrl_status), &_controller_status_pub, &rate_ctrl_status, &instance, ORB_PRIO_DEFAULT); } if (_v_control_mode.flag_control_termination_enabled) { @@ -1299,33 +1291,6 @@ MulticopterAttitudeControl::task_main() _actuators_0_pub = orb_advertise(_actuators_id, &_actuators); } } - - _controller_status.roll_rate_integ = _rates_int(0); - _controller_status.pitch_rate_integ = _rates_int(1); - _controller_status.yaw_rate_integ = _rates_int(2); - _controller_status.timestamp = hrt_absolute_time(); - - /* publish controller status */ - if (_controller_status_pub != nullptr) { - orb_publish(ORB_ID(mc_att_ctrl_status), _controller_status_pub, &_controller_status); - - } else { - _controller_status_pub = orb_advertise(ORB_ID(mc_att_ctrl_status), &_controller_status); - } - - /* publish attitude rates setpoint */ - _v_rates_sp.roll = _rates_sp(0); - _v_rates_sp.pitch = _rates_sp(1); - _v_rates_sp.yaw = _rates_sp(2); - _v_rates_sp.thrust = _thrust_sp; - _v_rates_sp.timestamp = hrt_absolute_time(); - - if (_v_rates_sp_pub != nullptr) { - orb_publish(_rates_sp_id, _v_rates_sp_pub, &_v_rates_sp); - - } else if (_rates_sp_id) { - _v_rates_sp_pub = orb_advertise(_rates_sp_id, &_v_rates_sp); - } } } } diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 83e5f9802f..b378f2df29 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -94,7 +94,7 @@ #include #include #include -#include +#include #include #include #include @@ -1186,7 +1186,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct wind_estimate_s wind_estimate; struct vtol_vehicle_status_s vtol_status; struct time_offset_s time_offset; - struct mc_att_ctrl_status_s mc_att_ctrl_status; + struct rate_ctrl_status_s rate_ctrl_status; struct ekf2_innovations_s innovations; struct camera_trigger_s camera_trigger; struct vehicle_land_detected_s land_detected; @@ -1291,7 +1291,7 @@ int sdlog2_thread_main(int argc, char *argv[]) int servorail_status_sub; int wind_sub; int tsync_sub; - int mc_att_ctrl_status_sub; + int rate_ctrl_status_sub; int innov_sub; int cam_trig_sub; int land_detected_sub; @@ -1333,7 +1333,7 @@ int sdlog2_thread_main(int argc, char *argv[]) subs.servorail_status_sub = -1; subs.wind_sub = -1; subs.tsync_sub = -1; - subs.mc_att_ctrl_status_sub = -1; + subs.rate_ctrl_status_sub = -1; subs.innov_sub = -1; subs.cam_trig_sub = -1; subs.land_detected_sub = -1; @@ -2080,11 +2080,11 @@ int sdlog2_thread_main(int argc, char *argv[]) } /* --- MULTIROTOR ATTITUDE CONTROLLER STATUS --- */ - if (copy_if_updated(ORB_ID(mc_att_ctrl_status), &subs.mc_att_ctrl_status_sub, &buf.mc_att_ctrl_status)) { + if (copy_if_updated(ORB_ID(rate_ctrl_status), &subs.rate_ctrl_status_sub, &buf.rate_ctrl_status)) { log_msg.msg_type = LOG_MACS_MSG; - log_msg.body.log_MACS.roll_rate_integ = buf.mc_att_ctrl_status.roll_rate_integ; - log_msg.body.log_MACS.pitch_rate_integ = buf.mc_att_ctrl_status.pitch_rate_integ; - log_msg.body.log_MACS.yaw_rate_integ = buf.mc_att_ctrl_status.yaw_rate_integ; + log_msg.body.log_MACS.roll_rate_integ = buf.rate_ctrl_status.roll_integ; + log_msg.body.log_MACS.pitch_rate_integ = buf.rate_ctrl_status.pitch_integ; + log_msg.body.log_MACS.yaw_rate_integ = buf.rate_ctrl_status.yaw_integ; LOGBUFFER_WRITE_AND_COUNT(MACS); } }