diff options
author | Julian Oes <julian@oes.ch> | 2013-06-16 17:14:22 +0200 |
---|---|---|
committer | Julian Oes <julian@oes.ch> | 2013-06-16 17:14:22 +0200 |
commit | bca60b98bdc1bfe5a377946d8c50d8f5c54514d9 (patch) | |
tree | cbcf01bdd957a43eb08e4e440e6406a1fbd6eec8 /src/modules/multirotor_att_control/multirotor_att_control_main.c | |
parent | bd7f86bb6adf768169fe23b63d627a252586f6b7 (diff) | |
parent | 216617431da72ce7b34911f63fa5958302a531e1 (diff) | |
download | px4-firmware-bca60b98bdc1bfe5a377946d8c50d8f5c54514d9.tar.gz px4-firmware-bca60b98bdc1bfe5a377946d8c50d8f5c54514d9.tar.bz2 px4-firmware-bca60b98bdc1bfe5a377946d8c50d8f5c54514d9.zip |
Merge branch 'pid_fixes' into new_state_machine
Conflicts:
src/modules/multirotor_att_control/multirotor_rate_control.c
src/modules/sdlog2/sdlog2.c
src/modules/sdlog2/sdlog2_messages.h
and some fixes, logging of control PID values now working
Diffstat (limited to 'src/modules/multirotor_att_control/multirotor_att_control_main.c')
-rw-r--r-- | src/modules/multirotor_att_control/multirotor_att_control_main.c | 14 |
1 files changed, 11 insertions, 3 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; |