Rover: C++ify, minor cleanups

4 files changed, 59 insertions, 54 deletions

diff --git a/src/examples/rover_steering_control/main.c b/src/examples/rover_steering_control/main.cpp
index d3a646ee9..7ef914098 100644
--- a/src/examples/rover_steering_control/main.c
+++ b/src/examples/rover_steering_control/main.cpp
@@ -32,14 +32,11 @@
  ****************************************************************************/
 
 /**
- * @file main.c
+ * @file main.cpp
  *
- * Example implementation of a fixed wing attitude controller. This file is a complete
- * fixed wing controller for manual attitude control or auto waypoint control.
- * There is no need to touch any other system components to extend / modify the
- * complete control architecture.
+ * Example implementation of a rover steering controller.
  *
- * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Lorenz Meier <lorenz@px4.io>
  */
 
 #include <nuttx/config.h>
@@ -88,7 +85,27 @@
  * the command line to one particular process or the need for bg/fg
  * ^Z support by the shell.
  */
-__EXPORT int rover_steering_control_main(int argc, char *argv[]);
+extern "C" __EXPORT int rover_steering_control_main(int argc, char *argv[]);
+
+struct params {
+	float yaw_p;
+};
+
+struct param_handles {
+	param_t yaw_p;
+};
+
+/**
+ * Initialize all parameter handles and values
+ *
+ */
+int parameters_init(struct param_handles *h);
+
+/**
+ * Update all parameters
+ *
+ */
+int parameters_update(const struct param_handles *h, struct params *p);
 
 /**
  * Mainloop of daemon.
@@ -117,9 +134,24 @@ void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const st
 static bool thread_should_exit = false;		/**< Daemon exit flag */
 static bool thread_running = false;		/**< Daemon status flag */
 static int deamon_task;				/**< Handle of deamon task / thread */
-static struct params p;
+static struct params pp;
 static struct param_handles ph;
 
+int parameters_init(struct param_handles *h)
+{
+	/* PID parameters */
+	h->yaw_p 	=	param_find("RV_YAW_P");
+
+	return OK;
+}
+
+int parameters_update(const struct param_handles *h, struct params *p)
+{
+	param_get(h->yaw_p, &(p->yaw_p));
+
+	return OK;
+}
+
 void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att,
 		      struct actuator_controls_s *actuators)
 {
@@ -152,10 +184,12 @@ void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const st
 	 * Calculate roll error and apply P gain
 	 */
 	float yaw_err = att->yaw - att_sp->yaw_body;
-	actuators->control[2] = yaw_err * p.yaw_p;
+	actuators->control[2] = yaw_err * pp.yaw_p;
 
 	/* copy throttle */
 	actuators->control[3] = att_sp->thrust;
+
+	actuators->timestamp = hrt_absolute_time();
 }
 
 /* Main Thread */
@@ -172,7 +206,7 @@ int rover_steering_control_thread_main(int argc, char *argv[])
 
 	/* initialize parameters, first the handles, then the values */
 	parameters_init(&ph);
-	parameters_update(&ph, &p);
+	parameters_update(&ph, &pp);
 
 
 	/*
@@ -219,10 +253,12 @@ int rover_steering_control_thread_main(int argc, char *argv[])
 
 
 	/* publish actuator controls with zero values */
-	for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) {
+	for (unsigned i = 0; i < (sizeof(actuators.control) / sizeof(actuators.control[0])); i++) {
 		actuators.control[i] = 0.0f;
 	}
 
+	struct vehicle_attitude_setpoint_s _att_sp = {};
+
 	/*
 	 * Advertise that this controller will publish actuator
 	 * control values and the rate setpoint
@@ -239,9 +275,11 @@ int rover_steering_control_thread_main(int argc, char *argv[])
 
 	/* Setup of loop */
 
-	struct pollfd fds[2] = {{ .fd = param_sub, .events = POLLIN },
-		{ .fd = att_sub, .events = POLLIN }
-	};
+	struct pollfd fds[2];
+	fds[0].fd = param_sub;
+	fds[0].events = POLLIN;
+	fds[1].fd = att_sub;
+	fds[1].events = POLLIN;
 
 	while (!thread_should_exit) {
 
@@ -275,7 +313,7 @@ int rover_steering_control_thread_main(int argc, char *argv[])
 				orb_copy(ORB_ID(parameter_update), param_sub, &update);
 
 				/* if a param update occured, re-read our parameters */
-				parameters_update(&ph, &p);
+				parameters_update(&ph, &pp);
 			}
 
 			/* only run controller if attitude changed */
@@ -294,13 +332,12 @@ int rover_steering_control_thread_main(int argc, char *argv[])
 				orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
 
 				if (att_sp_updated) {
-					struct vehicle_attitude_setpoint_s _att_sp;
 					orb_copy(ORB_ID(vehicle_attitude_setpoint), att_sp_sub, &_att_sp);
-
-					/* control attitude / heading */
-					control_attitude(&_att_sp, &att, &actuators);
 				}
 
+				/* control attitude / heading */
+				control_attitude(&_att_sp, &att, &actuators);
+
 				if (manual_sp_updated)
 					/* get the RC (or otherwise user based) input */
 				{
@@ -331,9 +368,10 @@ int rover_steering_control_thread_main(int argc, char *argv[])
 	thread_running = false;
 
 	/* kill all outputs */
-	for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) {
+	for (unsigned i = 0; i < (sizeof(actuators.control) / sizeof(actuators.control[0])); i++) {
 		actuators.control[i] = 0.0f;
 	}
+	actuators.timestamp = hrt_absolute_time();
 
 	orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
 
diff --git a/src/examples/rover_steering_control/module.mk b/src/examples/rover_steering_control/module.mk
index 34f462bd4..8f7a7ed3a 100644
--- a/src/examples/rover_steering_control/module.mk
+++ b/src/examples/rover_steering_control/module.mk
@@ -37,7 +37,7 @@
 
 MODULE_COMMAND	= rover_steering_control
 
-SRCS		= main.c \
+SRCS		= main.cpp \
 		  params.c
 
 MODULE_STACKSIZE = 1200
diff --git a/src/examples/rover_steering_control/params.c b/src/examples/rover_steering_control/params.c
index c941c1a02..7eff0b0a2 100644
--- a/src/examples/rover_steering_control/params.c
+++ b/src/examples/rover_steering_control/params.c
@@ -47,18 +47,3 @@
  *
  */
 PARAM_DEFINE_FLOAT(RV_YAW_P, 0.1f);
-
-int parameters_init(struct param_handles *h)
-{
-	/* PID parameters */
-	h->yaw_p 	=	param_find("RV_YAW_P");
-
-	return OK;
-}
-
-int parameters_update(const struct param_handles *h, struct params *p)
-{
-	param_get(h->yaw_p, &(p->yaw_p));
-
-	return OK;
-}
diff --git a/src/examples/rover_steering_control/params.h b/src/examples/rover_steering_control/params.h
index b0f76bca0..438c605f5 100644
--- a/src/examples/rover_steering_control/params.h
+++ b/src/examples/rover_steering_control/params.h
@@ -40,22 +40,4 @@
 
 #include <systemlib/param/param.h>
 
-struct params {
-	float yaw_p;
-};
 
-struct param_handles {
-	param_t yaw_p;
-};
-
-/**
- * Initialize all parameter handles and values
- *
- */
-int parameters_init(struct param_handles *h);
-
-/**
- * Update all parameters
- *
- */
-int parameters_update(const struct param_handles *h, struct params *p);