Fix bugs found through compiler warnings

10 files changed, 33 insertions, 14 deletions

diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp
index 5954c40da..493678ae5 100644
--- a/src/drivers/mkblctrl/mkblctrl.cpp
+++ b/src/drivers/mkblctrl/mkblctrl.cpp
@@ -131,8 +131,8 @@ public:
 	int		set_motor_count(unsigned count);
 	int		set_motor_test(bool motortest);
 	int		set_overrideSecurityChecks(bool overrideSecurityChecks);
-	int		set_px4mode(int px4mode);
-	int		set_frametype(int frametype);
+	void	set_px4mode(int px4mode);
+	void	set_frametype(int frametype);
 	unsigned int		mk_check_for_blctrl(unsigned int count, bool showOutput, bool initI2C);
 
 private:
@@ -330,13 +330,13 @@ MK::set_update_rate(unsigned rate)
 	return OK;
 }
 
-int
+void
 MK::set_px4mode(int px4mode)
 {
 	_px4mode = px4mode;
 }
 
-int
+void
 MK::set_frametype(int frametype)
 {
 	_frametype = frametype;
diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp
index 8a4bfa18c..84ea9a3bc 100644
--- a/src/drivers/px4fmu/fmu.cpp
+++ b/src/drivers/px4fmu/fmu.cpp
@@ -741,7 +741,7 @@ PX4FMU::task_main()
 	}
 
 	for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) {
-		if (_control_subs > 0) {
+		if (_control_subs[i] > 0) {
 			::close(_control_subs[i]);
 			_control_subs[i] = -1;
 		}
diff --git a/src/drivers/roboclaw/RoboClaw.cpp b/src/drivers/roboclaw/RoboClaw.cpp
index dd5e4d3e0..b0a8527c2 100644
--- a/src/drivers/roboclaw/RoboClaw.cpp
+++ b/src/drivers/roboclaw/RoboClaw.cpp
@@ -182,7 +182,10 @@ float RoboClaw::getMotorPosition(e_motor motor)
 		return _motor1Position;
 	} else if (motor == MOTOR_2) {
 		return _motor2Position;
-	}
+	} else {
+        warnx("Unknown motor value passed to RoboClaw::getMotorPosition");
+        return NAN;
+    }
 }
 
 float RoboClaw::getMotorSpeed(e_motor motor)
@@ -191,7 +194,10 @@ float RoboClaw::getMotorSpeed(e_motor motor)
 		return _motor1Speed;
 	} else if (motor == MOTOR_2) {
 		return _motor2Speed;
-	}
+	} else {
+        warnx("Unknown motor value passed to RoboClaw::getMotorPosition");
+        return NAN;
+    }
 }
 
 int RoboClaw::setMotorSpeed(e_motor motor, float value)
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 1535967b1..3cd250458 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -603,6 +603,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 		// XXX TODO
 	}
 
+    return true;
 }
 
 int commander_thread_main(int argc, char *argv[])
diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp
index 1320b4668..f8b9e9fbd 100644
--- a/src/modules/ekf_att_pos_estimator/estimator.cpp
+++ b/src/modules/ekf_att_pos_estimator/estimator.cpp
@@ -1986,7 +1986,7 @@ void AttPosEKF::calcposNED(float (&posNED)[3], double lat, double lon, float hgt
     posNED[2] = -(hgt - hgtRef);
 }
 
-void AttPosEKF::calcLLH(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef)
+void AttPosEKF::calcLLH(float posNED[3], float &lat, float &lon, float &hgt, float latRef, float lonRef, float hgtRef)
 {
     lat = latRef + posNED[0] * earthRadiusInv;
     lon = lonRef + posNED[1] * earthRadiusInv / cos(latRef);
diff --git a/src/modules/ekf_att_pos_estimator/estimator.h b/src/modules/ekf_att_pos_estimator/estimator.h
index e821089f2..69d8cfce8 100644
--- a/src/modules/ekf_att_pos_estimator/estimator.h
+++ b/src/modules/ekf_att_pos_estimator/estimator.h
@@ -301,7 +301,7 @@ static void calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, flo
 
 static void calcposNED(float (&posNED)[3], double lat, double lon, float hgt, double latRef, double lonRef, float hgtRef);
 
-static void calcLLH(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef);
+static void calcLLH(float posNED[3], float &lat, float &lon, float &hgt, float latRef, float lonRef, float hgtRef);
 
 static void quat2Tnb(Mat3f &Tnb, const float (&quat)[4]);
 
diff --git a/src/modules/gpio_led/gpio_led.c b/src/modules/gpio_led/gpio_led.c
index 839a7890b..1fc6d1295 100644
--- a/src/modules/gpio_led/gpio_led.c
+++ b/src/modules/gpio_led/gpio_led.c
@@ -189,7 +189,8 @@ int gpio_led_main(int argc, char *argv[])
 		} else if (!strcmp(argv[1], "stop")) {
 			if (gpio_led_started) {
 				gpio_led_started = false;
-				warnx("stop");
+                // FIXME: Is this correct? changed from warnx
+				errx(0, "stop");
 
 			} else {
 				errx(1, "not running");
diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c
index 0e7dc621c..8ed69678c 100644
--- a/src/modules/px4iofirmware/sbus.c
+++ b/src/modules/px4iofirmware/sbus.c
@@ -119,13 +119,19 @@ sbus_init(const char *device)
 bool
 sbus1_output(uint16_t *values, uint16_t num_values)
 {
-	write(sbus_fd, 'A', 1);
+    // FIXME: I assume this was previously NYI?
+    ssize_t cBytes = num_values * sizeof(values[0]);
+	ssize_t cBytesWritten = write(sbus_fd, values, cBytes);
+    return cBytesWritten == cBytes;
 }
 
 bool
 sbus2_output(uint16_t *values, uint16_t num_values)
 {
-	write(sbus_fd, 'B', 1);
+    // FIXME: I assume this was previously NYI?
+    ssize_t cBytes = num_values * sizeof(values[0]);
+	ssize_t cBytesWritten = write(sbus_fd, values, cBytes);
+    return cBytesWritten == cBytes;
 }
 
 bool
diff --git a/src/systemcmds/mtd/mtd.c b/src/systemcmds/mtd/mtd.c
index a57eaafe7..8b43e54da 100644
--- a/src/systemcmds/mtd/mtd.c
+++ b/src/systemcmds/mtd/mtd.c
@@ -193,8 +193,12 @@ ramtron_attach(void)
 		errx(1, "failed to initialize mtd driver");
 
 	int ret = mtd_dev->ioctl(mtd_dev, MTDIOC_SETSPEED, (unsigned long)10*1000*1000);
-        if (ret != OK)
-            warnx(1, "failed to set bus speed");
+    if (ret != OK) {
+        // FIXME: From the previous warnx call, it looked like this should have been an errx instead. Tried
+        // that but setting the bug speed does fail all the time. Which was then exiting and the board would
+        // not run correctly. So changed to warnx.
+        warnx("failed to set bus speed");
+    }
 
 	attached = true;
 }
diff --git a/src/systemcmds/tests/test_mixer.cpp b/src/systemcmds/tests/test_mixer.cpp
index df382e2c6..dc2c20e79 100644
--- a/src/systemcmds/tests/test_mixer.cpp
+++ b/src/systemcmds/tests/test_mixer.cpp
@@ -372,6 +372,7 @@ int test_mixer(int argc, char *argv[])
 	}
 
 	warnx("SUCCESS: No errors in mixer test");
+    return 0;
 }
 
 static int