aboutsummaryrefslogtreecommitdiff
path: root/src/modules/fixedwing_att_control/fixedwing_att_control_main.c
diff options
context:
space:
mode:
Diffstat (limited to 'src/modules/fixedwing_att_control/fixedwing_att_control_main.c')
-rw-r--r--src/modules/fixedwing_att_control/fixedwing_att_control_main.c168
1 files changed, 82 insertions, 86 deletions
diff --git a/src/modules/fixedwing_att_control/fixedwing_att_control_main.c b/src/modules/fixedwing_att_control/fixedwing_att_control_main.c
index b96fc3e5a..b6b4546c2 100644
--- a/src/modules/fixedwing_att_control/fixedwing_att_control_main.c
+++ b/src/modules/fixedwing_att_control/fixedwing_att_control_main.c
@@ -53,6 +53,7 @@
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_global_position_setpoint.h>
#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/manual_control_setpoint.h>
@@ -116,6 +117,8 @@ int fixedwing_att_control_thread_main(int argc, char *argv[])
memset(&global_pos, 0, sizeof(global_pos));
struct manual_control_setpoint_s manual_sp;
memset(&manual_sp, 0, sizeof(manual_sp));
+ struct vehicle_control_mode_s control_mode;
+ memset(&control_mode, 0, sizeof(control_mode));
struct vehicle_status_s vstatus;
memset(&vstatus, 0, sizeof(vstatus));
@@ -137,7 +140,8 @@ int fixedwing_att_control_thread_main(int argc, char *argv[])
int att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
int global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
int manual_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
- int vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
+ int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
+ int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
/* Setup of loop */
float gyro[3] = {0.0f, 0.0f, 0.0f};
@@ -178,96 +182,88 @@ int fixedwing_att_control_thread_main(int argc, char *argv[])
}
orb_copy(ORB_ID(manual_control_setpoint), manual_sp_sub, &manual_sp);
- orb_copy(ORB_ID(vehicle_status), vstatus_sub, &vstatus);
+ orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode);
+ orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vstatus);
gyro[0] = att.rollspeed;
gyro[1] = att.pitchspeed;
gyro[2] = att.yawspeed;
- /* control */
-
-#warning fix this
-// if (vstatus.state_machine == SYSTEM_STATE_AUTO ||
-// vstatus.state_machine == SYSTEM_STATE_STABILIZED) {
-// /* attitude control */
-// fixedwing_att_control_attitude(&att_sp, &att, speed_body, &rates_sp);
-//
-// /* angular rate control */
-// fixedwing_att_control_rates(&rates_sp, gyro, &actuators);
-//
-// /* pass through throttle */
-// actuators.control[3] = att_sp.thrust;
-//
-// /* set flaps to zero */
-// actuators.control[4] = 0.0f;
-//
-// } else if (vstatus.state_machine == SYSTEM_STATE_MANUAL) {
-// if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) {
-//
-// /* if the RC signal is lost, try to stay level and go slowly back down to ground */
-// if (vstatus.rc_signal_lost) {
-//
-// /* put plane into loiter */
-// att_sp.roll_body = 0.3f;
-// att_sp.pitch_body = 0.0f;
-//
-// /* limit throttle to 60 % of last value if sane */
-// if (isfinite(manual_sp.throttle) &&
-// (manual_sp.throttle >= 0.0f) &&
-// (manual_sp.throttle <= 1.0f)) {
-// att_sp.thrust = 0.6f * manual_sp.throttle;
-//
-// } else {
-// att_sp.thrust = 0.0f;
-// }
-//
-// att_sp.yaw_body = 0;
-//
-// // XXX disable yaw control, loiter
-//
-// } else {
-//
-// att_sp.roll_body = manual_sp.roll;
-// att_sp.pitch_body = manual_sp.pitch;
-// att_sp.yaw_body = 0;
-// att_sp.thrust = manual_sp.throttle;
-// }
-//
-// att_sp.timestamp = hrt_absolute_time();
-//
-// /* attitude control */
-// fixedwing_att_control_attitude(&att_sp, &att, speed_body, &rates_sp);
-//
-// /* angular rate control */
-// fixedwing_att_control_rates(&rates_sp, gyro, &actuators);
-//
-// /* pass through throttle */
-// actuators.control[3] = att_sp.thrust;
-//
-// /* pass through flaps */
-// if (isfinite(manual_sp.flaps)) {
-// actuators.control[4] = manual_sp.flaps;
-//
-// } else {
-// actuators.control[4] = 0.0f;
-// }
-//
-// } else if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) {
-// /* directly pass through values */
-// actuators.control[0] = manual_sp.roll;
-// /* positive pitch means negative actuator -> pull up */
-// actuators.control[1] = manual_sp.pitch;
-// actuators.control[2] = manual_sp.yaw;
-// actuators.control[3] = manual_sp.throttle;
-//
-// if (isfinite(manual_sp.flaps)) {
-// actuators.control[4] = manual_sp.flaps;
-//
-// } else {
-// actuators.control[4] = 0.0f;
-// }
-// }
-// }
+ /* set manual setpoints if required */
+ if (control_mode.flag_control_manual_enabled) {
+ if (control_mode.flag_control_attitude_enabled) {
+
+ /* if the RC signal is lost, try to stay level and go slowly back down to ground */
+ if (vstatus.rc_signal_lost) {
+
+ /* put plane into loiter */
+ att_sp.roll_body = 0.3f;
+ att_sp.pitch_body = 0.0f;
+
+ /* limit throttle to 60 % of last value if sane */
+ if (isfinite(manual_sp.throttle) &&
+ (manual_sp.throttle >= 0.0f) &&
+ (manual_sp.throttle <= 1.0f)) {
+ att_sp.thrust = 0.6f * manual_sp.throttle;
+
+ } else {
+ att_sp.thrust = 0.0f;
+ }
+
+ att_sp.yaw_body = 0;
+
+ // XXX disable yaw control, loiter
+
+ } else {
+
+ att_sp.roll_body = manual_sp.roll;
+ att_sp.pitch_body = manual_sp.pitch;
+ att_sp.yaw_body = 0;
+ att_sp.thrust = manual_sp.throttle;
+ }
+
+ att_sp.timestamp = hrt_absolute_time();
+
+ /* pass through flaps */
+ if (isfinite(manual_sp.flaps)) {
+ actuators.control[4] = manual_sp.flaps;
+
+ } else {
+ actuators.control[4] = 0.0f;
+ }
+
+ } else {
+ /* directly pass through values */
+ actuators.control[0] = manual_sp.roll;
+ /* positive pitch means negative actuator -> pull up */
+ actuators.control[1] = manual_sp.pitch;
+ actuators.control[2] = manual_sp.yaw;
+ actuators.control[3] = manual_sp.throttle;
+
+ if (isfinite(manual_sp.flaps)) {
+ actuators.control[4] = manual_sp.flaps;
+
+ } else {
+ actuators.control[4] = 0.0f;
+ }
+ }
+ }
+
+ /* execute attitude control if requested */
+ if (control_mode.flag_control_attitude_enabled) {
+ /* attitude control */
+ fixedwing_att_control_attitude(&att_sp, &att, speed_body, &rates_sp);
+
+ /* angular rate control */
+ fixedwing_att_control_rates(&rates_sp, gyro, &actuators);
+
+ /* pass through throttle */
+ actuators.control[3] = att_sp.thrust;
+
+ /* set flaps to zero */
+ actuators.control[4] = 0.0f;
+
+ }
/* publish rates */
orb_publish(ORB_ID(vehicle_rates_setpoint), rates_pub, &rates_sp);
generated by cgit v1.2.3 (git 2.39.1) at 2024-09-13 11:46:07 +0000