aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorJulian Oes <julian@oes.ch>2013-06-16 12:59:50 +0200
committerJulian Oes <julian@oes.ch>2013-06-16 15:25:24 +0200
commit2cb928d87c385335de72bb83710583a288d05cc4 (patch)
tree8c64b7050fd02df3ec00f59d1aeab559fbc26910
parentc189ac1c85a272142f925339879f22563c092725 (diff)
downloadpx4-firmware-2cb928d87c385335de72bb83710583a288d05cc4.tar.gz
px4-firmware-2cb928d87c385335de72bb83710583a288d05cc4.tar.bz2
px4-firmware-2cb928d87c385335de72bb83710583a288d05cc4.zip
Added ctrl debugging values
Conflicts: src/modules/sdlog2/sdlog2.c
-rw-r--r--src/modules/multirotor_att_control/multirotor_rate_control.c16
-rw-r--r--src/modules/sdlog2/sdlog2.c36
-rw-r--r--src/modules/sdlog2/sdlog2_messages.h28
-rw-r--r--src/modules/uORB/objects_common.cpp3
-rw-r--r--src/modules/uORB/topics/vehicle_control_debug.h87
5 files changed, 165 insertions, 5 deletions
diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.c b/src/modules/multirotor_att_control/multirotor_rate_control.c
index 322c5485a..01bf383e2 100644
--- a/src/modules/multirotor_att_control/multirotor_rate_control.c
+++ b/src/modules/multirotor_att_control/multirotor_rate_control.c
@@ -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);
}
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index a14bd6f80..347d5dd20 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -73,6 +73,7 @@
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/vehicle_vicon_position.h>
+#include <uORB/topics/vehicle_control_debug.h>
#include <uORB/topics/optical_flow.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/differential_pressure.h>
@@ -612,7 +613,7 @@ int sdlog2_thread_main(int argc, char *argv[])
/* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */
/* number of messages */
- const ssize_t fdsc = 15;
+ const ssize_t fdsc = 16;
/* Sanity check variable and index */
ssize_t fdsc_count = 0;
/* file descriptors to wait for */
@@ -635,6 +636,7 @@ int sdlog2_thread_main(int argc, char *argv[])
struct vehicle_global_position_s global_pos;
struct vehicle_gps_position_s gps_pos;
struct vehicle_vicon_position_s vicon_pos;
+ struct vehicle_control_debug_s control_debug;
struct optical_flow_s flow;
struct rc_channels_s rc;
struct differential_pressure_s diff_pres;
@@ -656,6 +658,7 @@ int sdlog2_thread_main(int argc, char *argv[])
int global_pos_sub;
int gps_pos_sub;
int vicon_pos_sub;
+ int control_debug_sub;
int flow_sub;
int rc_sub;
} subs;
@@ -675,6 +678,7 @@ int sdlog2_thread_main(int argc, char *argv[])
struct log_GPS_s log_GPS;
struct log_ATTC_s log_ATTC;
struct log_STAT_s log_STAT;
+ struct log_CTRL_s log_CTRL;
struct log_RC_s log_RC;
struct log_OUT0_s log_OUT0;
} body;
@@ -762,6 +766,12 @@ int sdlog2_thread_main(int argc, char *argv[])
fds[fdsc_count].events = POLLIN;
fdsc_count++;
+ /* --- CONTROL DEBUG --- */
+ subs.control_debug_sub = orb_subscribe(ORB_ID(vehicle_control_debug));
+ fds[fdsc_count].fd = subs.control_debug_sub;
+ fds[fdsc_count].events = POLLIN;
+ fdsc_count++;
+
/* --- OPTICAL FLOW --- */
subs.flow_sub = orb_subscribe(ORB_ID(optical_flow));
fds[fdsc_count].fd = subs.flow_sub;
@@ -1031,6 +1041,30 @@ int sdlog2_thread_main(int argc, char *argv[])
// TODO not implemented yet
}
+ /* --- CONTROL DEBUG --- */
+ if (fds[ifds++].revents & POLLIN) {
+ orb_copy(ORB_ID(vehicle_control_debug), subs.control_debug_sub, &buf.control_debug);
+
+ log_msg.body.log_CTRL.roll_p = buf.control_debug.roll_p;
+ log_msg.body.log_CTRL.roll_i = buf.control_debug.roll_i;
+ log_msg.body.log_CTRL.roll_d = buf.control_debug.roll_d;
+ log_msg.body.log_CTRL.roll_rate_p = buf.control_debug.roll_rate_p;
+ log_msg.body.log_CTRL.roll_rate_i = buf.control_debug.roll_rate_i;
+ log_msg.body.log_CTRL.roll_rate_d = buf.control_debug.roll_rate_d;
+ log_msg.body.log_CTRL.pitch_p = buf.control_debug.pitch_p;
+ log_msg.body.log_CTRL.pitch_i = buf.control_debug.pitch_i;
+ log_msg.body.log_CTRL.pitch_d = buf.control_debug.pitch_d;
+ log_msg.body.log_CTRL.pitch_rate_p = buf.control_debug.pitch_rate_p;
+ log_msg.body.log_CTRL.pitch_rate_i = buf.control_debug.pitch_rate_i;
+ log_msg.body.log_CTRL.pitch_rate_d = buf.control_debug.pitch_rate_d;
+ log_msg.body.log_CTRL.yaw_p = buf.control_debug.yaw_p;
+ log_msg.body.log_CTRL.yaw_i = buf.control_debug.yaw_i;
+ log_msg.body.log_CTRL.yaw_d = buf.control_debug.yaw_d;
+ log_msg.body.log_CTRL.yaw_rate_p = buf.control_debug.yaw_rate_p;
+ log_msg.body.log_CTRL.yaw_rate_i = buf.control_debug.yaw_rate_i;
+ log_msg.body.log_CTRL.yaw_rate_d = buf.control_debug.yaw_rate_d;
+ }
+
/* --- FLOW --- */
if (fds[ifds++].revents & POLLIN) {
orb_copy(ORB_ID(optical_flow), subs.flow_sub, &buf.flow);
diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h
index 40763ee1e..a3d35b596 100644
--- a/src/modules/sdlog2/sdlog2_messages.h
+++ b/src/modules/sdlog2/sdlog2_messages.h
@@ -156,14 +156,37 @@ struct log_STAT_s {
unsigned char battery_warning;
};
+/* --- CTRL - CONTROL DEBUG --- */
+#define LOG_CTRL_MSG 11
+struct log_CTRL_s {
+ float roll_p;
+ float roll_i;
+ float roll_d;
+ float roll_rate_p;
+ float roll_rate_i;
+ float roll_rate_d;
+ float pitch_p;
+ float pitch_i;
+ float pitch_d;
+ float pitch_rate_p;
+ float pitch_rate_i;
+ float pitch_rate_d;
+ float yaw_p;
+ float yaw_i;
+ float yaw_d;
+ float yaw_rate_p;
+ float yaw_rate_i;
+ float yaw_rate_d;
+};
+
/* --- RC - RC INPUT CHANNELS --- */
-#define LOG_RC_MSG 11
+#define LOG_RC_MSG 12
struct log_RC_s {
float channel[8];
};
/* --- OUT0 - ACTUATOR_0 OUTPUT --- */
-#define LOG_OUT0_MSG 12
+#define LOG_OUT0_MSG 13
struct log_OUT0_s {
float output[8];
};
@@ -182,6 +205,7 @@ static const struct log_format_s log_formats[] = {
LOG_FORMAT(GPS, "QBffLLfffff", "GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"),
LOG_FORMAT(ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"),
LOG_FORMAT(STAT, "BBBBBfffB", "State,FlightMode,CtlMode,SASMode,Armed,BatV,BatC,BatRem,BatWarn"),
+ LOG_FORMAT(CTRL, "ffffffffffffffffff", "RollP,RollI,RollD,RollRP,RollRI,RollRD,PitchP,PitchI,PitchD,PitchRP,PitchRI,PitchRD,YawP,YawI,YawD,YawRP,YawRI,YawRD"),
LOG_FORMAT(RC, "ffffffff", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7"),
LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"),
};
diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp
index e22b58cad..ddf9272ec 100644
--- a/src/modules/uORB/objects_common.cpp
+++ b/src/modules/uORB/objects_common.cpp
@@ -116,6 +116,9 @@ ORB_DEFINE(vehicle_attitude_setpoint, struct vehicle_attitude_setpoint_s);
#include "topics/manual_control_setpoint.h"
ORB_DEFINE(manual_control_setpoint, struct manual_control_setpoint_s);
+#include "topics/vehicle_control_debug.h"
+ORB_DEFINE(vehicle_control_debug, struct vehicle_control_debug_s);
+
#include "topics/offboard_control_setpoint.h"
ORB_DEFINE(offboard_control_setpoint, struct offboard_control_setpoint_s);
diff --git a/src/modules/uORB/topics/vehicle_control_debug.h b/src/modules/uORB/topics/vehicle_control_debug.h
new file mode 100644
index 000000000..6184284a4
--- /dev/null
+++ b/src/modules/uORB/topics/vehicle_control_debug.h
@@ -0,0 +1,87 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ * Author: @author Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file vehicle_control_debug.h
+ * For debugging purposes to log PID parts of controller
+ */
+
+#ifndef TOPIC_VEHICLE_CONTROL_DEBUG_H_
+#define TOPIC_VEHICLE_CONTROL_DEBUG_H_
+
+#include <stdint.h>
+#include "../uORB.h"
+
+/**
+ * @addtogroup topics
+ * @{
+ */
+struct vehicle_control_debug_s
+{
+ uint64_t timestamp; /**< in microseconds since system start */
+
+ float roll_p; /**< roll P control part */
+ float roll_i; /**< roll I control part */
+ float roll_d; /**< roll D control part */
+
+ float roll_rate_p; /**< roll rate P control part */
+ float roll_rate_i; /**< roll rate I control part */
+ float roll_rate_d; /**< roll rate D control part */
+
+ float pitch_p; /**< pitch P control part */
+ float pitch_i; /**< pitch I control part */
+ float pitch_d; /**< pitch D control part */
+
+ float pitch_rate_p; /**< pitch rate P control part */
+ float pitch_rate_i; /**< pitch rate I control part */
+ float pitch_rate_d; /**< pitch rate D control part */
+
+ float yaw_p; /**< yaw P control part */
+ float yaw_i; /**< yaw I control part */
+ float yaw_d; /**< yaw D control part */
+
+ float yaw_rate_p; /**< yaw rate P control part */
+ float yaw_rate_i; /**< yaw rate I control part */
+ float yaw_rate_d; /**< yaw rate D control part */
+
+}; /**< vehicle_control_debug */
+
+ /**
+ * @}
+ */
+
+/* register this as object request broker structure */
+ORB_DECLARE(vehicle_control_debug);
+
+#endif