diff options
Diffstat (limited to 'src')
7 files changed, 42 insertions, 52 deletions
diff --git a/src/modules/multirotor_att_control/multirotor_att_control_main.c b/src/modules/multirotor_att_control/multirotor_att_control_main.c index b4d7fb630..c3b2f5d2a 100644 --- a/src/modules/multirotor_att_control/multirotor_att_control_main.c +++ b/src/modules/multirotor_att_control/multirotor_att_control_main.c @@ -64,6 +64,7 @@ #include <uORB/topics/manual_control_setpoint.h> #include <uORB/topics/offboard_control_setpoint.h> #include <uORB/topics/vehicle_rates_setpoint.h> +#include <uORB/topics/vehicle_control_debug.h> #include <uORB/topics/sensor_combined.h> #include <uORB/topics/actuator_controls.h> #include <uORB/topics/parameter_update.h> @@ -84,7 +85,7 @@ static int mc_task; static bool motor_test_mode = false; static orb_advert_t actuator_pub; - +static orb_advert_t control_debug_pub; static int @@ -111,6 +112,9 @@ mc_thread_main(int argc, char *argv[]) struct actuator_controls_s actuators; memset(&actuators, 0, sizeof(actuators)); + struct vehicle_control_debug_s control_debug; + memset(&control_debug, 0, sizeof(control_debug)); + /* subscribe to attitude, motor setpoints and system state */ int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); int param_sub = orb_subscribe(ORB_ID(parameter_update)); @@ -138,6 +142,8 @@ mc_thread_main(int argc, char *argv[]) for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) { actuators.control[i] = 0.0f; } + + control_debug_pub = orb_advertise(ORB_ID(vehicle_control_debug), &control_debug); actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators); orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); @@ -380,7 +386,7 @@ mc_thread_main(int argc, char *argv[]) /** STEP 3: Identify the controller setup to run and set up the inputs correctly */ if (control_mode.flag_control_attitude_enabled) { - multirotor_control_attitude(&att_sp, &att, &rates_sp, control_yaw_position); + multirotor_control_attitude(&att_sp, &att, &rates_sp, control_yaw_position, &control_debug_pub, &control_debug); orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp); } @@ -403,9 +409,11 @@ mc_thread_main(int argc, char *argv[]) gyro[1] = att.pitchspeed; gyro[2] = att.yawspeed; - multirotor_control_rates(&rates_sp, gyro, &actuators); + multirotor_control_rates(&rates_sp, gyro, &actuators, &control_debug_pub, &control_debug); orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); + orb_publish(ORB_ID(vehicle_control_debug), control_debug_pub, &control_debug); + /* update control_mode */ flag_control_attitude_enabled = control_mode.flag_control_attitude_enabled; flag_control_manual_enabled = control_mode.flag_control_manual_enabled; diff --git a/src/modules/multirotor_att_control/multirotor_attitude_control.c b/src/modules/multirotor_att_control/multirotor_attitude_control.c index 51faaa8c0..6da808da4 100644 --- a/src/modules/multirotor_att_control/multirotor_attitude_control.c +++ b/src/modules/multirotor_att_control/multirotor_attitude_control.c @@ -123,7 +123,8 @@ static int parameters_update(const struct mc_att_control_param_handles *h, struc } void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, - const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, bool control_yaw_position) + const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, bool control_yaw_position, + const orb_advert_t *control_debug_pub, struct vehicle_control_debug_s *control_debug) { static uint64_t last_run = 0; static uint64_t last_input = 0; @@ -182,11 +183,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, NULL, NULL, NULL); + att->pitch, att->pitchspeed, deltaT, &control_debug->roll_p, &control_debug->roll_i, &control_debug->roll_d); /* control roll (left/right) output */ rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_body , - att->roll, att->rollspeed, deltaT, NULL, NULL, NULL); + att->roll, att->rollspeed, deltaT, &control_debug->pitch_p, &control_debug->pitch_i, &control_debug->pitch_d); // 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_attitude_control.h b/src/modules/multirotor_att_control/multirotor_attitude_control.h index 2cf83e443..4e70a5103 100644 --- a/src/modules/multirotor_att_control/multirotor_attitude_control.h +++ b/src/modules/multirotor_att_control/multirotor_attitude_control.h @@ -50,8 +50,11 @@ #include <uORB/topics/vehicle_attitude_setpoint.h> #include <uORB/topics/vehicle_rates_setpoint.h> #include <uORB/topics/actuator_controls.h> +#include <uORB/topics/vehicle_control_debug.h> + void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, - const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, bool control_yaw_position); + const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, bool control_yaw_position, + const orb_advert_t *control_debug_pub, struct vehicle_control_debug_s *control_debug); #endif /* MULTIROTOR_ATTITUDE_CONTROL_H_ */ diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.c b/src/modules/multirotor_att_control/multirotor_rate_control.c index e8dcbacc7..8cd97d7c1 100644 --- a/src/modules/multirotor_att_control/multirotor_rate_control.c +++ b/src/modules/multirotor_att_control/multirotor_rate_control.c @@ -55,7 +55,7 @@ #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 */ @@ -151,7 +151,8 @@ static int parameters_update(const struct mc_rate_control_param_handles *h, stru } void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, - const float rates[], struct actuator_controls_s *actuators) + const float rates[], struct actuator_controls_s *actuators, + const orb_advert_t *control_debug_pub, struct vehicle_control_debug_s *control_debug) { static uint64_t last_run = 0; const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; @@ -178,10 +179,6 @@ 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) { @@ -194,7 +191,6 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, 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 */ @@ -207,11 +203,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, &control_debug.pitch_rate_p, &control_debug.pitch_rate_i, &control_debug.pitch_rate_d); + 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, &control_debug.roll_rate_p, &control_debug.roll_rate_i, &control_debug.roll_rate_d); + 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)) { @@ -246,7 +242,4 @@ 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); - printf("Published control debug\n"); } diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.h b/src/modules/multirotor_att_control/multirotor_rate_control.h index 03dec317a..465549540 100644 --- a/src/modules/multirotor_att_control/multirotor_rate_control.h +++ b/src/modules/multirotor_att_control/multirotor_rate_control.h @@ -49,8 +49,10 @@ #include <uORB/topics/vehicle_attitude.h> #include <uORB/topics/vehicle_rates_setpoint.h> #include <uORB/topics/actuator_controls.h> +#include <uORB/topics/vehicle_control_debug.h> void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, - const float rates[], struct actuator_controls_s *actuators); + const float rates[], struct actuator_controls_s *actuators, + const orb_advert_t *control_debug_pub, struct vehicle_control_debug_s *control_debug); #endif /* MULTIROTOR_RATE_CONTROL_H_ */ diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index a86623304..5a3478665 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -91,7 +91,6 @@ #define LOGBUFFER_WRITE_AND_COUNT(_msg) if (logbuffer_write(&lb, &log_msg, LOG_PACKET_SIZE(_msg))) { \ log_msgs_written++; \ - printf("size: %d\n", LOG_PACKET_SIZE(_msg)); \ } else { \ log_msgs_skipped++; \ /*printf("skip\n");*/ \ @@ -616,6 +615,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 = 17; + /* Sanity check variable and index */ ssize_t fdsc_count = 0; /* file descriptors to wait for */ @@ -1072,26 +1072,20 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- CONTROL DEBUG --- */ if (fds[ifds++].revents & POLLIN) { orb_copy(ORB_ID(vehicle_control_debug), subs.control_debug_sub, &buf.control_debug); - printf("copied control debug\n"); - // 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.msg_type = LOG_CTRL_MSG; + 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_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; LOGBUFFER_WRITE_AND_COUNT(CTRL); } diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 8930db33b..98a64ef21 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -159,29 +159,18 @@ struct log_STAT_s { /* --- CTRL - CONTROL DEBUG --- */ #define LOG_CTRL_MSG 11 struct log_CTRL_s { - // float roll_p; - // float roll_i; - // float roll_d; - + 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_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 --- */ @@ -210,7 +199,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, "fffffffff", "RollRP,RollRI,RollRD,PitchRP,PitchRI,PitchRD,YawRP,YawRI,YawRD"), + LOG_FORMAT(CTRL, "ffffffffffff", "RP,RI,RD,RRP,RRI,RRD,PP,PI,PD,PRP,PRI,PRD"), LOG_FORMAT(RC, "ffffffff", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7"), LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"), }; |