3 files changed, 48 insertions, 23 deletions

diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c
index 1f745baac..bf0ed346e 100644
--- a/apps/mavlink/mavlink.c
+++ b/apps/mavlink/mavlink.c
@@ -1415,10 +1415,10 @@ void handleMessage(mavlink_message_t *msg)
 			memset(&rc_hil, 0, sizeof(rc_hil));
 			static orb_advert_t rc_pub = 0;
 
-			rc_hil.chan[0].raw = 1510 + man.x * 500;
-			rc_hil.chan[1].raw = 1520 + man.y * 500;
-			rc_hil.chan[2].raw = 1590 + man.r * 500;
-			rc_hil.chan[3].raw = 1420 + man.z * 500;
+			rc_hil.chan[0].raw = 1510 + man.x / 2;
+			rc_hil.chan[1].raw = 1520 + man.y / 2;
+			rc_hil.chan[2].raw = 1590 + man.r / 2;
+			rc_hil.chan[3].raw = 1420 + man.z / 2;
 
 			rc_hil.chan[0].scaled = man.x;
 			rc_hil.chan[1].scaled = man.y;
@@ -1428,10 +1428,10 @@ void handleMessage(mavlink_message_t *msg)
 			struct manual_control_setpoint_s mc;
 			static orb_advert_t mc_pub = 0;
 
-			mc.roll = man.x*1000;
-			mc.pitch = man.y*1000;
-			mc.yaw = man.r*1000;
-			mc.roll = man.z*1000;
+			mc.roll = man.x / 1000.0f;
+			mc.pitch = man.y / 1000.0f;
+			mc.yaw = man.r / 1000.0f;
+			mc.roll = man.z / 1000.0f;
 
 			/* fake RC channels with manual control input from simulator */
 
diff --git a/apps/systemlib/pid/pid.c b/apps/systemlib/pid/pid.c
index fbf3edc33..61dd5757f 100644
--- a/apps/systemlib/pid/pid.c
+++ b/apps/systemlib/pid/pid.c
@@ -58,13 +58,36 @@ __EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax,
 	pid->error_previous = 0;
 	pid->integral = 0;
 }
-__EXPORT void pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax)
+__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax)
 {
-	pid->kp = kp;
-	pid->ki = ki;
-	pid->kd = kd;
-	pid->intmax = intmax;
+	int ret = 0;
+
+	if (isfinite(kp)) {
+		pid->kp = kp;
+	} else {
+		ret = 1;
+	}
+
+	if (isfinite(ki)) {
+		pid->ki = ki;
+	} else {
+		ret = 1;
+	}
+
+	if (isfinite(kd)) {
+		pid->kd = kd;
+	} else {
+		ret = 1;
+	}
+
+	if (isfinite(intmax)) {
+		pid->intmax = intmax;
+	}  else {
+		ret = 1;
+	}
+
 	// pid->limit = limit;
+	return ret;
 }
 
 //void pid_set(PID_t *pid, float sp)
@@ -133,19 +156,21 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo
 		d = -val_dot;
 
 	} else {
-		d = 0;
+		d = 0.0f;
 	}
 
-	if (pid->kd == 0) {
-		d = 0;
+	if (pid->kd == 0.0f) {
+		d = 0.0f;
 	}
 
-	if (pid->ki == 0) {
+	if (pid->ki == 0.0f) {
 		i = 0;
 	}
 
-	if (pid->kp == 0) {
-		p = 0;
+	float p;
+
+	if (pid->kp == 0.0f) {
+		p = 0.0f;
 	} else {
 		p = error;
 	}
@@ -154,10 +179,10 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo
 		pid->error_previous = error;
 	}
 
-	float val = (error * pid->kp) + (i * pid->ki) + (d * pid->kd);
+	float output = (error * pid->kp) + (i * pid->ki) + (d * pid->kd);
 
-	if (isfinite(val)) {
-		last_output = val;
+	if (isfinite(output)) {
+		pid->last_output = output;
 	}
 
 	if (!isfinite(pid->integral)) {
diff --git a/apps/systemlib/pid/pid.h b/apps/systemlib/pid/pid.h
index d4bbcaf31..098b2bef8 100644
--- a/apps/systemlib/pid/pid.h
+++ b/apps/systemlib/pid/pid.h
@@ -66,7 +66,7 @@ typedef struct {
 } PID_t;
 
 __EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, uint8_t mode);
-__EXPORT void pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax);
+__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax);
 //void pid_set(PID_t *pid, float sp);
 __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt);