diff options
| author | Lorenz Meier <lm@inf.ethz.ch> | 2012-12-13 11:12:34 +0100 |
|---|---|---|
| committer | Lorenz Meier <lm@inf.ethz.ch> | 2012-12-13 11:12:34 +0100 |
| commit | 1fc0a6546e30cfc913de8c0c03cfc625fba5d2ae (patch) | |
| tree | 65cf4f9b03e9dc63ffdf05ff698272f326d76908 /apps/fixedwing_att_control | |
| parent | 26faab64e5e1679d15afe88ef0edebd598f47dc7 (diff) | |
| parent | 03076a72ca75917cf62d7889c6c6d0de36866b04 (diff) | |
| download | px4-firmware-1fc0a6546e30cfc913de8c0c03cfc625fba5d2ae.tar.gz px4-firmware-1fc0a6546e30cfc913de8c0c03cfc625fba5d2ae.tar.bz2 px4-firmware-1fc0a6546e30cfc913de8c0c03cfc625fba5d2ae.zip | |
Merged IO feature branch
Diffstat (limited to 'apps/fixedwing_att_control')
| -rw-r--r-- | apps/fixedwing_att_control/fixedwing_att_control_main.c | 20 |
1 files changed, 11 insertions, 9 deletions
diff --git a/apps/fixedwing_att_control/fixedwing_att_control_main.c b/apps/fixedwing_att_control/fixedwing_att_control_main.c index 8ca87ab53..3e1fc4952 100644 --- a/apps/fixedwing_att_control/fixedwing_att_control_main.c +++ b/apps/fixedwing_att_control/fixedwing_att_control_main.c @@ -183,13 +183,11 @@ int fixedwing_att_control_thread_main(int argc, char *argv[]) gyro[1] = att.pitchspeed; gyro[2] = att.yawspeed; - /* Control */ - if (vstatus.state_machine == SYSTEM_STATE_AUTO) { + /* control */ + if (vstatus.state_machine == SYSTEM_STATE_AUTO) { /* attitude control */ fixedwing_att_control_attitude(&att_sp, &att, speed_body, &rates_sp); - /* publish rate setpoint */ - orb_publish(ORB_ID(vehicle_rates_setpoint), rates_pub, &rates_sp); /* angular rate control */ fixedwing_att_control_rates(&rates_sp, gyro, &actuators); @@ -219,11 +217,9 @@ int fixedwing_att_control_thread_main(int argc, char *argv[]) att_sp.timestamp = hrt_absolute_time(); } + /* attitude control */ fixedwing_att_control_attitude(&att_sp, &att, speed_body, &rates_sp); - /* publish rate setpoint */ - orb_publish(ORB_ID(vehicle_rates_setpoint), rates_pub, &rates_sp); - /* angular rate control */ fixedwing_att_control_rates(&rates_sp, gyro, &actuators); @@ -242,8 +238,14 @@ int fixedwing_att_control_thread_main(int argc, char *argv[]) /* publish rates */ orb_publish(ORB_ID(vehicle_rates_setpoint), rates_pub, &rates_sp); - /* publish actuator outputs */ - orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); + /* sanity check and publish actuator outputs */ + if (isfinite(actuators.control[0]) && + isfinite(actuators.control[1]) && + isfinite(actuators.control[2]) && + isfinite(actuators.control[3])) + { + orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); + } } printf("[fixedwing_att_control] exiting, stopping all motors.\n"); |