Added manual control abstraction layer, reworked sensors and ardrone_control apps to use it instead of direct RC channels

8 files changed, 254 insertions, 90 deletions

diff --git a/apps/ardrone_control/ardrone_control.c b/apps/ardrone_control/ardrone_control.c
index f80fdf971..f8c17fe2d 100644
--- a/apps/ardrone_control/ardrone_control.c
+++ b/apps/ardrone_control/ardrone_control.c
@@ -58,7 +58,7 @@
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/ardrone_control.h>
-#include <uORB/topics/rc_channels.h>
+#include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/ardrone_motors_setpoint.h>
 #include <uORB/topics/sensor_combined.h>
 
@@ -177,14 +177,14 @@ int ardrone_control_main(int argc, char *argv[])
 	struct vehicle_status_s state;
 	struct vehicle_attitude_s att;
 	struct ardrone_control_s ar_control;
-	struct rc_channels_s rc;
+	struct manual_control_setpoint_s manual;
 	struct sensor_combined_s raw;
 	struct ardrone_motors_setpoint_s setpoint;
 
 	/* subscribe to attitude, motor setpoints and system state */
 	int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
 	int state_sub = orb_subscribe(ORB_ID(vehicle_status));
-	int rc_sub = orb_subscribe(ORB_ID(rc_channels));
+	int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
 	int sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
 	int setpoint_sub = orb_subscribe(ORB_ID(ardrone_motors_setpoint));
 
@@ -194,8 +194,8 @@ int ardrone_control_main(int argc, char *argv[])
 	while (1) {
 		/* get a local copy of the vehicle state */
 		orb_copy(ORB_ID(vehicle_status), state_sub, &state);
-		/* get a local copy of rc */
-		orb_copy(ORB_ID(rc_channels), rc_sub, &rc);
+		/* 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);
 
@@ -208,7 +208,7 @@ int ardrone_control_main(int argc, char *argv[])
 				position_control_thread_started = true;
 			}
 
-			control_attitude(&rc, &att, &state, ardrone_pub, &ar_control);
+			control_attitude(0, 0, 0, 0, &att, &state, ardrone_pub, &ar_control);
 
 			//No check for remote sticks to disarm in auto mode, land/disarm with ground station
 
@@ -219,7 +219,8 @@ int ardrone_control_main(int argc, char *argv[])
 				control_rates(&raw, &setpoint);
 
 			} else if (control_mode == CONTROL_MODE_ATTITUDE) {
-				control_attitude(&rc, &att, &state, ardrone_pub, &ar_control);
+				control_attitude(manual.roll, manual.pitch, manual.yaw,
+					manual.throttle, &att, &state, ardrone_pub, &ar_control);
 			}
 
 		} else {
diff --git a/apps/ardrone_control/attitude_control.c b/apps/ardrone_control/attitude_control.c
index f96d22fe6..d9942fc73 100644
--- a/apps/ardrone_control/attitude_control.c
+++ b/apps/ardrone_control/attitude_control.c
@@ -60,13 +60,18 @@ extern int gpios;
 #define CONTROL_PID_ATTITUDE_INTERVAL	5e-3
 
 void turn_xy_plane(const float_vect3 *vector, float yaw,
+		   float_vect3 *result);
+void navi2body_xy_plane(const float_vect3 *vector, const float yaw,
+			float_vect3 *result);
+
+void turn_xy_plane(const float_vect3 *vector, float yaw,
 		   float_vect3 *result)
 {
 	//turn clockwise
 	static uint16_t counter;
 
-	result->x = (cos(yaw) * vector->x + sin(yaw) * vector->y);
-	result->y = (-sin(yaw) * vector->x + cos(yaw) * vector->y);
+	result->x = (cosf(yaw) * vector->x + sinf(yaw) * vector->y);
+	result->y = (-sinf(yaw) * vector->x + cosf(yaw) * vector->y);
 	result->z = vector->z; //leave direction normal to xy-plane untouched
 
 	counter++;
@@ -84,7 +89,7 @@ void navi2body_xy_plane(const float_vect3 *vector, const float yaw,
 	//	result->z = vector->z; //leave direction normal to xy-plane untouched
 }
 
-void control_attitude(const struct rc_channels_s *rc, const struct vehicle_attitude_s *att, const struct vehicle_status_s *status, int ardrone_pub, struct ardrone_control_s *ar_control)
+void control_attitude(float roll, float pitch, float yaw, float thrust, const struct vehicle_attitude_s *att, const struct vehicle_status_s *status, int ardrone_pub, struct ardrone_control_s *ar_control)
 {
 	static int motor_skip_counter = 0;
 
@@ -93,8 +98,13 @@ void control_attitude(const struct rc_channels_s *rc, const struct vehicle_attit
 	static PID_t nick_controller;
 	static PID_t roll_controller;
 
-	static const float min_gas = 10;
-	static const float max_gas = 512;
+	const float min_thrust = 0.02f;			/**< 2% minimum thrust */
+	const float max_thrust = 1.0f;			/**< 100% max thrust */
+	const float scaling = 512.0f;			/**< 100% thrust equals a value of 512 */
+
+	const float min_gas = min_thrust * scaling;
+	const float max_gas = max_thrust * scaling;
+
 	static uint16_t motor_pwm[4] = {0, 0, 0, 0};
 	static float motor_calc[4] = {0.0f, 0.0f, 0.0f, 0.0f};
 //	static float remote_control_weight_z = 1;
@@ -160,10 +170,10 @@ void control_attitude(const struct rc_channels_s *rc, const struct vehicle_attit
 	/* load new parameters with lower rate */
 	if (motor_skip_counter % 50 == 0) {
 		pid_set_parameters(&yaw_pos_controller,
-				   (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_P],
-				   (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_I],
-				   (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_D],
-				   (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_AWU]);
+				   global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_P],
+				   global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_I],
+				   global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_D],
+				   global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_AWU]);
 
 		pid_set_parameters(&yaw_speed_controller,
 				   (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_P],
@@ -189,7 +199,7 @@ void control_attitude(const struct rc_channels_s *rc, const struct vehicle_attit
 	}
 
 	current_state = status->state_machine;
-	float_vect3 attitude_setpoint_bodyframe = {}; //this is the setpoint in the bodyframe "mixed" together from the setpoint from the remote and the setpoint from the position controller
+	float_vect3 attitude_setpoint_bodyframe = {.x = 0.0f, .y = 0.0f, .z = 0.0f}; //this is the setpoint in the bodyframe "mixed" together from the setpoint from the remote and the setpoint from the position controller
 
 	if (current_state == SYSTEM_STATE_AUTO) {
 
@@ -200,15 +210,15 @@ void control_attitude(const struct rc_channels_s *rc, const struct vehicle_attit
 		float yaw_e = att->yaw - attitude_setpoint_navigationframe_from_positioncontroller.z;
 
 		// don't turn around the wrong side (only works if yaw angle is between +- 180 degree)
-		if (yaw_e > M_PI) {
+		if (yaw_e > M_PI_F) {
 			yaw_e -= 2.0f * M_PI_F;
 		}
 
-		if (yaw_e < -M_PI) {
+		if (yaw_e < -M_PI_F) {
 			yaw_e += 2.0f * M_PI_F;
 		}
 
-		attitude_setpoint_navigationframe_from_positioncontroller.z = pid_calculate(&yaw_pos_controller, 0, yaw_e, 0, CONTROL_PID_ATTITUDE_INTERVAL);
+		attitude_setpoint_navigationframe_from_positioncontroller.z = pid_calculate(&yaw_pos_controller, 0.0f, yaw_e, 0.0f, CONTROL_PID_ATTITUDE_INTERVAL);
 
 
 		/* limit control output */
@@ -234,9 +244,9 @@ void control_attitude(const struct rc_channels_s *rc, const struct vehicle_attit
 		attitude_setpoint_bodyframe.z = attitude_setpoint_bodyframe_from_positioncontroller.z;
 
 	} else if (current_state == SYSTEM_STATE_MANUAL) {
-		attitude_setpoint_bodyframe.x = -((float)rc->chan[rc->function[ROLL]].scale / 10000.0f) * M_PI_F / 8.0f;
-		attitude_setpoint_bodyframe.y = -((float)rc->chan[rc->function[PITCH]].scale / 10000.0f) * M_PI_F / 8.0f;
-		attitude_setpoint_bodyframe.z = -((float)rc->chan[rc->function[YAW]].scale / 10000.0f) * M_PI_F;
+		attitude_setpoint_bodyframe.x = -roll * M_PI_F / 8.0f;
+		attitude_setpoint_bodyframe.y = -pitch * M_PI_F / 8.0f;
+		attitude_setpoint_bodyframe.z = -yaw * M_PI_F;
 	}
 
 	/* add an attitude offset which needs to be estimated somewhere */
@@ -245,23 +255,23 @@ void control_attitude(const struct rc_channels_s *rc, const struct vehicle_attit
 
 	/*Calculate Controllers*/
 	//control Nick
-	float nick = pid_calculate(&nick_controller, attitude_setpoint_bodyframe.y, att->pitch, att->pitchspeed, CONTROL_PID_ATTITUDE_INTERVAL);
+	float nick_control = pid_calculate(&nick_controller, attitude_setpoint_bodyframe.y, att->pitch, att->pitchspeed, CONTROL_PID_ATTITUDE_INTERVAL);
 	//control Roll
-	float roll = pid_calculate(&roll_controller, attitude_setpoint_bodyframe.x, att->roll, att->rollspeed, CONTROL_PID_ATTITUDE_INTERVAL);
+	float roll_control = pid_calculate(&roll_controller, attitude_setpoint_bodyframe.x, att->roll, att->rollspeed, CONTROL_PID_ATTITUDE_INTERVAL);
 	//control Yaw Speed
-	float yaw = pid_calculate(&yaw_speed_controller, attitude_setpoint_bodyframe.z, att->yawspeed, 0, CONTROL_PID_ATTITUDE_INTERVAL); 	//attitude_setpoint_bodyframe.z is yaw speed!
+	float yaw_rate_control = pid_calculate(&yaw_speed_controller, attitude_setpoint_bodyframe.z, att->yawspeed, 0.0f, CONTROL_PID_ATTITUDE_INTERVAL); 	//attitude_setpoint_bodyframe.z is yaw speed!
 
 	//compensation to keep force in z-direction
 	float zcompensation;
 
-	if (fabs(att->roll) > 0.5f) {
+	if (fabsf(att->roll) > 0.5f) {
 		zcompensation = 1.13949393f;
 
 	} else {
 		zcompensation = 1.0f / cosf(att->roll);
 	}
 
-	if (fabs(att->pitch) > 0.5f) {
+	if (fabsf(att->pitch) > 0.5f) {
 		zcompensation *= 1.13949393f;
 
 	} else {
@@ -282,13 +292,13 @@ void control_attitude(const struct rc_channels_s *rc, const struct vehicle_attit
 
 	// FLYING MODES
 	if (current_state == SYSTEM_STATE_MANUAL) {
-		motor_thrust = (float)rc->chan[rc->function[THROTTLE]].scale / 20000.0f;
+		motor_thrust = thrust;
 
 	} else if (current_state == SYSTEM_STATE_GROUND_READY || current_state == SYSTEM_STATE_STABILIZED || current_state == SYSTEM_STATE_AUTO || current_state == SYSTEM_STATE_MISSION_ABORT) {
-		motor_thrust = (float)rc->chan[rc->function[THROTTLE]].scale / 20000.0f;	//TODO
+		motor_thrust = thrust;	//TODO
 
 	} else if (current_state == SYSTEM_STATE_EMCY_LANDING) {
-		motor_thrust = (float)rc->chan[rc->function[THROTTLE]].scale / 20000.0f;	//TODO
+		motor_thrust = thrust;	//TODO
 
 	} else if (current_state == SYSTEM_STATE_EMCY_CUTOFF) {
 		motor_thrust = 0;	//immediately cut off thrust!
@@ -301,39 +311,37 @@ void control_attitude(const struct rc_channels_s *rc, const struct vehicle_attit
 
 	// Convertion to motor-step units
 	motor_thrust *= zcompensation;
-	/* scale up from 0..1 to 10..512) */
-	motor_thrust *= ((float)max_gas - min_gas);
 
 	//limit control output
 	//yawspeed
-	if (yaw > pid_yawspeed_lim) {
-		yaw = pid_yawspeed_lim;
+	if (yaw_rate_control > pid_yawspeed_lim) {
+		yaw_rate_control = pid_yawspeed_lim;
 		yaw_speed_controller.saturated = 1;
 	}
 
-	if (yaw < -pid_yawspeed_lim) {
-		yaw = -pid_yawspeed_lim;
+	if (yaw_rate_control < -pid_yawspeed_lim) {
+		yaw_rate_control = -pid_yawspeed_lim;
 		yaw_speed_controller.saturated = 1;
 	}
 
-	if (nick > pid_att_lim) {
-		nick = pid_att_lim;
+	if (nick_control > pid_att_lim) {
+		nick_control = pid_att_lim;
 		nick_controller.saturated = 1;
 	}
 
-	if (nick < -pid_att_lim) {
-		nick = -pid_att_lim;
+	if (nick_control < -pid_att_lim) {
+		nick_control = -pid_att_lim;
 		nick_controller.saturated = 1;
 	}
 
 
-	if (roll > pid_att_lim) {
-		roll = pid_att_lim;
+	if (roll_control > pid_att_lim) {
+		roll_control = pid_att_lim;
 		roll_controller.saturated = 1;
 	}
 
-	if (roll < -pid_att_lim) {
-		roll = -pid_att_lim;
+	if (roll_control < -pid_att_lim) {
+		roll_control = -pid_att_lim;
 		roll_controller.saturated = 1;
 	}
 
@@ -342,44 +350,44 @@ void control_attitude(const struct rc_channels_s *rc, const struct vehicle_attit
 	ar_control->setpoint_attitude[0] = attitude_setpoint_bodyframe.x;
 	ar_control->setpoint_attitude[1] = attitude_setpoint_bodyframe.y;
 	ar_control->setpoint_attitude[2] = attitude_setpoint_bodyframe.z;
-	ar_control->attitude_control_output[0] = roll;
-	ar_control->attitude_control_output[1] = nick;
-	ar_control->attitude_control_output[2] = yaw;
+	ar_control->attitude_control_output[0] = roll_control;
+	ar_control->attitude_control_output[1] = nick_control;
+	ar_control->attitude_control_output[2] = yaw_rate_control;
 	ar_control->zcompensation = zcompensation;
 	orb_publish(ORB_ID(ardrone_control), ardrone_pub, ar_control);
 
-	static float output_band = 0.f;
-	static float band_factor = 0.75f;
-	static float startpoint_full_control = 150.0f;		//TODO
-	static float yaw_factor = 1.0f;
+	float output_band = 0.f;
+	float band_factor = 0.75f;
+	const float startpoint_full_control = 0.25f;	/**< start full control at 25% thrust */
+	float yaw_factor = 1.0f;
 
-	if (motor_thrust <= min_gas) {
-		motor_thrust = min_gas;
+	if (motor_thrust <= min_thrust) {
+		motor_thrust = min_thrust;
 		output_band = 0.f;
 
-	} else if (motor_thrust < startpoint_full_control && motor_thrust > min_gas) {
-		output_band = band_factor * (motor_thrust - min_gas);
+	} else if (motor_thrust < startpoint_full_control && motor_thrust > min_thrust) {
+		output_band = band_factor * (motor_thrust - min_thrust);
 
-	} else if (motor_thrust >= startpoint_full_control && motor_thrust < max_gas - band_factor * startpoint_full_control) {
+	} else if (motor_thrust >= startpoint_full_control && motor_thrust < max_thrust - band_factor * startpoint_full_control) {
 		output_band = band_factor * startpoint_full_control;
 
-	} else if (motor_thrust >= max_gas - band_factor * startpoint_full_control) {
-		output_band = band_factor * (max_gas - motor_thrust);
+	} else if (motor_thrust >= max_thrust - band_factor * startpoint_full_control) {
+		output_band = band_factor * (max_thrust - motor_thrust);
 	}
 
 	//add the yaw, nick and roll components to the basic thrust //TODO:this should be done by the mixer
 
 	// FRONT (MOTOR 1)
-	motor_calc[0] = motor_thrust + (roll / 2 + nick / 2 - yaw);
+	motor_calc[0] = motor_thrust + (roll_control / 2 + nick_control / 2 - yaw_rate_control);
 
 	// RIGHT (MOTOR 2)
-	motor_calc[1] = motor_thrust + (-roll / 2 + nick / 2 + yaw);
+	motor_calc[1] = motor_thrust + (-roll_control / 2 + nick_control / 2 + yaw_rate_control);
 
 	// BACK (MOTOR 3)
-	motor_calc[2] = motor_thrust + (-roll / 2 - nick / 2 - yaw);
+	motor_calc[2] = motor_thrust + (-roll_control / 2 - nick_control / 2 - yaw_rate_control);
 
 	// LEFT (MOTOR 4)
-	motor_calc[3] = motor_thrust + (roll / 2 - nick / 2 + yaw);
+	motor_calc[3] = motor_thrust + (roll_control / 2 - nick_control / 2 + yaw_rate_control);
 
 	// if we are not in the output band
 	if (!(motor_calc[0] < motor_thrust + output_band && motor_calc[0] > motor_thrust - output_band
@@ -389,16 +397,16 @@ void control_attitude(const struct rc_channels_s *rc, const struct vehicle_attit
 
 		yaw_factor = 0.5f;
 		// FRONT (MOTOR 1)
-		motor_calc[0] = motor_thrust + (roll / 2 + nick / 2 - yaw * yaw_factor);
+		motor_calc[0] = motor_thrust + (roll_control / 2 + nick_control / 2 - yaw_rate_control * yaw_factor);
 
 		// RIGHT (MOTOR 2)
-		motor_calc[1] = motor_thrust + (-roll / 2 + nick / 2 + yaw * yaw_factor);
+		motor_calc[1] = motor_thrust + (-roll_control / 2 + nick_control / 2 + yaw_rate_control * yaw_factor);
 
 		// BACK (MOTOR 3)
-		motor_calc[2] = motor_thrust + (-roll / 2 - nick / 2 - yaw * yaw_factor);
+		motor_calc[2] = motor_thrust + (-roll_control / 2 - nick_control / 2 - yaw_rate_control * yaw_factor);
 
 		// LEFT (MOTOR 4)
-		motor_calc[3] = motor_thrust + (roll / 2 - nick / 2 + yaw * yaw_factor);
+		motor_calc[3] = motor_thrust + (roll_control / 2 - nick_control / 2 + yaw_rate_control * yaw_factor);
 	}
 
 	uint8_t i;
@@ -414,14 +422,17 @@ void control_attitude(const struct rc_channels_s *rc, const struct vehicle_attit
 		}
 	}
 
-	// Write out actual thrust
-	motor_pwm[0] = (uint16_t) motor_calc[0];
-	motor_pwm[1] = (uint16_t) motor_calc[1];
-	motor_pwm[2] = (uint16_t) motor_calc[2];
-	motor_pwm[3] = (uint16_t) motor_calc[3];
+	/* set the motor values */
+
+	/* scale up from 0..1 to 10..512) */
+	motor_pwm[0] = (uint16_t) motor_calc[0] * ((float)max_gas - min_gas) + min_gas;
+	motor_pwm[1] = (uint16_t) motor_calc[1] * ((float)max_gas - min_gas) + min_gas;
+	motor_pwm[2] = (uint16_t) motor_calc[2] * ((float)max_gas - min_gas) + min_gas;
+	motor_pwm[3] = (uint16_t) motor_calc[3] * ((float)max_gas - min_gas) + min_gas;
 
-	// Keep motors spinning while armed
+	/* Keep motors spinning while armed and prevent overflows */
 
+	/* Failsafe logic - should never be necessary */
 	motor_pwm[0] = (motor_pwm[0] > 0) ? motor_pwm[0] : 10;
 	motor_pwm[1] = (motor_pwm[1] > 0) ? motor_pwm[1] : 10;
 	motor_pwm[2] = (motor_pwm[2] > 0) ? motor_pwm[2] : 10;
diff --git a/apps/ardrone_control/attitude_control.h b/apps/ardrone_control/attitude_control.h
index 6f66926c0..a93a511ba 100644
--- a/apps/ardrone_control/attitude_control.h
+++ b/apps/ardrone_control/attitude_control.h
@@ -45,7 +45,7 @@
 #include <uORB/topics/ardrone_control.h>
 #include <uORB/topics/vehicle_status.h>
 
-void control_attitude(const struct rc_channels_s *rc, const struct vehicle_attitude_s *att,
+void control_attitude(float roll, float pitch, float yaw, float thrust, const struct vehicle_attitude_s *att,
 		      const struct vehicle_status_s *status, int ardrone_pub,
 		      struct ardrone_control_s *ar_control);
 
diff --git a/apps/commander/commander.c b/apps/commander/commander.c
index dc5647223..3663503d1 100644
--- a/apps/commander/commander.c
+++ b/apps/commander/commander.c
@@ -631,8 +631,10 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
 
 }
 
-
-static void *command_handling_loop(void *arg)  //handles commands which come from the mavlink app
+/**
+ * Handle commands sent by the ground control station via MAVLink.
+ */
+static void *command_handling_loop(void *arg)
 {
 	/* Set thread name */
 	prctl(PR_SET_NAME, "commander cmd handler", getpid());
@@ -645,7 +647,7 @@ static void *command_handling_loop(void *arg)  //handles commands which come fro
 		struct pollfd fds[1] = { { .fd = cmd_sub, .events = POLLIN } };
 
 		if (poll(fds, 1, 5000) == 0) {
-			/* timeout, but this is no problem */
+			/* timeout, but this is no problem, silently ignore */
 		} else {
 			/* got command */
 			orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd);
@@ -785,7 +787,8 @@ int commander_main(int argc, char *argv[])
 	stat_pub = orb_advertise(ORB_ID(vehicle_status), &current_status);
 
 	if (stat_pub < 0) {
-		printf("[commander] ERROR: orb_advertise failed.\n");
+		printf("[commander] ERROR: orb_advertise for topic vehicle_status failed.\n");
+		exit(ERROR);
 	}
 
 	/* make sure we are in preflight state */
@@ -1035,6 +1038,8 @@ int commander_main(int argc, char *argv[])
 			int16_t rc_yaw_scale =  rc.chan[rc.function[YAW]].scale;
 			int16_t rc_throttle_scale =  rc.chan[rc.function[THROTTLE]].scale;
 			int16_t mode_switch_rc_value = rc.chan[rc.function[OVERRIDE]].scale;
+			/* Check the value of the rc channel of the mode switch */
+			mode_switch_rc_value = rc.chan[rc.function[OVERRIDE]].scale;
 
 			/* check if left stick is in lower left position --> switch to standby state */
 			if (rc_yaw_scale < -STICK_ON_OFF_LIMIT && rc_throttle_scale < STICK_THRUST_RANGE*0.2f) { //TODO: remove hardcoded values
@@ -1061,9 +1066,6 @@ int commander_main(int argc, char *argv[])
 			}
 			//printf("RC: y:%i/t:%i s:%i chans: %i\n", rc_yaw_scale, rc_throttle_scale, mode_switch_rc_value, rc.chan_count);
 
-			/* Check the value of the rc channel of the mode switch */
-			mode_switch_rc_value = rc.chan[rc.function[OVERRIDE]].scale;
-
 			if (mode_switch_rc_value > STICK_ON_OFF_LIMIT) {
 				update_state_machine_mode_manual(stat_pub, &current_status);
 
@@ -1074,6 +1076,9 @@ int commander_main(int argc, char *argv[])
 				update_state_machine_mode_stabilized(stat_pub, &current_status);
 			}
 
+			/* Publish RC signal */
+
+
 			/* handle the case where RC signal was regained */
 			if (current_status.rc_signal_lost) mavlink_log_critical(mavlink_fd, "[commander] RECOVERY - RC SIGNAL GAINED!");
 			current_status.rc_signal_lost = false;
diff --git a/apps/sensors/sensors.c b/apps/sensors/sensors.c
index b456c4dfb..098db4456 100644
--- a/apps/sensors/sensors.c
+++ b/apps/sensors/sensors.c
@@ -63,6 +63,7 @@
 #include <uORB/uORB.h>
 #include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/rc_channels.h>
+#include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/vehicle_status.h>
 
 #include "sensors.h"
@@ -383,10 +384,23 @@ int sensors_main(int argc, char *argv[])
 	pthread_mutex_init(&sensors_read_ready_mutex, NULL);
 	pthread_cond_init(&sensors_read_ready, NULL);
 
-	/* advertise the topic and make the initial publication */
+	/* advertise the sensor_combined topic and make the initial publication */
 	sensor_pub = orb_advertise(ORB_ID(sensor_combined), &raw);
 	publishing = true;
 
+	/* advertise the manual_control topic */
+	struct manual_control_setpoint_s manual_control = { .mode = ROLLPOS_PITCHPOS_YAWRATE_THROTTLE,
+						   .roll = 0.0f,
+						   .pitch = 0.0f,
+						   .yaw = 0.0f,
+						   .throttle = 0.0f };
+
+	int manual_control_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual_control);
+
+	if (manual_control_pub < 0) {
+		printf("[sensors] ERROR: orb_advertise for topic manual_control_setpoint failed.\n");
+	}
+
 	/* advertise the rc topic */
 	struct rc_channels_s rc;
 	memset(&rc, 0, sizeof(rc));
@@ -451,23 +465,23 @@ int sensors_main(int argc, char *argv[])
 
 
 				/* Update RC scalings and function mappings */
-				rc.chan[0].scaling_factor = (10000 / ((global_data_parameter_storage->pm.param_values[PARAM_RC1_MAX] - global_data_parameter_storage->pm.param_values[PARAM_RC1_MIN]) / 2)
+				rc.chan[0].scaling_factor = (1 / ((global_data_parameter_storage->pm.param_values[PARAM_RC1_MAX] - global_data_parameter_storage->pm.param_values[PARAM_RC1_MIN]) / 2)
 							     * global_data_parameter_storage->pm.param_values[PARAM_RC1_REV]);
 				rc.chan[0].mid = (uint16_t)global_data_parameter_storage->pm.param_values[PARAM_RC1_TRIM];
 
-				rc.chan[1].scaling_factor = (10000 / ((global_data_parameter_storage->pm.param_values[PARAM_RC2_MAX] - global_data_parameter_storage->pm.param_values[PARAM_RC2_MIN]) / 2)
+				rc.chan[1].scaling_factor = (1 / ((global_data_parameter_storage->pm.param_values[PARAM_RC2_MAX] - global_data_parameter_storage->pm.param_values[PARAM_RC2_MIN]) / 2)
 							     * global_data_parameter_storage->pm.param_values[PARAM_RC2_REV]);
 				rc.chan[1].mid = (uint16_t)global_data_parameter_storage->pm.param_values[PARAM_RC2_TRIM];
 
-				rc.chan[2].scaling_factor = (10000 / ((global_data_parameter_storage->pm.param_values[PARAM_RC3_MAX] - global_data_parameter_storage->pm.param_values[PARAM_RC3_MIN]) / 2)
+				rc.chan[2].scaling_factor = (1 / ((global_data_parameter_storage->pm.param_values[PARAM_RC3_MAX] - global_data_parameter_storage->pm.param_values[PARAM_RC3_MIN]) / 2)
 							     * global_data_parameter_storage->pm.param_values[PARAM_RC3_REV]);
 				rc.chan[2].mid = (uint16_t)global_data_parameter_storage->pm.param_values[PARAM_RC3_TRIM];
 
-				rc.chan[3].scaling_factor = (10000 / ((global_data_parameter_storage->pm.param_values[PARAM_RC4_MAX] - global_data_parameter_storage->pm.param_values[PARAM_RC4_MIN]) / 2)
+				rc.chan[3].scaling_factor = (1 / ((global_data_parameter_storage->pm.param_values[PARAM_RC4_MAX] - global_data_parameter_storage->pm.param_values[PARAM_RC4_MIN]) / 2)
 							     * global_data_parameter_storage->pm.param_values[PARAM_RC4_REV]);
 				rc.chan[3].mid = (uint16_t)global_data_parameter_storage->pm.param_values[PARAM_RC4_TRIM];
 
-				rc.chan[4].scaling_factor = (10000 / ((global_data_parameter_storage->pm.param_values[PARAM_RC5_MAX] - global_data_parameter_storage->pm.param_values[PARAM_RC5_MIN]) / 2)
+				rc.chan[4].scaling_factor = (1 / ((global_data_parameter_storage->pm.param_values[PARAM_RC5_MAX] - global_data_parameter_storage->pm.param_values[PARAM_RC5_MIN]) / 2)
 							     * global_data_parameter_storage->pm.param_values[PARAM_RC5_REV]);
 				rc.chan[4].mid = (uint16_t)global_data_parameter_storage->pm.param_values[PARAM_RC5_TRIM];
 
@@ -708,20 +722,48 @@ int sensors_main(int argc, char *argv[])
 
 				/* require at least two channels
 				 * to consider the signal valid
+				 * check that decoded measurement is up to date
 				 */
 				if (ppm_decoded_channels > 1 && (hrt_absolute_time() - ppm_last_valid_decode) < 45000) {
 					/* Read out values from HRT */
 					for (int i = 0; i < ppm_decoded_channels; i++) {
 						rc.chan[i].raw = ppm_buffer[i];
 						/* Set the range to +-, then scale up */
-						rc.chan[i].scale = (ppm_buffer[i] - rc.chan[i].mid) * rc.chan[i].scaling_factor;
+						rc.chan[i].scale = (ppm_buffer[i] - rc.chan[i].mid) * rc.chan[i].scaling_factor * 10000;
+						rc.chan[i].scaled = (ppm_buffer[i] - rc.chan[i].mid) * rc.chan[i].scaling_factor;
 					}
 
 					rc.chan_count = ppm_decoded_channels;
-
 					rc.timestamp = ppm_last_valid_decode;
+
 					/* publish a few lines of code later if set to true */
 					ppm_updated = true;
+
+					/* roll input */
+					manual_control.roll = rc.chan[rc.function[ROLL]].scaled;
+					if (manual_control.roll < -1.0f) manual_control.roll = -1.0f;
+					if (manual_control.roll >  1.0f) manual_control.roll =  1.0f;
+
+					/* pitch input */
+					manual_control.pitch = rc.chan[rc.function[PITCH]].scaled;
+					if (manual_control.pitch < -1.0f) manual_control.pitch = -1.0f;
+					if (manual_control.pitch >  1.0f) manual_control.pitch =  1.0f;
+
+					/* yaw input */
+					manual_control.yaw = rc.chan[rc.function[YAW]].scaled;
+					if (manual_control.yaw < -1.0f) manual_control.yaw = -1.0f;
+					if (manual_control.yaw >  1.0f) manual_control.yaw =  1.0f;
+					
+					/* throttle input */
+					manual_control.throttle = rc.chan[rc.function[THROTTLE]].scaled;
+					if (manual_control.throttle < 0.0f) manual_control.throttle = 0.0f;
+					if (manual_control.throttle > 2.0f) manual_control.throttle = 2.0f;
+
+					/* mode switch input */
+					manual_control.override_mode_switch = rc.chan[rc.function[OVERRIDE]].scaled;
+					if (manual_control.override_mode_switch < -1.0f) manual_control.override_mode_switch = -1.0f;
+					if (manual_control.override_mode_switch >  1.0f) manual_control.override_mode_switch =  1.0f;
+
 				}
 				ppmcounter = 0;
 			}
@@ -832,6 +874,7 @@ int sensors_main(int argc, char *argv[])
 
 			if (ppm_updated) {
 				orb_publish(ORB_ID(rc_channels), rc_pub, &rc);
+				orb_publish(ORB_ID(manual_control_setpoint), manual_control_pub, &manual_control);
 			}
 
 #endif
diff --git a/apps/uORB/objects_common.cpp b/apps/uORB/objects_common.cpp
index 98e6c2987..2b12939a3 100644
--- a/apps/uORB/objects_common.cpp
+++ b/apps/uORB/objects_common.cpp
@@ -100,7 +100,10 @@ ORB_DEFINE(vehicle_local_position_setpoint, struct vehicle_local_position_setpoi
 ORB_DEFINE(vehicle_global_position_setpoint, struct vehicle_global_position_setpoint_s);
 
 #include "topics/vehicle_attitude_setpoint.h"
-ORB_DEFINE(vehicle_attitude_setpoint, struct vehicle_attitude_s);
+ORB_DEFINE(vehicle_attitude_setpoint, struct vehicle_attitude_setpoint_s);
+
+#include "topics/manual_control_setpoint.h"
+ORB_DEFINE(manual_control_setpoint, struct manual_control_setpoint_s);
 
 #include "topics/actuator_controls.h"
 ORB_DEFINE(actuator_controls_0, struct actuator_controls_s);
diff --git a/apps/uORB/topics/manual_control_setpoint.h b/apps/uORB/topics/manual_control_setpoint.h
new file mode 100644
index 000000000..b01c7438d
--- /dev/null
+++ b/apps/uORB/topics/manual_control_setpoint.h
@@ -0,0 +1,95 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
+ *   Author: @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
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file manual_control_setpoint.h
+ * Definition of the manual_control_setpoint uORB topic.
+ */
+
+#ifndef TOPIC_MANUAL_CONTROL_SETPOINT_H_
+#define TOPIC_MANUAL_CONTROL_SETPOINT_H_
+
+#include <stdint.h>
+#include "../uORB.h"
+
+/**
+ * @addtogroup topics
+ * @{
+ */
+
+/**
+ * Defines how RC channels map to control inputs.
+ *
+ * The default mode on quadrotors and fixed wing is
+ * roll and pitch position of the right stick and
+ * throttle and yaw rate on the left stick
+ */
+enum MANUAL_CONTROL_MODE
+{
+  DIRECT = 0,
+  ROLLPOS_PITCHPOS_YAWRATE_THROTTLE = 1,
+	ROLLRATE_PITCHRATE_YAWRATE_THROTTLE = 2,
+  ROLLPOS_PITCHPOS_YAWPOS_THROTTLE = 3
+};
+
+struct manual_control_setpoint_s {
+
+  enum MANUAL_CONTROL_MODE mode;     /**< The current control inputs mode */
+  float roll;                   /**< roll / roll rate input */
+  float pitch;
+  float yaw;
+  float throttle;
+
+  float override_mode_switch;
+
+  float ailerons;
+  float elevator;
+  float rudder;
+  float flaps;
+
+  float aux1_cam_pan;
+  float aux2_cam_tilt;
+  float aux3_cam_zoom;
+  float aux4_cam_roll;
+
+}; /**< manual control inputs */
+
+/**
+ * @}
+ */
+
+/* register this as object request broker structure */
+ORB_DECLARE(manual_control_setpoint);
+
+#endif
diff --git a/apps/uORB/topics/vehicle_attitude_setpoint.h b/apps/uORB/topics/vehicle_attitude_setpoint.h
index 6b3757402..6a1908007 100644
--- a/apps/uORB/topics/vehicle_attitude_setpoint.h
+++ b/apps/uORB/topics/vehicle_attitude_setpoint.h
@@ -61,8 +61,14 @@ struct vehicle_attitude_setpoint_s
 	float yaw_tait_bryan;			/**< Tait-Bryan angle in NED frame		*/
 	float tait_bryan_valid;			/**< Set to true if Tait-Bryan angles are valid */
 
+	float roll_body;				/**< body angle in NED frame		*/
+	float pitch_body;				/**< body angle in NED frame		*/
+	float yaw_body;					/**< body angle in NED frame		*/
+	float body_valid;				/**< Set to true if Tait-Bryan angles are valid */
+
 	float R_body[9];				/**< Rotation matrix describing the setpoint as rotation from the current body frame */
 	bool R_valid;					/**< Set to true if rotation matrix is valid */
+
 	float thrust;					/**< Thrust in Newton the power system should generate */
 
 };