Further cleanups, added sanity check against system state machine

6 files changed, 60 insertions, 43 deletions

diff --git a/apps/ardrone_control/ardrone_control.c b/apps/ardrone_control/ardrone_control.c
index a9b04f592..e9a48bbfe 100644
--- a/apps/ardrone_control/ardrone_control.c
+++ b/apps/ardrone_control/ardrone_control.c
@@ -186,31 +186,40 @@ int ardrone_control_main(int argc, char *argv[])
 	int setpoint_sub = orb_subscribe(ORB_ID(ardrone_motors_setpoint));
 
 	while (1) {
-		if (control_mode == CONTROL_MODE_RATES) {
-			orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw);
-			orb_copy(ORB_ID(ardrone_motors_setpoint), setpoint_sub, &setpoint);
-			control_rates(ardrone_write, &raw, &setpoint);
-
-		} else if (control_mode == CONTROL_MODE_ATTITUDE) {
-
-			// XXX Add failsafe logic for RC loss situations
-			/* hardcore, last-resort safety checking */
-			//if (status->rc_signal_lost) {
-
-			/* get a local copy of the vehicle state */
-			orb_copy(ORB_ID(vehicle_status), state_sub, &state);
-			/* get a local copy of manual setpoint */
-			orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual);
-			/* get a local copy of attitude */
-			orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
-			/* get a local copy of attitude setpoint */
-			orb_copy(ORB_ID(vehicle_attitude_setpoint), att_setpoint_sub, &att_sp);
-
-			att_sp.roll_body = -manual.roll * M_PI_F / 8.0f;
-			att_sp.pitch_body = -manual.pitch * M_PI_F / 8.0f;
-			att_sp.yaw_body = -manual.yaw * M_PI_F;
-
-			control_attitude(ardrone_write, &att_sp, &att, &state);
+
+		if (state.state_machine == SYSTEM_STATE_MANUAL ||
+			state.state_machine == SYSTEM_STATE_GROUND_READY ||
+			state.state_machine == SYSTEM_STATE_STABILIZED ||
+			state.state_machine == SYSTEM_STATE_AUTO ||
+			state.state_machine == SYSTEM_STATE_MISSION_ABORT ||
+			state.state_machine == SYSTEM_STATE_EMCY_LANDING) {
+
+			if (control_mode == CONTROL_MODE_RATES) {
+				orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw);
+				orb_copy(ORB_ID(ardrone_motors_setpoint), setpoint_sub, &setpoint);
+				control_rates(ardrone_write, &raw, &setpoint);
+
+			} else if (control_mode == CONTROL_MODE_ATTITUDE) {
+
+				// XXX Add failsafe logic for RC loss situations
+				/* hardcore, last-resort safety checking */
+				//if (status->rc_signal_lost) {
+
+				/* get a local copy of the vehicle state */
+				orb_copy(ORB_ID(vehicle_status), state_sub, &state);
+				/* get a local copy of manual setpoint */
+				orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual);
+				/* get a local copy of attitude */
+				orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
+				/* get a local copy of attitude setpoint */
+				orb_copy(ORB_ID(vehicle_attitude_setpoint), att_setpoint_sub, &att_sp);
+
+				att_sp.roll_body = -manual.roll * M_PI_F / 8.0f;
+				att_sp.pitch_body = -manual.pitch * M_PI_F / 8.0f;
+				att_sp.yaw_body = -manual.yaw * M_PI_F;
+
+				control_attitude(ardrone_write, &att_sp, &att, &state);
+			}
 		}
 
 		if (counter % 30 == 0) {
diff --git a/apps/ardrone_control/attitude_control.c b/apps/ardrone_control/attitude_control.c
index db82a206e..034a9c5a6 100644
--- a/apps/ardrone_control/attitude_control.c
+++ b/apps/ardrone_control/attitude_control.c
@@ -1,12 +1,12 @@
 /****************************************************************************
  *
  *   Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
- *   Author: Thomas Gubler <thomasgubler@student.ethz.ch>
- *           Julian Oes <joes@student.ethz.ch>
- *           Laurens Mackay <mackayl@student.ethz.ch>
- *           Tobias Naegeli <naegelit@student.ethz.ch>
- *           Martin Rutschmann <rutmarti@student.ethz.ch>
- *           Lorenz Meier <lm@inf.ethz.ch>
+ *   Author: @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ *           @author Julian Oes <joes@student.ethz.ch>
+ *           @author Laurens Mackay <mackayl@student.ethz.ch>
+ *           @author Tobias Naegeli <naegelit@student.ethz.ch>
+ *           @author Martin Rutschmann <rutmarti@student.ethz.ch>
+ *           @author Lorenz Meier <lm@inf.ethz.ch>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -38,7 +38,8 @@
  ****************************************************************************/
 
 /*
- * @file Implementation of attitude controller
+ * @file attitude_control.c
+ * Implementation of attitude controller
  */
 
 #include "attitude_control.h"
@@ -331,7 +332,7 @@ void control_attitude(int ardrone_write, const struct vehicle_attitude_setpoint_
 
 	/* send motors via UART */
 	if (motor_skip_counter % 5 == 0) {
-		if (motor_skip_counter % 25) printf("mot: %1.3f-%i\n", motor_thrust, motor_pwm[0]);
+		if (motor_skip_counter % 50) printf("mot: %1.3f-%i-%i-%i-%i\n", motor_thrust, motor_pwm[0], motor_pwm[1], motor_pwm[2], motor_pwm[3]);
 		uint8_t buf[5] = {1, 2, 3, 4, 5};
 		ar_get_motor_packet(buf, motor_pwm[0], motor_pwm[1], motor_pwm[2], motor_pwm[3]);
 		write(ardrone_write, buf, sizeof(buf));
diff --git a/apps/ardrone_control/attitude_control.h b/apps/ardrone_control/attitude_control.h
index a7b01268b..1067d362c 100644
--- a/apps/ardrone_control/attitude_control.h
+++ b/apps/ardrone_control/attitude_control.h
@@ -1,9 +1,12 @@
 /****************************************************************************
  *
  *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: Thomas Gubler <thomasgubler@student.ethz.ch>
- *           Julian Oes <joes@student.ethz.ch>
- *
+ *   Author: @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ *           @author Julian Oes <joes@student.ethz.ch>
+ *           @author Laurens Mackay <mackayl@student.ethz.ch>
+ *           @author Tobias Naegeli <naegelit@student.ethz.ch>
+ *           @author Martin Rutschmann <rutmarti@student.ethz.ch>
+ *           @author Lorenz Meier <lm@inf.ethz.ch>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -34,7 +37,10 @@
  *
  ****************************************************************************/
 
-/* @file attitude control for quadrotors */
+/*
+ * @file attitude_control.h
+ * attitude control for multi rotors
+ */
 
 #ifndef ATTITUDE_CONTROL_H_
 #define ATTITUDE_CONTROL_H_
diff --git a/apps/ardrone_control/rate_control.c b/apps/ardrone_control/rate_control.c
index d2bedadf2..df20cd1ab 100644
--- a/apps/ardrone_control/rate_control.c
+++ b/apps/ardrone_control/rate_control.c
@@ -1,8 +1,8 @@
 /****************************************************************************
  *
  *   Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
- *   Author: Tobias Naegeli <nagelit@student.ethz.ch>
- *           Lorenz Meier <lm@inf.ethz.ch>
+ *   Author: @author Lorenz Meier <lm@inf.ethz.ch>
+ *           @author Tobias Naegeli <nagelit@student.ethz.ch>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -34,7 +34,8 @@
  ****************************************************************************/
 
 /*
- * @file Implementation of attitude rate control
+ * @file rate_control.c
+ * Implementation of attitude rate control
  */
 
 #include "rate_control.h"
diff --git a/apps/ardrone_control/rate_control.h b/apps/ardrone_control/rate_control.h
index bd3191c1c..61bd7bf38 100644
--- a/apps/ardrone_control/rate_control.h
+++ b/apps/ardrone_control/rate_control.h
@@ -1,8 +1,8 @@
 /****************************************************************************
  *
  *   Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
- *   Author: Tobias Naegeli <nagelit@student.ethz.ch>
- *           Lorenz Meier <lm@inf.ethz.ch>
+ *   Author: @author Lorenz Meier <lm@inf.ethz.ch>
+ *           @author Tobias Naegeli <nagelit@student.ethz.ch>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c
index 981520e7c..6745c0102 100644
--- a/apps/mavlink/mavlink.c
+++ b/apps/mavlink/mavlink.c
@@ -71,7 +71,7 @@
 #include <uORB/topics/vehicle_command.h>
 #include <uORB/topics/vehicle_local_position_setpoint.h>
 #include <uORB/topics/vehicle_global_position_setpoint.h>
- #include <uORB/topics/vehicle_attitude_setpoint.h>
+#include <uORB/topics/vehicle_attitude_setpoint.h>
 #include "waypoints.h"
 #include "mavlink_log.h"