diff options
Diffstat (limited to 'apps/multirotor_att_control/multirotor_att_control_main.c')
-rw-r--r-- | apps/multirotor_att_control/multirotor_att_control_main.c | 16 |
1 files changed, 13 insertions, 3 deletions
diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c index 21c720096..70b9d8e28 100644 --- a/apps/multirotor_att_control/multirotor_att_control_main.c +++ b/apps/multirotor_att_control/multirotor_att_control_main.c @@ -76,7 +76,7 @@ __EXPORT int multirotor_att_control_main(int argc, char *argv[]); static bool thread_should_exit; static int mc_task; static bool motor_test_mode = false; -static struct actuator_controls_s actuators; + static orb_advert_t actuator_pub; /** @@ -86,6 +86,8 @@ static void *rate_control_thread_main(void *arg) { prctl(PR_SET_NAME, "mc rate control", getpid()); + struct actuator_controls_s actuators; + int gyro_sub = orb_subscribe(ORB_ID(sensor_gyro)); int rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); @@ -125,8 +127,8 @@ static void *rate_control_thread_main(void *arg) gyro_lp[1] = gyro_report.y; gyro_lp[2] = gyro_report.z; - multirotor_control_rates(&rates_sp, gyro_lp, &actuators); - orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); +// multirotor_control_rates(&rates_sp, gyro_lp, &actuators); +// orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); // } } } @@ -153,6 +155,8 @@ mc_thread_main(int argc, char *argv[]) struct vehicle_rates_setpoint_s rates_sp; memset(&rates_sp, 0, sizeof(rates_sp)); + struct actuator_controls_s actuators; + /* subscribe to attitude, motor setpoints and system state */ int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); int att_setpoint_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); @@ -228,6 +232,7 @@ mc_thread_main(int argc, char *argv[]) /* set yaw rate */ rates_sp.yaw = manual.yaw; att_sp.thrust = manual.throttle; + //printf("Roll: %4.4f, Pitch: %4.4f, Yaw: %4.4f, Thrust: %4.4f\n",att_sp.roll_body, att_sp.pitch_body, att_sp.yaw_body, att_sp.thrust); att_sp.timestamp = hrt_absolute_time(); /* STEP 2: publish the result to the vehicle actuators */ orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); @@ -269,10 +274,15 @@ mc_thread_main(int argc, char *argv[]) /* run attitude controller */ if (state.flag_control_attitude_enabled && !state.flag_control_rates_enabled) { multirotor_control_attitude(&att_sp, &att, NULL, &actuators); +// printf("publish actuator\n"); + +// printf("MAC_PUB: Roll: %4.4f, Pitch: %4.4f, Yaw: %4.4f, Thrust: %4.4f\n",actuators.control[0], actuators.control[1], actuators.control[2], actuators.control[3]); + orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); } else if (state.flag_control_attitude_enabled && state.flag_control_rates_enabled) { multirotor_control_attitude(&att_sp, &att, &rates_sp, NULL); orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp); +// printf("publish attitude\n"); } |