dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Log rate controller integrators (FW + MC)

This commit is contained in:
Andreas Antener 2017-11-26 15:24:00 +01:00 committed by Daniel Agar
parent 3504ce8efe
commit 98441ac100
7 changed files with 41 additions and 63 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
msg/CMakeLists.txt
View File

@ -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

4
msg/mc_att_ctrl_status.msg
View File

@ -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

5
msg/rate_ctrl_status.msg Normal file
View File

@ -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

23
src/modules/fw_att_control/fw_att_control_main.cpp
View File

@ -41,11 +41,6 @@
*
*/
#include <px4_config.h>
#include <px4_defines.h>
#include <px4_tasks.h>
#include <px4_posix.h>
#include <drivers/drv_hrt.h>
#include <ecl/attitude_fw/ecl_pitch_controller.h>
#include <ecl/attitude_fw/ecl_roll_controller.h>
@ -53,6 +48,10 @@
#include <ecl/attitude_fw/ecl_yaw_controller.h>
#include <geo/geo.h>
#include <mathlib/mathlib.h>
#include <px4_config.h>
#include <px4_defines.h>
#include <px4_posix.h>
#include <px4_tasks.h>
#include <systemlib/param/param.h>
#include <systemlib/perf_counter.h>
#include <uORB/Subscription.hpp>
@ -61,6 +60,7 @@
#include <uORB/topics/battery_status.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/rate_ctrl_status.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_control_mode.h>
@ -68,7 +68,6 @@
#include <uORB/topics/vehicle_land_detected.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/uORB.h>
#include <vtol_att_control/vtol_type.h>
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;

1
src/modules/logger/logger.cpp
View File

@ -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);

53
src/modules/mc_att_control/mc_att_control_main.cpp
View File

@ -57,23 +57,21 @@
#include <drivers/drv_hrt.h>
#include <lib/geo/geo.h>
#include <lib/mathlib/mathlib.h>
#include <lib/mixer/mixer.h>
#include <lib/tailsitter_recovery/tailsitter_recovery.h>
#include <px4_config.h>
#include <px4_defines.h>
#include <px4_posix.h>
#include <px4_tasks.h>
#include <systemlib/circuit_breaker.h>
#include <systemlib/err.h>
#include <lib/mixer/mixer.h>
#include <systemlib/param/param.h>
#include <systemlib/perf_counter.h>
#include <systemlib/systemlib.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/mc_att_ctrl_status.h>
#include <uORB/topics/multirotor_motor_limits.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/rate_ctrl_status.h>
#include <uORB/topics/sensor_bias.h>
#include <uORB/topics/sensor_correction.h>
#include <uORB/topics/sensor_gyro.h>
@ -82,7 +80,6 @@
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/uORB.h>
/**
* 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);
}
}
}
}

16
src/modules/sdlog2/sdlog2.c
View File

@ -94,7 +94,7 @@
#include <uORB/topics/wind_estimate.h>
#include <uORB/topics/vtol_vehicle_status.h>
#include <uORB/topics/time_offset.h>
#include <uORB/topics/mc_att_ctrl_status.h>
#include <uORB/topics/rate_ctrl_status.h>
#include <uORB/topics/ekf2_innovations.h>
#include <uORB/topics/camera_trigger.h>
#include <uORB/topics/vehicle_land_detected.h>
@ -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);
}
}