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 | 41 |
1 files changed, 22 insertions, 19 deletions
diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c index 79ca9fe2d..3329c5c48 100644 --- a/apps/multirotor_att_control/multirotor_att_control_main.c +++ b/apps/multirotor_att_control/multirotor_att_control_main.c @@ -150,7 +150,9 @@ mc_thread_main(int argc, char *argv[]) /* store last control mode to detect mode switches */ bool flag_control_manual_enabled = false; bool flag_control_attitude_enabled = false; - bool flag_system_armed = false; + bool flag_fmu_armed = false; + bool flag_control_position_enabled = false; + bool flag_control_velocity_enabled = false; /* store if yaw position or yaw speed has been changed */ bool control_yaw_position = true; @@ -162,7 +164,6 @@ mc_thread_main(int argc, char *argv[]) param_t failsafe_throttle_handle = param_find("MC_RCLOSS_THR"); float failsafe_throttle = 0.0f; - while (!thread_should_exit) { /* wait for a sensor update, check for exit condition every 500 ms */ @@ -247,7 +248,7 @@ mc_thread_main(int argc, char *argv[]) /* initialize to current yaw if switching to manual or att control */ if (state.flag_control_attitude_enabled != flag_control_attitude_enabled || state.flag_control_manual_enabled != flag_control_manual_enabled || - state.flag_fmu_armed != flag_system_armed) { + state.flag_fmu_armed != flag_fmu_armed) { att_sp.yaw_body = att.yaw; } @@ -313,20 +314,20 @@ mc_thread_main(int argc, char *argv[]) // * settings as well. // */ // -// /* only move setpoint if manual input is != 0 */ -// if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.3f) { -// rates_sp.yaw = manual.yaw; -// control_yaw_position = false; -// first_time_after_yaw_speed_control = true; -// -// } else { -// if (first_time_after_yaw_speed_control) { -// att_sp.yaw_body = att.yaw; -// first_time_after_yaw_speed_control = false; -// } -// -// control_yaw_position = true; -// } + /* only move setpoint if manual input is != 0 */ + if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.3f) { + rates_sp.yaw = manual.yaw; + control_yaw_position = false; + first_time_after_yaw_speed_control = true; + + } else { + if (first_time_after_yaw_speed_control) { + att_sp.yaw_body = att.yaw; + first_time_after_yaw_speed_control = false; + } + + control_yaw_position = true; + } // } // } @@ -337,6 +338,7 @@ mc_thread_main(int argc, char *argv[]) /* STEP 2: publish the controller output */ orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); + if (motor_test_mode) { printf("testmode"); att_sp.roll_body = 0.0f; @@ -395,8 +397,9 @@ mc_thread_main(int argc, char *argv[]) /* update state */ flag_control_attitude_enabled = state.flag_control_attitude_enabled; flag_control_manual_enabled = state.flag_control_manual_enabled; - flag_system_armed = state.flag_fmu_armed; - // XXX add some logic to this + flag_control_position_enabled = state.flag_control_position_enabled; + flag_control_velocity_enabled = state.flag_control_velocity_enabled; + flag_fmu_armed = state.flag_fmu_armed; perf_end(mc_loop_perf); } /* end of poll call for attitude updates */ |