fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-21 07:13:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merged IO feature branch

Browse Source
This commit is contained in:
Lorenz Meier
2012-12-13 11:12:34 +01:00
parent 26faab64e5 03076a72ca
commit 1fc0a6546e
13 changed files with 253 additions and 60 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/fixedwing_att_control/fixedwing_att_control_main.c
+11 -9
View File
@@ -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");