From 303694f5f7b7a03488c6075ff2bd67014badfacb Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sun, 16 Jun 2013 09:55:28 +0200 Subject: Fixed pid bug, attitude was not controlled --- .../multirotor_att_control/multirotor_attitude_control.c | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) (limited to 'src/modules/multirotor_att_control') diff --git a/src/modules/multirotor_att_control/multirotor_attitude_control.c b/src/modules/multirotor_att_control/multirotor_attitude_control.c index d7e1a3adc..4c8f72b8d 100644 --- a/src/modules/multirotor_att_control/multirotor_attitude_control.c +++ b/src/modules/multirotor_att_control/multirotor_attitude_control.c @@ -155,8 +155,8 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s parameters_init(&h); parameters_update(&h, &p); - pid_init(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 0.0f, PID_MODE_DERIVATIV_SET); - pid_init(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 0.0f, PID_MODE_DERIVATIV_SET); + pid_init(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 0.2f, PID_MODE_DERIVATIV_SET); + pid_init(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 0.2f, PID_MODE_DERIVATIV_SET); initialized = true; } @@ -167,8 +167,8 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s parameters_update(&h, &p); /* apply parameters */ - pid_set_parameters(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 0.0f); - pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 0.0f); + pid_set_parameters(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 0.2f); + pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 0.2f); } /* reset integral if on ground */ @@ -188,6 +188,8 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_body , att->roll, att->rollspeed, deltaT); + // printf("rates_sp: %4.4f, att setpoint: %4.4f\n, pitch: %4.4f, pitchspeed: %4.4f, dT: %4.4f", rates_sp->pitch, att_sp->pitch_body, att->pitch, att->pitchspeed, deltaT); + if (control_yaw_position) { /* control yaw rate */ -- cgit v1.2.3 From c189ac1c85a272142f925339879f22563c092725 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 13 Mar 2013 18:00:00 -0700 Subject: Added possibility to log pid control values Conflicts: apps/multirotor_pos_control/multirotor_pos_control.c src/drivers/ardrone_interface/ardrone_interface.c --- src/modules/fixedwing_att_control/fixedwing_att_control_att.c | 4 ++-- src/modules/fixedwing_att_control/fixedwing_att_control_rate.c | 6 +++--- src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c | 8 ++++---- src/modules/multirotor_att_control/multirotor_attitude_control.c | 4 ++-- src/modules/multirotor_att_control/multirotor_rate_control.c | 4 ++-- src/modules/systemlib/pid/pid.c | 8 ++++++-- src/modules/systemlib/pid/pid.h | 2 +- 7 files changed, 20 insertions(+), 16 deletions(-) (limited to 'src/modules/multirotor_att_control') diff --git a/src/modules/fixedwing_att_control/fixedwing_att_control_att.c b/src/modules/fixedwing_att_control/fixedwing_att_control_att.c index a226757e0..7009356b7 100644 --- a/src/modules/fixedwing_att_control/fixedwing_att_control_att.c +++ b/src/modules/fixedwing_att_control/fixedwing_att_control_att.c @@ -142,7 +142,7 @@ int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att } /* Roll (P) */ - rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_body, att->roll, 0, 0); + rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_body, att->roll, 0, 0, NULL, NULL, NULL); /* Pitch (P) */ @@ -152,7 +152,7 @@ int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att /* set pitch plus feedforward roll compensation */ rates_sp->pitch = pid_calculate(&pitch_controller, att_sp->pitch_body + pitch_sp_rollcompensation, - att->pitch, 0, 0); + att->pitch, 0, 0, NULL, NULL, NULL); /* Yaw (from coordinated turn constraint or lateral force) */ rates_sp->yaw = (att->rollspeed * rates_sp->roll + 9.81f * sinf(att->roll) * cosf(att->pitch) + speed_body[0] * rates_sp->pitch * sinf(att->roll)) diff --git a/src/modules/fixedwing_att_control/fixedwing_att_control_rate.c b/src/modules/fixedwing_att_control/fixedwing_att_control_rate.c index 79194e515..991d5ec5d 100644 --- a/src/modules/fixedwing_att_control/fixedwing_att_control_rate.c +++ b/src/modules/fixedwing_att_control/fixedwing_att_control_rate.c @@ -196,11 +196,11 @@ int fixedwing_att_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, /* roll rate (PI) */ - actuators->control[0] = pid_calculate(&roll_rate_controller, rate_sp->roll, rates[0], 0.0f, deltaT); + actuators->control[0] = pid_calculate(&roll_rate_controller, rate_sp->roll, rates[0], 0.0f, deltaT, NULL, NULL, NULL); /* pitch rate (PI) */ - actuators->control[1] = -pid_calculate(&pitch_rate_controller, rate_sp->pitch, rates[1], 0.0f, deltaT); + actuators->control[1] = -pid_calculate(&pitch_rate_controller, rate_sp->pitch, rates[1], 0.0f, deltaT, NULL, NULL, NULL); /* yaw rate (PI) */ - actuators->control[2] = pid_calculate(&yaw_rate_controller, rate_sp->yaw, rates[2], 0.0f, deltaT); + actuators->control[2] = pid_calculate(&yaw_rate_controller, rate_sp->yaw, rates[2], 0.0f, deltaT, NULL, NULL, NULL); counter++; diff --git a/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c b/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c index 48ec3603f..9c19c6786 100644 --- a/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c +++ b/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c @@ -319,7 +319,7 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[]) // XXX what is xtrack_err.past_end? if (distance_res == OK /*&& !xtrack_err.past_end*/) { - float delta_psi_c = pid_calculate(&offtrack_controller, 0, xtrack_err.distance, 0.0f, 0.0f); //p.xtrack_p * xtrack_err.distance + float delta_psi_c = pid_calculate(&offtrack_controller, 0, xtrack_err.distance, 0.0f, 0.0f, NULL, NULL, NULL); //p.xtrack_p * xtrack_err.distance float psi_c = psi_track + delta_psi_c; float psi_e = psi_c - att.yaw; @@ -336,7 +336,7 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[]) } /* calculate roll setpoint, do this artificially around zero */ - float delta_psi_rate_c = pid_calculate(&heading_controller, psi_e, 0.0f, 0.0f, 0.0f); + float delta_psi_rate_c = pid_calculate(&heading_controller, psi_e, 0.0f, 0.0f, 0.0f, NULL, NULL, NULL); float psi_rate_track = 0; //=V_gr/r_track , this will be needed for implementation of arc following float psi_rate_c = delta_psi_rate_c + psi_rate_track; @@ -359,7 +359,7 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[]) float psi_rate_e_scaled = psi_rate_e * ground_speed / 9.81f; //* V_gr / g - attitude_setpoint.roll_body = pid_calculate(&heading_rate_controller, psi_rate_e_scaled, 0.0f, 0.0f, deltaT); + attitude_setpoint.roll_body = pid_calculate(&heading_rate_controller, psi_rate_e_scaled, 0.0f, 0.0f, deltaT, NULL, NULL, NULL); if (verbose) { printf("psi_rate_c %.4f ", (double)psi_rate_c); @@ -379,7 +379,7 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[]) if (pos_updated) { //TODO: take care of relative vs. ab. altitude - attitude_setpoint.pitch_body = pid_calculate(&altitude_controller, global_setpoint.altitude, global_pos.alt, 0.0f, 0.0f); + attitude_setpoint.pitch_body = pid_calculate(&altitude_controller, global_setpoint.altitude, global_pos.alt, 0.0f, 0.0f, NULL, NULL, NULL); } diff --git a/src/modules/multirotor_att_control/multirotor_attitude_control.c b/src/modules/multirotor_att_control/multirotor_attitude_control.c index 4c8f72b8d..51faaa8c0 100644 --- a/src/modules/multirotor_att_control/multirotor_attitude_control.c +++ b/src/modules/multirotor_att_control/multirotor_attitude_control.c @@ -182,11 +182,11 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s /* control pitch (forward) output */ rates_sp->pitch = pid_calculate(&pitch_controller, att_sp->pitch_body , - att->pitch, att->pitchspeed, deltaT); + att->pitch, att->pitchspeed, deltaT, NULL, NULL, NULL); /* control roll (left/right) output */ rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_body , - att->roll, att->rollspeed, deltaT); + att->roll, att->rollspeed, deltaT, NULL, NULL, NULL); // printf("rates_sp: %4.4f, att setpoint: %4.4f\n, pitch: %4.4f, pitchspeed: %4.4f, dT: %4.4f", rates_sp->pitch, att_sp->pitch_body, att->pitch, att->pitchspeed, deltaT); diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.c b/src/modules/multirotor_att_control/multirotor_rate_control.c index 57aea726a..322c5485a 100644 --- a/src/modules/multirotor_att_control/multirotor_rate_control.c +++ b/src/modules/multirotor_att_control/multirotor_rate_control.c @@ -197,11 +197,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); + rates[1], 0.0f, deltaT, NULL, NULL, NULL); /* control roll (left/right) output */ float roll_control = pid_calculate(&roll_rate_controller, rate_sp->roll , - rates[0], 0.0f, deltaT); + rates[0], 0.0f, deltaT, NULL, NULL, NULL); /* increase resilience to faulty control inputs */ if (isfinite(pitch_control)) { diff --git a/src/modules/systemlib/pid/pid.c b/src/modules/systemlib/pid/pid.c index 0188a51c4..3685b2aeb 100644 --- a/src/modules/systemlib/pid/pid.c +++ b/src/modules/systemlib/pid/pid.c @@ -124,7 +124,7 @@ __EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float * @param dt * @return */ -__EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt) +__EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt, float *ctrl_p, float *ctrl_i, float *ctrl_d) { /* error = setpoint - actual_position integral = integral + (error*dt) @@ -152,7 +152,7 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo if (pid->mode == PID_MODE_DERIVATIV_CALC) { // d = (error_filtered - pid->error_previous_filtered) / dt; - d = pid->error_previous_filtered - error_filtered; + d = error_filtered - pid->error_previous_filtered ; pid->error_previous_filtered = error_filtered; } else if (pid->mode == PID_MODE_DERIVATIV_SET) { d = -val_dot; @@ -194,6 +194,10 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo // pid->error_previous = error; // } + *ctrl_p = (error * pid->kp); + *ctrl_i = (i * pid->ki); + *ctrl_d = (d * pid->kd); + return pid->last_output; } diff --git a/src/modules/systemlib/pid/pid.h b/src/modules/systemlib/pid/pid.h index f89c36d75..e3d9038cd 100644 --- a/src/modules/systemlib/pid/pid.h +++ b/src/modules/systemlib/pid/pid.h @@ -72,7 +72,7 @@ typedef struct { __EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, float limit, float diff_filter_factor, uint8_t mode); __EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax, float limit, float diff_filter_factor); //void pid_set(PID_t *pid, float sp); -__EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt); +__EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt, float *ctrl_p, float *ctrl_i, float *ctrl_d); __EXPORT void pid_reset_integral(PID_t *pid); -- cgit v1.2.3 From 2cb928d87c385335de72bb83710583a288d05cc4 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sun, 16 Jun 2013 12:59:50 +0200 Subject: Added ctrl debugging values Conflicts: src/modules/sdlog2/sdlog2.c --- .../multirotor_rate_control.c | 16 +++- src/modules/sdlog2/sdlog2.c | 36 ++++++++- src/modules/sdlog2/sdlog2_messages.h | 28 ++++++- src/modules/uORB/objects_common.cpp | 3 + src/modules/uORB/topics/vehicle_control_debug.h | 87 ++++++++++++++++++++++ 5 files changed, 165 insertions(+), 5 deletions(-) create mode 100644 src/modules/uORB/topics/vehicle_control_debug.h (limited to 'src/modules/multirotor_att_control') 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 #include #include +#include +#include + 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 #include #include +#include #include #include #include @@ -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 + * + * 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 +#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 -- cgit v1.2.3