Got rid of a bunch of magic numbers, manual controls can now be set up fine-grained

4 files changed, 43 insertions, 13 deletions

diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c
index eec42fa7b..3fb505174 100644
--- a/apps/multirotor_att_control/multirotor_att_control_main.c
+++ b/apps/multirotor_att_control/multirotor_att_control_main.c
@@ -140,9 +140,9 @@ mc_thread_main(int argc, char *argv[])
 		/* 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_rate_body = -manual.yaw * M_PI_F;
+		att_sp.roll_body = manual.roll;
+		att_sp.pitch_body = manual.pitch;
+		att_sp.yaw_rate_body = manual.yaw;
 		att_sp.timestamp = hrt_absolute_time();
 		
 		if (motor_test_mode) {
diff --git a/apps/multirotor_att_control/multirotor_attitude_control.c b/apps/multirotor_att_control/multirotor_attitude_control.c
index db2d28601..2e8c61fd2 100644
--- a/apps/multirotor_att_control/multirotor_attitude_control.c
+++ b/apps/multirotor_att_control/multirotor_attitude_control.c
@@ -60,17 +60,17 @@
 // PARAM_DEFINE_FLOAT(MC_YAWPOS_AWU, 1.0f);
 // PARAM_DEFINE_FLOAT(MC_YAWPOS_LIM, 3.0f);
 
-PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.1f); /* same on Flamewheel */
+PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.08f); /* same on Flamewheel */
 PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.02f);
 PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f);
 PARAM_DEFINE_FLOAT(MC_YAWRATE_AWU, 0.02f);
 PARAM_DEFINE_FLOAT(MC_YAWRATE_LIM, 0.1f);
 
-PARAM_DEFINE_FLOAT(MC_ATT_P, 0.3f); /* 0.15 F405 Flamewheel */
+PARAM_DEFINE_FLOAT(MC_ATT_P, 0.2f); /* 0.15 F405 Flamewheel */
 PARAM_DEFINE_FLOAT(MC_ATT_I, 0.0f);
-PARAM_DEFINE_FLOAT(MC_ATT_D, 0.1f); /* 0.025 F405 Flamewheel */
+PARAM_DEFINE_FLOAT(MC_ATT_D, 0.05f); /* 0.025 F405 Flamewheel */
 PARAM_DEFINE_FLOAT(MC_ATT_AWU, 0.05f);
-PARAM_DEFINE_FLOAT(MC_ATT_LIM, 0.3f);
+PARAM_DEFINE_FLOAT(MC_ATT_LIM, 0.4f);
 
 PARAM_DEFINE_FLOAT(MC_ATT_XOFF, 0.0f);
 PARAM_DEFINE_FLOAT(MC_ATT_YOFF, 0.0f);
diff --git a/apps/sensors/sensor_params.c b/apps/sensors/sensor_params.c
index fcd0608a4..370a2e0cc 100644
--- a/apps/sensors/sensor_params.c
+++ b/apps/sensors/sensor_params.c
@@ -126,3 +126,8 @@ PARAM_DEFINE_INT32(RC_MAP_PITCH, 2);
 PARAM_DEFINE_INT32(RC_MAP_THROTTLE, 3);
 PARAM_DEFINE_INT32(RC_MAP_YAW, 4);
 PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 5);
+
+PARAM_DEFINE_FLOAT(RC_SCALE_ROLL, 0.4f);
+PARAM_DEFINE_FLOAT(RC_SCALE_PITCH, 0.4f);
+PARAM_DEFINE_FLOAT(RC_SCALE_YAW, 3.0f);
+
diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp
index 7c6437608..7d72fb85d 100644
--- a/apps/sensors/sensors.cpp
+++ b/apps/sensors/sensors.cpp
@@ -193,6 +193,10 @@ private:
 		int rc_map_throttle;
 		int rc_map_mode_sw;
 
+		int rc_scale_roll;
+		int rc_scale_pitch;
+		int rc_scale_yaw;
+
 		float battery_voltage_scaling;
 	}		_parameters;			/**< local copies of interesting parameters */
 
@@ -214,6 +218,10 @@ private:
 		param_t rc_map_throttle;
 		param_t rc_map_mode_sw;
 
+		param_t rc_scale_roll;
+		param_t rc_scale_pitch;
+		param_t rc_scale_yaw;
+
 		param_t battery_voltage_scaling;
 	}		_parameter_handles;		/**< handles for interesting parameters */
 
@@ -384,6 +392,10 @@ Sensors::Sensors() :
 	_parameter_handles.rc_map_throttle = param_find("RC_MAP_THROTTLE");
 	_parameter_handles.rc_map_mode_sw = param_find("RC_MAP_MODE_SW");
 
+	_parameter_handles.rc_scale_roll = param_find("RC_SCALE_ROLL");
+	_parameter_handles.rc_scale_pitch = param_find("RC_SCALE_PITCH");
+	_parameter_handles.rc_scale_yaw = param_find("RC_SCALE_YAW");
+
 	/* gyro offsets */
 	_parameter_handles.gyro_offset[0] = param_find("SENS_GYRO_XOFF");
 	_parameter_handles.gyro_offset[1] = param_find("SENS_GYRO_YOFF");
@@ -492,6 +504,16 @@ Sensors::parameters_update()
 		warnx("Failed getting mode sw chan index");
 	}
 
+	if (param_get(_parameter_handles.rc_scale_roll, &(_parameters.rc_scale_roll)) != OK) {
+		warnx("Failed getting rc scaling for roll");
+	}
+	if (param_get(_parameter_handles.rc_scale_pitch, &(_parameters.rc_scale_pitch)) != OK) {
+		warnx("Failed getting rc scaling for pitch");
+	}
+	if (param_get(_parameter_handles.rc_scale_yaw, &(_parameters.rc_scale_yaw)) != OK) {
+		warnx("Failed getting rc scaling for yaw");
+	}
+
 	/* gyro offsets */
 	param_get(_parameter_handles.gyro_offset[0], &(_parameters.gyro_offset[0]));
 	param_get(_parameter_handles.gyro_offset[1], &(_parameters.gyro_offset[1]));
@@ -928,18 +950,21 @@ Sensors::ppm_poll()
 	_rc.chan_count = ppm_decoded_channels;
 	_rc.timestamp = ppm_last_valid_decode;
 
-	/* roll input */
-	manual_control.roll = _rc.chan[_rc.function[ROLL]].scaled;
+	/* roll input - rolling right is stick-wise and rotation-wise positive */
+	manual_control.roll = _rc.chan[_rc.function[ROLL]].scaled * _parameters.rc_scale_roll;
 	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;
+	/*
+	 * pitch input - stick down is negative, but stick down is pitching up (pos) in NED,
+	 * so reverse sign.
+	 */
+	manual_control.pitch = -1.0f * _rc.chan[_rc.function[PITCH]].scaled * _parameters.rc_scale_pitch;
 	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;
+	/* yaw input - stick right is positive and positive rotation */
+	manual_control.yaw = _rc.chan[_rc.function[YAW]].scaled * _parameters.rc_scale_yaw;
 	if (manual_control.yaw < -1.0f) manual_control.yaw = -1.0f;
 	if (manual_control.yaw >  1.0f) manual_control.yaw =  1.0f;