mode switching for all platforms, additional fixed wing modes

5 files changed, 198 insertions, 115 deletions

diff --git a/apps/commander/commander.c b/apps/commander/commander.c
index 7277e9fa4..33ed5ebc2 100644
--- a/apps/commander/commander.c
+++ b/apps/commander/commander.c
@@ -1496,14 +1496,20 @@ int commander_thread_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);
 
-				if (sp_man.override_mode_switch > STICK_ON_OFF_LIMIT) {
+				if (sp_man.aux1_cam_pan_flaps < -STICK_ON_OFF_LIMIT) {
 					update_state_machine_mode_manual(stat_pub, &current_status, mavlink_fd);
+				} else {
+					if (sp_man.override_mode_switch > STICK_ON_OFF_LIMIT) {
+						//update_state_machine_mode_manual(stat_pub, &current_status, mavlink_fd);
+						// XXX hack
+						update_state_machine_mode_stabilized(stat_pub, &current_status, mavlink_fd);
 
-				} else if (sp_man.override_mode_switch < -STICK_ON_OFF_LIMIT) {
-					update_state_machine_mode_auto(stat_pub, &current_status, mavlink_fd);
+					} else if (sp_man.override_mode_switch < -STICK_ON_OFF_LIMIT) {
+						update_state_machine_mode_auto(stat_pub, &current_status, mavlink_fd);
 
-				} else {
-					update_state_machine_mode_stabilized(stat_pub, &current_status, mavlink_fd);
+					} else {
+						update_state_machine_mode_stabilized(stat_pub, &current_status, mavlink_fd);
+					}
 				}
 
 				/* handle the case where RC signal was regained */
diff --git a/apps/fixedwing_att_control/fixedwing_att_control_main.c b/apps/fixedwing_att_control/fixedwing_att_control_main.c
index 38f58ac33..9dd34b9ec 100644
--- a/apps/fixedwing_att_control/fixedwing_att_control_main.c
+++ b/apps/fixedwing_att_control/fixedwing_att_control_main.c
@@ -62,8 +62,8 @@
 #include <systemlib/param/param.h>
 #include <systemlib/pid/pid.h>
 #include <systemlib/geo/geo.h>
+#include <systemlib/perf_counter.h>
 #include <systemlib/systemlib.h>
-
 #include <fixedwing_att_control_rate.h>
 #include <fixedwing_att_control_att.h>
 
@@ -126,7 +126,7 @@ int fixedwing_att_control_thread_main(int argc, char *argv[])
 		}
 
 		/* welcome user */
-		printf("[fixedwing att_control] started\n");
+		printf("[fixedwing att control] started\n");
 
 		/* declare and safely initialize all structs */
 		struct vehicle_attitude_s att;
@@ -153,6 +153,7 @@ int fixedwing_att_control_thread_main(int argc, char *argv[])
 			actuators.control[i] = 0.0f;
 		}
 		orb_advert_t actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators);
+		orb_advert_t rates_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp);
 //		orb_advert_t debug_pub = orb_advertise(ORB_ID(debug_key_value), &debug_output);
 //		debug_output.key[0] = '1';
 
@@ -185,19 +186,51 @@ int fixedwing_att_control_thread_main(int argc, char *argv[])
 			/* Control */
 
 			if (vstatus.state_machine == SYSTEM_STATE_AUTO) {
-				/* Attitude Control */
-				fixedwing_att_control_attitude(&att_sp,
-						&att,
-						&rates_sp);
+				/* attitude control */
+				fixedwing_att_control_attitude(&att_sp, &att, &rates_sp);
+				/* publish rate setpoint */
+				orb_publish(ORB_ID(vehicle_rates_setpoint), rates_pub, &rates_sp);
 
-				/* Attitude Rate Control */
+				/* angular rate control */
 				fixedwing_att_control_rates(&rates_sp, gyro, &actuators);
 
-				//REMOVEME XXX
-				actuators.control[3] = 0.7f;
+				/* pass through throttle */
+				actuators.control[3] = att_sp.thrust;
+
+			} else if (vstatus.state_machine == SYSTEM_STATE_STABILIZED) {
+
+				/* if the RC signal is lost, try to stay level and go slowly back down to ground */
+				if(vstatus.rc_signal_lost) {
+					
+					// XXX define failsafe throttle param
+					//param_get(failsafe_throttle_handle, &failsafe_throttle);
+					att_sp.roll_body = 0.3f;
+					att_sp.pitch_body = 0.0f;
+					att_sp.thrust = 0.5f;
+
+					// 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();
+				}
+
+				fixedwing_att_control_attitude(&att_sp, &att, &rates_sp);
+
+				/* publish rate setpoint */
+				orb_publish(ORB_ID(vehicle_rates_setpoint), rates_pub, &rates_sp);
+
+				/* angular rate control */
+				fixedwing_att_control_rates(&rates_sp, gyro, &actuators);
+
+				/* pass through throttle */
+				actuators.control[3] = att_sp.thrust;
+
 
-	//			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;
@@ -224,6 +257,7 @@ int fixedwing_att_control_thread_main(int argc, char *argv[])
 
 		close(att_sub);
 		close(actuator_pub);
+		close(rates_pub);
 
 		fflush(stdout);
 		exit(0);
@@ -268,7 +302,7 @@ int fixedwing_att_control_main(int argc, char *argv[])
 		deamon_task = task_spawn("fixedwing_att_control",
 					 SCHED_DEFAULT,
 					 SCHED_PRIORITY_MAX - 20,
-					 4096,
+					 2048,
 					 fixedwing_att_control_thread_main,
 					 (argv) ? (const char **)&argv[2] : (const char **)NULL);
 		thread_running = true;
diff --git a/apps/fixedwing_att_control/fixedwing_att_control_rate.c b/apps/fixedwing_att_control/fixedwing_att_control_rate.c
index b81d50168..f46761922 100644
--- a/apps/fixedwing_att_control/fixedwing_att_control_rate.c
+++ b/apps/fixedwing_att_control/fixedwing_att_control_rate.c
@@ -1,7 +1,7 @@
 /****************************************************************************
  *
  *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ *   Author: Thomas Gubler <thomasgubler@student.ethz.ch>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -34,6 +34,8 @@
 /**
  * @file fixedwing_att_control_rate.c
  * Implementation of a fixed wing attitude controller.
+ *
+ * @author Thomas Gubler <thomasgubler@student.ethz.ch>
  */
 #include <fixedwing_att_control_rate.h>
 
@@ -171,8 +173,8 @@ int fixedwing_att_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
 	actuators->control[0] = pid_calculate(&roll_rate_controller, rate_sp->roll, rates[0], 0, deltaT);
 
 
-	actuators->control[1] = pid_calculate(&pitch_rate_controller, rate_sp->pitch, rates[1], 0, deltaT); //TODO: (-) sign comes from the elevator (positive --> deflection downwards), this has to be moved to the mixer...
-	actuators->control[2] = 0;//pid_calculate(&yaw_rate_controller, rate_sp->yaw, rates[2], 0, deltaT); 	//XXX TODO disabled for now
+	actuators->control[1] = pid_calculate(&pitch_rate_controller, rate_sp->pitch, rates[1], 0, deltaT);
+	actuators->control[2] = 0.0f;//pid_calculate(&yaw_rate_controller, rate_sp->yaw, rates[2], 0, deltaT); 	//XXX TODO disabled for now
 
 	counter++;
 
diff --git a/apps/fixedwing_pos_control/fixedwing_pos_control_main.c b/apps/fixedwing_pos_control/fixedwing_pos_control_main.c
index a70b9bd30..d9acd8db9 100644
--- a/apps/fixedwing_pos_control/fixedwing_pos_control_main.c
+++ b/apps/fixedwing_pos_control/fixedwing_pos_control_main.c
@@ -57,9 +57,11 @@
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/vehicle_rates_setpoint.h>
 #include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/parameter_update.h>
 #include <systemlib/param/param.h>
 #include <systemlib/pid/pid.h>
 #include <systemlib/geo/geo.h>
+#include <systemlib/perf_counter.h>
 #include <systemlib/systemlib.h>
 
 /*
@@ -162,16 +164,16 @@ static int parameters_update(const struct fw_pos_control_param_handles *h, struc
 int fixedwing_pos_control_thread_main(int argc, char *argv[])
 {
 	/* read arguments */
-		bool verbose = false;
+		// bool verbose = false;
 
-		for (int i = 1; i < argc; i++) {
-			if (strcmp(argv[i], "-v") == 0 || strcmp(argv[i], "--verbose") == 0) {
-				verbose = true;
-			}
-		}
+		// for (int i = 1; i < argc; i++) {
+		// 	if (strcmp(argv[i], "-v") == 0 || strcmp(argv[i], "--verbose") == 0) {
+		// 		verbose = true;
+		// 	}
+		// }
 
 		/* welcome user */
-		printf("[fixedwing att_control] started\n");
+		printf("[fixedwing pos control] started\n");
 
 		/* declare and safely initialize all structs */
 		struct vehicle_global_position_s global_pos;
@@ -184,6 +186,8 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[])
 		memset(&att, 0, sizeof(att));
 		struct crosstrack_error_s xtrack_err;
 		memset(&xtrack_err, 0, sizeof(xtrack_err));
+		struct parameter_update_s param_update;
+		memset(&param_update, 0, sizeof(param_update));
 		
 		/* output structs */
 		struct vehicle_attitude_setpoint_s attitude_setpoint;
@@ -193,129 +197,151 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[])
 		attitude_setpoint.roll_body = 0.0f;
 		attitude_setpoint.pitch_body = 0.0f;
 		attitude_setpoint.yaw_body = 0.0f;
+		attitude_setpoint.thrust = 0.0f;
 		orb_advert_t attitude_setpoint_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &attitude_setpoint);
 
 		/* subscribe */
 		int global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
 		int global_setpoint_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint));
 		int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
+		int param_sub = orb_subscribe(ORB_ID(parameter_update));
 
 		/* Setup of loop */
-		struct pollfd fds = { .fd = att_sub, .events = POLLIN };
+		struct pollfd fds[2] = { 
+			{ .fd = param_sub, .events = POLLIN },
+			{ .fd = att_sub, .events = POLLIN }
+		};
 		bool global_sp_updated_set_once = false;
 
 		float psi_track = 0.0f;
 
+		int counter = 0;
+
+		struct fw_pos_control_params p;
+		struct fw_pos_control_param_handles h;
+
+		PID_t heading_controller;
+		PID_t altitude_controller;
+
+		parameters_init(&h);
+		parameters_update(&h, &p);
+		pid_init(&heading_controller, p.heading_p, 0.0f, 0.0f, 0.0f,p.roll_lim,PID_MODE_DERIVATIV_NONE);
+		pid_init(&altitude_controller, p.altitude_p, 0.0f, 0.0f, 0.0f,p.pitch_lim,PID_MODE_DERIVATIV_NONE);
+
+		/* error and performance monitoring */
+		perf_counter_t fw_interval_perf = perf_alloc(PC_INTERVAL, "fixedwing_pos_control_interval");
+		perf_counter_t fw_err_perf = perf_alloc(PC_COUNT, "fixedwing_pos_control_err");
+
 		while(!thread_should_exit)
 		{
 			/* wait for a sensor update, check for exit condition every 500 ms */
-			poll(&fds, 1, 500);
-
-			static int counter = 0;
-			static bool initialized = false;
-
-			static struct fw_pos_control_params p;
-			static struct fw_pos_control_param_handles h;
+			int ret = poll(fds, 2, 500);
+
+			if (ret < 0) {
+				/* poll error, count it in perf */
+				perf_count(fw_err_perf);
+			} else if (ret == 0) {
+				/* no return value, ignore */
+			} else {
+
+				/* only update parameters if they changed */
+				if (fds[0].revents & POLLIN) {
+					/* read from param to clear updated flag */
+					struct parameter_update_s update;
+					orb_copy(ORB_ID(parameter_update), param_sub, &update);
+
+					/* update parameters from storage */
+					parameters_update(&h, &p);
+					pid_set_parameters(&heading_controller, p.heading_p, 0, 0, 0, p.roll_lim);
+					pid_set_parameters(&altitude_controller, p.altitude_p, 0, 0, 0, p.pitch_lim);
+				}
 
-			PID_t heading_controller;
-			PID_t altitude_controller;
+				/* only run controller if attitude changed */
+				if (fds[1].revents & POLLIN) {
 
-			if(!initialized)
-			{
-				parameters_init(&h);
-				parameters_update(&h, &p);
-				pid_init(&heading_controller, p.heading_p, 0.0f, 0.0f, 0.0f,p.roll_lim,PID_MODE_DERIVATIV_NONE);
-				pid_init(&altitude_controller, p.altitude_p, 0.0f, 0.0f, 0.0f,p.pitch_lim,PID_MODE_DERIVATIV_NONE);
-				initialized = true;
-			}
+					/* check if there is a new position or setpoint */
+					bool pos_updated;
+					orb_check(global_pos_sub, &pos_updated);
+					bool global_sp_updated;
+					orb_check(global_setpoint_sub, &global_sp_updated);
 
-			/* load new parameters with lower rate */
-			if (counter % 100 == 0) {
-				/* update parameters from storage */
-				parameters_update(&h, &p);
-				pid_set_parameters(&heading_controller, p.heading_p, 0, 0, 0, p.roll_lim);
-				pid_set_parameters(&altitude_controller, p.altitude_p, 0, 0, 0, p.pitch_lim);
+					/* load local copies */
+					orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
 
-			}
+					if (pos_updated) {
+						orb_copy(ORB_ID(vehicle_global_position), global_pos_sub, &global_pos);
+					}
 
-			/* Check if there is a new position or setpoint */
-			bool pos_updated;
-			orb_check(global_pos_sub, &pos_updated);
-			bool global_sp_updated;
-			orb_check(global_setpoint_sub, &global_sp_updated);
-
-			/* Load local copies */
-			orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
-			if(pos_updated)
-				orb_copy(ORB_ID(vehicle_global_position), global_pos_sub, &global_pos);
-			if (global_sp_updated)
-				orb_copy(ORB_ID(vehicle_global_position_setpoint), global_setpoint_sub, &global_setpoint);
-
-			if(global_sp_updated) {
-				start_pos = global_pos; //for now using the current position as the startpoint (= approx. last waypoint because the setpoint switch occurs at the waypoint)
-				global_sp_updated_set_once = true;
-				psi_track = get_bearing_to_next_waypoint((double)global_pos.lat / (double)1e7d, (double)global_pos.lon / (double)1e7d,
-						(double)global_setpoint.lat / (double)1e7d, (double)global_setpoint.lon / (double)1e7d);
-				printf("psi_track: %0.4f\n", (double)psi_track);
-			}
+					if (global_sp_updated) {
+						orb_copy(ORB_ID(vehicle_global_position_setpoint), global_setpoint_sub, &global_setpoint);
 
-			/* Control */
+						start_pos = global_pos; //for now using the current position as the startpoint (= approx. last waypoint because the setpoint switch occurs at the waypoint)
+						global_sp_updated_set_once = true;
+						psi_track = get_bearing_to_next_waypoint((double)global_pos.lat / (double)1e7d, (double)global_pos.lon / (double)1e7d,
+								(double)global_setpoint.lat / (double)1e7d, (double)global_setpoint.lon / (double)1e7d);
+						printf("psi_track: %0.4f\n", (double)psi_track);
+					}
 
+					/* simple horizontal control, execute if line between wps is known */
+					if(global_sp_updated_set_once)
+					{
+		//				if (counter % 100 == 0)
+		//					printf("lat_sp %d, ln_sp %d, lat: %d, lon: %d\n", global_setpoint.lat, global_setpoint.lon, global_pos.lat, global_pos.lon);
+						
+						/* calculate crosstrack error */
+						// Only the case of a straight line track following handled so far
+						 int distance_res = get_distance_to_line(&xtrack_err, (double)global_pos.lat / (double)1e7d, (double)global_pos.lon / (double)1e7d,
+								(double)start_pos.lat / (double)1e7d, (double)start_pos.lon / (double)1e7d,
+								(double)global_setpoint.lat / (double)1e7d, (double)global_setpoint.lon / (double)1e7d);
 
-			/* Simple Horizontal Control */
-			if(global_sp_updated_set_once)
-			{
-//				if (counter % 100 == 0)
-//					printf("lat_sp %d, ln_sp %d, lat: %d, lon: %d\n", global_setpoint.lat, global_setpoint.lon, global_pos.lat, global_pos.lon);
-				
-				/* calculate crosstrack error */
-				// Only the case of a straight line track following handled so far
-				 int distance_res = get_distance_to_line(&xtrack_err, (double)global_pos.lat / (double)1e7d, (double)global_pos.lon / (double)1e7d,
-						(double)start_pos.lat / (double)1e7d, (double)start_pos.lon / (double)1e7d,
-						(double)global_setpoint.lat / (double)1e7d, (double)global_setpoint.lon / (double)1e7d);
+						if(!(distance_res != OK || xtrack_err.past_end)) {
 
-				if(!(distance_res != OK || xtrack_err.past_end)) {
+							float delta_psi_c = -p.xtrack_p * xtrack_err.distance; //(-) because z axis points downwards
 
-					float delta_psi_c = -p.xtrack_p * xtrack_err.distance; //(-) because z axis points downwards
+							if(delta_psi_c > 60.0f*M_DEG_TO_RAD_F)
+								delta_psi_c = 60.0f*M_DEG_TO_RAD_F;
 
-					if(delta_psi_c > 60.0f*M_DEG_TO_RAD_F)
-						delta_psi_c = 60.0f*M_DEG_TO_RAD_F;
+							if(delta_psi_c < -60.0f*M_DEG_TO_RAD_F)
+								delta_psi_c = -60.0f*M_DEG_TO_RAD_F;
 
-					if(delta_psi_c < -60.0f*M_DEG_TO_RAD_F)
-						delta_psi_c = -60.0f*M_DEG_TO_RAD_F;
+							float psi_c = psi_track + delta_psi_c;
 
-					float psi_c = psi_track + delta_psi_c;
+							float psi_e = psi_c - att.yaw;
 
-					float psi_e = psi_c - att.yaw;
+							/* shift error to prevent wrapping issues */
+							psi_e = _wrap_pi(psi_e);
 
-					/* shift error to prevent wrapping issues */
-					psi_e = _wrap_pi(psi_e);
+							/* calculate roll setpoint, do this artificially around zero */
+							attitude_setpoint.roll_body = pid_calculate(&heading_controller, psi_e, 0.0f, 0.0f, 0.0f);
 
-					/* calculate roll setpoint, do this artificially around zero */
-					attitude_setpoint.roll_body = pid_calculate(&heading_controller, psi_e, 0.0f, 0.0f, 0.0f);
+		//					if (counter % 100 == 0)
+		//						printf("xtrack_err.distance: %0.4f, delta_psi_c: %0.4f\n",xtrack_err.distance, delta_psi_c);
+						}
+						else {
+							if (counter % 100 == 0)
+								printf("distance_res: %d, past_end %d\n", distance_res, xtrack_err.past_end);
+						}
 
-//					if (counter % 100 == 0)
-//						printf("xtrack_err.distance: %0.4f, delta_psi_c: %0.4f\n",xtrack_err.distance, delta_psi_c);
-				}
-				else {
-					if (counter % 100 == 0)
-						printf("distance_res: %d, past_end %d\n", distance_res, xtrack_err.past_end);
-				}
+						// XXX SIMPLE ALTITUDE, BUT NO SPEED CONTROL
+						if (pos_updated) {
+							//TODO: take care of relative vs. ab. altitude
+							attitude_setpoint.pitch_body = pid_calculate(&altitude_controller, global_setpoint.altitude, global_pos.alt, 0.0f, 0.0f);
+						}
 
-			}
+						// XXX need speed control
+						attitude_setpoint.thrust = 0.7f;
 
-			/* Very simple Altitude Control */
-			if(global_sp_updated_set_once && pos_updated)
-			{
+						/* publish the attitude setpoint */
+						orb_publish(ORB_ID(vehicle_attitude_setpoint), attitude_setpoint_pub, &attitude_setpoint);
 
-				//TODO: take care of relative vs. ab. altitude
-				attitude_setpoint.pitch_body = pid_calculate(&altitude_controller, global_setpoint.altitude, global_pos.alt, 0.0f, 0.0f);
+						/* measure in what intervals the controller runs */
+						perf_count(fw_interval_perf);
 
+					} else {
+						// XXX no setpoint, decent default needed (loiter?)
+					}
+				}
 			}
-			/*Publish the attitude setpoint */
-			orb_publish(ORB_ID(vehicle_attitude_setpoint), attitude_setpoint_pub, &attitude_setpoint);
-
-			counter++;
 		}
 
 		printf("[fixedwing_pos_control] exiting.\n");
@@ -367,7 +393,7 @@ int fixedwing_pos_control_main(int argc, char *argv[])
 		deamon_task = task_spawn("fixedwing_pos_control",
 					 SCHED_DEFAULT,
 					 SCHED_PRIORITY_MAX - 20,
-					 4096,
+					 2048,
 					 fixedwing_pos_control_thread_main,
 					 (argv) ? (const char **)&argv[2] : (const char **)NULL);
 		thread_running = true;
diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp
index 466284a1b..a633e012c 100644
--- a/apps/sensors/sensors.cpp
+++ b/apps/sensors/sensors.cpp
@@ -973,7 +973,7 @@ Sensors::ppm_poll()
 		}
 
 		/* yaw input - stick right is positive and positive rotation */
-		manual_control.yaw = _rc.chan[_rc.function[YAW]].scaled * _parameters.rc_scale_yaw;
+		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;
 		if (!isnan(_parameters.rc_scale_yaw) || !isinf(_parameters.rc_scale_yaw)) {
@@ -990,6 +990,21 @@ Sensors::ppm_poll()
 		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;
 
+		/* aux inputs */
+		manual_control.aux1_cam_pan_flaps = _rc.chan[_rc.function[FUNC_0]].scaled;
+		if (manual_control.aux1_cam_pan_flaps < -1.0f) manual_control.aux1_cam_pan_flaps = -1.0f;
+		if (manual_control.aux1_cam_pan_flaps >  1.0f) manual_control.aux1_cam_pan_flaps =  1.0f;
+
+		/* aux inputs */
+		manual_control.aux2_cam_tilt = _rc.chan[_rc.function[FUNC_1]].scaled;
+		if (manual_control.aux2_cam_tilt < -1.0f) manual_control.aux2_cam_tilt = -1.0f;
+		if (manual_control.aux2_cam_tilt >  1.0f) manual_control.aux2_cam_tilt =  1.0f;
+
+		/* aux inputs */
+		manual_control.aux3_cam_zoom = _rc.chan[_rc.function[FUNC_2]].scaled;
+		if (manual_control.aux3_cam_zoom < -1.0f) manual_control.aux3_cam_zoom = -1.0f;
+		if (manual_control.aux3_cam_zoom >  1.0f) manual_control.aux3_cam_zoom =  1.0f;
+
 		orb_publish(ORB_ID(rc_channels), _rc_pub, &_rc);
 		orb_publish(ORB_ID(manual_control_setpoint), _manual_control_pub, &manual_control);
 	}