fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-21 11:23:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

Added ctrl debugging values

Browse Source
This commit is contained in:
Julian Oes
2013-06-16 12:59:50 +02:00
parent 562253c508
commit b52d561b11
5 changed files with 170 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/multirotor_att_control/multirotor_rate_control.c
+14 -2
View File
@@ -54,6 +54,9 @@
#include <systemlib/param/param.h>
#include <systemlib/err.h>
#include <drivers/drv_hrt.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_control_debug.h>
PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.0f); /* same on Flamewheel */
PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f);
@@ -175,6 +178,11 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
static bool initialized = false;
static struct vehicle_control_debug_s control_debug;
static orb_advert_t control_debug_pub;
/* initialize the pid controllers when the function is called for the first time */
if (initialized == false) {
parameters_init(&h);
@@ -185,6 +193,8 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
1000.0f, 0.2f, PID_MODE_DERIVATIV_CALC);
pid_init(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1000.0f,
1000.0f, 0.2f, PID_MODE_DERIVATIV_CALC);
control_debug_pub = orb_advertise(ORB_ID(vehicle_control_debug), &control_debug);
}
/* load new parameters with lower rate */
@@ -197,11 +207,11 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
/* control pitch (forward) output */
float pitch_control = pid_calculate(&pitch_rate_controller, rate_sp->pitch ,
rates[1], 0.0f, deltaT, NULL, NULL, NULL);
rates[1], 0.0f, deltaT, &control_debug.pitch_rate_p, &control_debug.pitch_rate_i, &control_debug.pitch_rate_d);
/* control roll (left/right) output */
float roll_control = pid_calculate(&roll_rate_controller, rate_sp->roll ,
rates[0], 0.0f, deltaT, NULL, NULL, NULL);
rates[0], 0.0f, deltaT, &control_debug.roll_rate_p, &control_debug.roll_rate_i, &control_debug.roll_rate_d);
/* increase resilience to faulty control inputs */
if (isfinite(pitch_control)) {
@@ -236,4 +246,6 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
actuators->control[3] = rate_sp->thrust;
motor_skip_counter++;
orb_publish(ORB_ID(vehicle_control_debug), control_debug_pub, &control_debug);
}