diff options
Diffstat (limited to 'apps/multirotor_att_control/multirotor_att_control_main.c')
-rw-r--r--[-rwxr-xr-x] | apps/multirotor_att_control/multirotor_att_control_main.c | 19 |
1 files changed, 7 insertions, 12 deletions
diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c index 63c7d0097..cb82036fe 100755..100644 --- a/apps/multirotor_att_control/multirotor_att_control_main.c +++ b/apps/multirotor_att_control/multirotor_att_control_main.c @@ -172,7 +172,7 @@ mc_thread_main(int argc, char *argv[]) rates_sp.pitch = offboard_sp.p2; rates_sp.yaw = offboard_sp.p3; rates_sp.thrust = offboard_sp.p4; - //printf("thrust_rate=%8.4f\n",offboard_sp.p4); +// printf("thrust_rate=%8.4f\n",offboard_sp.p4); rates_sp.timestamp = hrt_absolute_time(); orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp); } else if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE) { @@ -180,7 +180,7 @@ mc_thread_main(int argc, char *argv[]) att_sp.pitch_body = offboard_sp.p2; att_sp.yaw_body = offboard_sp.p3; att_sp.thrust = offboard_sp.p4; - //printf("thrust_att=%8.4f\n",offboard_sp.p4); +// printf("thrust_att=%8.4f\n",offboard_sp.p4); 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); @@ -193,6 +193,7 @@ mc_thread_main(int argc, char *argv[]) if (state.flag_control_rates_enabled && !state.flag_control_attitude_enabled) { rates_sp.roll = manual.roll; + rates_sp.pitch = manual.pitch; rates_sp.yaw = manual.yaw; rates_sp.thrust = manual.throttle; @@ -203,7 +204,7 @@ mc_thread_main(int argc, char *argv[]) if (state.flag_control_attitude_enabled) { att_sp.roll_body = manual.roll; att_sp.pitch_body = manual.pitch; - att_sp.yaw_body = manual.yaw; // XXX Hack, remove, switch to yaw rate controller + att_sp.yaw_body = manual.yaw; // XXX Hack, clean up /* set yaw rate */ rates_sp.yaw = manual.yaw; att_sp.thrust = manual.throttle; @@ -226,21 +227,15 @@ mc_thread_main(int argc, char *argv[]) } /** STEP 3: Identify the controller setup to run and set up the inputs correctly */ - - - /* run attitude controller */ - if (state.flag_control_attitude_enabled && !state.flag_control_rates_enabled) { - multirotor_control_attitude(&att_sp, &att, NULL, &actuators); - orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); - } else if (state.flag_control_attitude_enabled && state.flag_control_rates_enabled) { + if (state.flag_control_attitude_enabled) { multirotor_control_attitude(&att_sp, &att, &rates_sp, NULL); orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp); - } + } if (state.flag_control_rates_enabled) { - float gyro[3] = {0.0f, 0.0f, 0.0f}; + float gyro[3]; /* get current rate setpoint */ bool rates_sp_valid = false; |