fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-05 01:21:35 +08:00
Code Issues 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-29 09:10:56 -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_s rate_ctrl_status;
rate_ctrl_status.timestamp = hrt_absolute_time(); rate_ctrl_status.timestamp = hrt_absolute_time();
rate_ctrl_status.roll_integ = _roll_ctrl.get_integrator(); rate_ctrl_status.rollspeed = _att.rollspeed;
rate_ctrl_status.pitch_integ = _pitch_ctrl.get_integrator(); rate_ctrl_status.pitchspeed = _att.pitchspeed;
rate_ctrl_status.yaw_integ = _yaw_ctrl.get_integrator(); 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(); rate_ctrl_status.additional_integ1 = _wheel_ctrl.get_integrator();
int instance; 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("mission_result");
add_topic("optical_flow", 50); add_topic("optical_flow", 50);
add_topic("position_setpoint_triplet", 200); add_topic("position_setpoint_triplet", 200);
add_topic("rate_ctrl_status", 200); add_topic("rate_ctrl_status", 30);
add_topic("safety"); add_topic("safety");
add_topic("sensor_combined", 100); add_topic("sensor_combined", 100);
add_topic("sensor_preflight", 200); add_topic("sensor_preflight", 200);
@@ -629,6 +629,7 @@ void Logger::add_high_rate_topics()
add_topic("actuator_controls_0"); add_topic("actuator_controls_0");
add_topic("actuator_outputs"); add_topic("actuator_outputs");
add_topic("manual_control_setpoint"); add_topic("manual_control_setpoint");
add_topic("rate_ctrl_status");
add_topic("sensor_combined"); add_topic("sensor_combined");
add_topic("vehicle_attitude"); add_topic("vehicle_attitude");
add_topic("vehicle_attitude_setpoint"); 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 */ /* publish controller status */
rate_ctrl_status_s rate_ctrl_status; rate_ctrl_status_s rate_ctrl_status;
rate_ctrl_status.timestamp = hrt_absolute_time(); rate_ctrl_status.timestamp = hrt_absolute_time();
rate_ctrl_status.roll_integ = _rates_int(0); rate_ctrl_status.rollspeed = _rates_prev(0);
rate_ctrl_status.pitch_integ = _rates_int(1); rate_ctrl_status.pitchspeed = _rates_prev(1);
rate_ctrl_status.yaw_integ = _rates_int(2); 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; int instance;
orb_publish_auto(ORB_ID(rate_ctrl_status), &_controller_status_pub, &rate_ctrl_status, &instance, ORB_PRIO_DEFAULT); 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 --- */ /* --- MULTIROTOR ATTITUDE CONTROLLER STATUS --- */
if (copy_if_updated(ORB_ID(rate_ctrl_status), &subs.rate_ctrl_status_sub, &buf.rate_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.msg_type = LOG_MACS_MSG;
log_msg.body.log_MACS.roll_rate_integ = buf.rate_ctrl_status.roll_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.pitch_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.yaw_integ; log_msg.body.log_MACS.yaw_rate_integ = buf.rate_ctrl_status.yawspeed_integ;
LOGBUFFER_WRITE_AND_COUNT(MACS); LOGBUFFER_WRITE_AND_COUNT(MACS);
} }
} }