forked from Archive/PX4-Autopilot
Added ctrl debugging values
Conflicts: src/modules/sdlog2/sdlog2.c
This commit is contained in:
Julian Oes
parent
c189ac1c85
commit
2cb928d87c
|
|
@ -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);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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"),
|
||||
};
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
Loading…
Reference in New Issue