Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-20 03:13:44 +08:00
Code Issues Actions 21 Packages Projects Releases Wiki Activity

rate controller status include rates

Browse Source 
- the actual corrected rates currently used by mc_att_control are not
   logged
This commit is contained in:
Daniel Agar
2018-01-04 12:37:00 -05:00
parent 98441ac100
commit 2bea09a997
5 changed files with 27 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files
msg/rate_ctrl_status.msg
+10 -4
View File
@@ -1,5 +1,11 @@
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
# rates used by the controller
float32 rollspeed # Bias corrected angular velocity about X body axis in rad/s
float32 pitchspeed # Bias corrected angular velocity about Y body axis in rad/s
float32 yawspeed # Bias corrected angular velocity about Z body axis in rad/s
# rate controller integrator status
float32 rollspeed_integ
float32 pitchspeed_integ
float32 yawspeed_integ
float32 additional_integ1 # FW: wheel rate integrator (optional)
src/modules/fw_att_control/fw_att_control_main.cpp
+6 -3
View File
@@ -1179,9 +1179,12 @@ FixedwingAttitudeControl::task_main()
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.rollspeed = _att.rollspeed;
rate_ctrl_status.pitchspeed = _att.pitchspeed;
rate_ctrl_status.yawspeed = _att.yawspeed;
rate_ctrl_status.rollspeed_integ = _roll_ctrl.get_integrator();
rate_ctrl_status.pitchspeed_integ = _pitch_ctrl.get_integrator();
rate_ctrl_status.yawspeed_integ = _yaw_ctrl.get_integrator();
rate_ctrl_status.additional_integ1 = _wheel_ctrl.get_integrator();
int instance;
src/modules/logger/logger.cpp
+2 -1
View File
@@ -599,7 +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("rate_ctrl_status", 30);
add_topic("safety");
add_topic("sensor_combined", 100);
add_topic("sensor_preflight", 200);
@@ -629,6 +629,7 @@ void Logger::add_high_rate_topics()
add_topic("actuator_controls_0");
add_topic("actuator_outputs");
add_topic("manual_control_setpoint");
add_topic("rate_ctrl_status");
add_topic("sensor_combined");
add_topic("vehicle_attitude");
add_topic("vehicle_attitude_setpoint");
src/modules/mc_att_control/mc_att_control_main.cpp
+6 -3
View File
@@ -1257,9 +1257,12 @@ MulticopterAttitudeControl::task_main()
/* publish 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);
rate_ctrl_status.rollspeed = _rates_prev(0);
rate_ctrl_status.pitchspeed = _rates_prev(1);
rate_ctrl_status.yawspeed = _rates_prev(2);
rate_ctrl_status.rollspeed_integ = _rates_int(0);
rate_ctrl_status.pitchspeed_integ = _rates_int(1);
rate_ctrl_status.yawspeed_integ = _rates_int(2);
int instance;
orb_publish_auto(ORB_ID(rate_ctrl_status), &_controller_status_pub, &rate_ctrl_status, &instance, ORB_PRIO_DEFAULT);
src/modules/sdlog2/sdlog2.c
+3 -3
View File
@@ -2082,9 +2082,9 @@ int sdlog2_thread_main(int argc, char *argv[])
/* --- MULTIROTOR ATTITUDE CONTROLLER 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.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;
log_msg.body.log_MACS.roll_rate_integ = buf.rate_ctrl_status.rollspeed_integ;
log_msg.body.log_MACS.pitch_rate_integ = buf.rate_ctrl_status.pitchspeed_integ;
log_msg.body.log_MACS.yaw_rate_integ = buf.rate_ctrl_status.yawspeed_integ;
LOGBUFFER_WRITE_AND_COUNT(MACS);
}
}