fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-01 11:31:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

multirotor_att_control: run rate controller only if vehicle_control_mode flag set, codestyle fixed

Browse Source
This commit is contained in:
Anton Babushkin
2013-08-16 15:08:10 +02:00
parent 57f8fe3719
commit 63af0a80ee
1 changed files with 23 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/multirotor_att_control/multirotor_att_control_main.c
+23 -20
View File
@@ -242,7 +242,7 @@ mc_thread_main(int argc, char *argv[])
/* control attitude, update attitude setpoint depending on mode */ /* control attitude, update attitude setpoint depending on mode */
/* initialize to current yaw if switching to manual or att control */ /* initialize to current yaw if switching to manual or att control */
if (control_mode.flag_control_attitude_enabled != flag_control_attitude_enabled || if (control_mode.flag_control_attitude_enabled != flag_control_attitude_enabled ||
control_mode.flag_control_manual_enabled != flag_control_manual_enabled) { control_mode.flag_control_manual_enabled != flag_control_manual_enabled) {
att_sp.yaw_body = att.yaw; att_sp.yaw_body = att.yaw;
} }
@@ -305,6 +305,7 @@ mc_thread_main(int argc, char *argv[])
att_sp.yaw_body = att.yaw; att_sp.yaw_body = att.yaw;
reset_yaw_sp = false; reset_yaw_sp = false;
} }
control_yaw_position = true; control_yaw_position = true;
} }
@@ -312,6 +313,7 @@ mc_thread_main(int argc, char *argv[])
/* don't update attitude setpoint in position control mode */ /* don't update attitude setpoint in position control mode */
att_sp.roll_body = manual.roll; att_sp.roll_body = manual.roll;
att_sp.pitch_body = manual.pitch; att_sp.pitch_body = manual.pitch;
if (!control_mode.flag_control_position_enabled) { if (!control_mode.flag_control_position_enabled) {
/* don't set throttle in altitude hold modes */ /* don't set throttle in altitude hold modes */
att_sp.thrust = manual.throttle; att_sp.thrust = manual.throttle;
@@ -355,24 +357,25 @@ mc_thread_main(int argc, char *argv[])
/* measure in what intervals the controller runs */ /* measure in what intervals the controller runs */
perf_count(mc_interval_perf); perf_count(mc_interval_perf);
float gyro[3]; if (control_mode.flag_control_rates_enabled) {
/* get current rate setpoint */
bool rates_sp_valid = false;
orb_check(rates_sp_sub, &rates_sp_valid);
/* get current rate setpoint */ if (rates_sp_valid) {
bool rates_sp_valid = false; orb_copy(ORB_ID(vehicle_rates_setpoint), rates_sp_sub, &rates_sp);
orb_check(rates_sp_sub, &rates_sp_valid); }
if (rates_sp_valid) { /* apply controller */
orb_copy(ORB_ID(vehicle_rates_setpoint), rates_sp_sub, &rates_sp); float gyro[3];
gyro[0] = att.rollspeed;
gyro[1] = att.pitchspeed;
gyro[2] = att.yawspeed;
multirotor_control_rates(&rates_sp, gyro, &actuators);
orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
} }
/* apply controller */
gyro[0] = att.rollspeed;
gyro[1] = att.pitchspeed;
gyro[2] = att.yawspeed;
multirotor_control_rates(&rates_sp, gyro, &actuators);
orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
/* update state */ /* update state */
flag_control_attitude_enabled = control_mode.flag_control_attitude_enabled; flag_control_attitude_enabled = control_mode.flag_control_attitude_enabled;
flag_control_manual_enabled = control_mode.flag_control_manual_enabled; flag_control_manual_enabled = control_mode.flag_control_manual_enabled;
@@ -449,11 +452,11 @@ int multirotor_att_control_main(int argc, char *argv[])
thread_should_exit = false; thread_should_exit = false;
mc_task = task_spawn_cmd("multirotor_att_control", mc_task = task_spawn_cmd("multirotor_att_control",
SCHED_DEFAULT, SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 15, SCHED_PRIORITY_MAX - 15,
2048, 2048,
mc_thread_main, mc_thread_main,
NULL); NULL);
exit(0); exit(0);
} }