fixed wing manual setpoints in manual mode

1 files changed, 35 insertions, 15 deletions

diff --git a/apps/fixedwing_att_control/fixedwing_att_control_main.c b/apps/fixedwing_att_control/fixedwing_att_control_main.c
index ad0f201e1..38f58ac33 100644
--- a/apps/fixedwing_att_control/fixedwing_att_control_main.c
+++ b/apps/fixedwing_att_control/fixedwing_att_control_main.c
@@ -135,6 +135,11 @@ int fixedwing_att_control_thread_main(int argc, char *argv[])
 		memset(&att_sp, 0, sizeof(att_sp));
 		struct vehicle_rates_setpoint_s rates_sp;
 		memset(&rates_sp, 0, sizeof(rates_sp));
+		struct manual_control_setpoint_s manual_sp;
+		memset(&manual_sp, 0, sizeof(manual_sp));
+		struct vehicle_status_s vstatus;
+		memset(&vstatus, 0, sizeof(vstatus));
+
 //		static struct debug_key_value_s debug_output;
 //		memset(&debug_output, 0, sizeof(debug_output));
 
@@ -144,16 +149,19 @@ int fixedwing_att_control_thread_main(int argc, char *argv[])
 
 
 		/* publish actuator controls */
-		for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++)
+		for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) {
 			actuators.control[i] = 0.0f;
+		}
 		orb_advert_t actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators);
 //		orb_advert_t debug_pub = orb_advertise(ORB_ID(debug_key_value), &debug_output);
 //		debug_output.key[0] = '1';
 
 
-		/* subscribe  */
+		/* subscribe */
 		int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
 		int att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
+		int manual_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
+		int vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
 
 		/* Setup of loop */
 		float gyro[3] = {0.0f, 0.0f, 0.0f};
@@ -164,10 +172,11 @@ int fixedwing_att_control_thread_main(int argc, char *argv[])
 			/* wait for a sensor update, check for exit condition every 500 ms */
 			poll(&fds, 1, 500);
 
-			/*Get Local Copies */
 			/* get a local copy of attitude */
 			orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
 			orb_copy(ORB_ID(vehicle_attitude_setpoint), att_sp_sub, &att_sp);
+			orb_copy(ORB_ID(manual_control_setpoint), manual_sp_sub, &manual_sp);
+			orb_copy(ORB_ID(vehicle_status), vstatus_sub, &vstatus);
 
 			gyro[0] = att.rollspeed;
 			gyro[1] = att.pitchspeed;
@@ -175,20 +184,31 @@ int fixedwing_att_control_thread_main(int argc, char *argv[])
 
 			/* Control */
 
-			/* Attitude Control */
-			fixedwing_att_control_attitude(&att_sp,
-					&att,
-					&rates_sp);
-
-			/* Attitude Rate Control */
-			fixedwing_att_control_rates(&rates_sp, gyro, &actuators);
-
-			//REMOVEME XXX
-			actuators.control[3] = 0.7f;
+			if (vstatus.state_machine == SYSTEM_STATE_AUTO) {
+				/* Attitude Control */
+				fixedwing_att_control_attitude(&att_sp,
+						&att,
+						&rates_sp);
+
+				/* Attitude Rate Control */
+				fixedwing_att_control_rates(&rates_sp, gyro, &actuators);
+
+				//REMOVEME XXX
+				actuators.control[3] = 0.7f;
+
+	//			debug_output.value = rates_sp.pitch;
+	//			orb_publish(ORB_ID(debug_key_value), debug_pub, &debug_output);
+			} else if (vstatus.state_machine == SYSTEM_STATE_MANUAL) {
+				/* 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;
+			}
 
+			/* publish output */
 			orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
-//			debug_output.value = rates_sp.pitch;
-//			orb_publish(ORB_ID(debug_key_value), debug_pub, &debug_output);
 		}
 
 		printf("[fixedwing_att_control] exiting, stopping all motors.\n");