Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-21 04:33:10 +08:00
Code Issues Actions 9 Packages Projects Releases Wiki Activity

Correctly initializing and updating yaw setpoint, sign still to be checked

Browse Source
This commit is contained in:
Lorenz Meier
2012-10-30 07:20:23 +01:00
parent d5af511f8d
commit fedf5470d6
1 changed files with 18 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/multirotor_att_control/multirotor_att_control_main.c
+18 -2
View File
@@ -136,6 +136,10 @@ mc_thread_main(int argc, char *argv[])
/* welcome user */
printf("[multirotor_att_control] starting\n");
/* store last control mode to detect mode switches */
bool flag_control_manual_enabled = false;
bool flag_control_attitude_enabled = false;
while (!thread_should_exit) {
/* wait for a sensor update, check for exit condition every 500 ms */
@@ -197,14 +201,23 @@ mc_thread_main(int argc, char *argv[])
rates_sp.pitch = manual.pitch;
rates_sp.yaw = manual.yaw;
rates_sp.thrust = manual.throttle;
//printf("rates\n");
rates_sp.timestamp = hrt_absolute_time();
}
if (state.flag_control_attitude_enabled) {
/* 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) {
att_sp.yaw_body = att.yaw;
}
att_sp.roll_body = manual.roll;
att_sp.pitch_body = manual.pitch;
att_sp.yaw_body = att.yaw + manual.yaw * -2.0f;
/* only move setpoint if manual input is != 0 */
if (-3.0f * FLT_EPSILON < manual.yaw || manual.yaw > 3.0f * FLT_EPSILON) {
att_sp.yaw_body = att.yaw + manual.yaw * -2.0f;
}
att_sp.thrust = manual.throttle;
att_sp.timestamp = hrt_absolute_time();
}
@@ -251,6 +264,9 @@ mc_thread_main(int argc, char *argv[])
orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
}
flag_control_attitude_enabled = state.flag_control_attitude_enabled;
flag_control_manual_enabled = state.flag_control_manual_enabled;
perf_end(mc_loop_perf);
}