Merged cmake-2

src/modules/simulator/simulator.cpp

@@ -45,7 +45,7 @@
 #include <errno.h>
 #include <stdio.h>
 #include <string.h>
-#include <sys/types.h> 
+#include <sys/types.h>
 #include <drivers/drv_led.h>
 
 #include "simulator.h"
@@ -91,27 +91,33 @@ bool Simulator::getAirspeedSample(uint8_t *buf, int len)
 	return _airspeed.copyData(buf, len);
 }
 
-void Simulator::write_MPU_data(void *buf) {
+void Simulator::write_MPU_data(void *buf)
+{
 	_mpu.writeData(buf);
 }
 
-void Simulator::write_accel_data(void *buf) {
+void Simulator::write_accel_data(void *buf)
+{
 	_accel.writeData(buf);
 }
 
-void Simulator::write_mag_data(void *buf) {
+void Simulator::write_mag_data(void *buf)
+{
 	_mag.writeData(buf);
 }
 
-void Simulator::write_baro_data(void *buf) {
+void Simulator::write_baro_data(void *buf)
+{
 	_baro.writeData(buf);
 }
 
-void Simulator::write_gps_data(void *buf) {
+void Simulator::write_gps_data(void *buf)
+{
 	_gps.writeData(buf);
 }
 
-void Simulator::write_airspeed_data(void *buf) {
+void Simulator::write_airspeed_data(void *buf)
+{
 	_airspeed.writeData(buf);
 }
 
@@ -119,20 +125,23 @@ int Simulator::start(int argc, char *argv[])
 {
 	int ret = 0;
 	_instance = new Simulator();
+
 	if (_instance) {
 		drv_led_start();
+
 		if (argv[2][1] == 's') {
 			_instance->initializeSensorData();
 #ifndef __PX4_QURT
 			// Update sensor data
 			_instance->pollForMAVLinkMessages(false);
 #endif
+
 		} else {
 			// Update sensor data
 			_instance->pollForMAVLinkMessages(true);
 		}
-	}
+
-	else {
+	} else {
 		PX4_WARN("Simulator creation failed");
 		ret = 1;
 	}
@@ -153,52 +162,54 @@ __END_DECLS
 
 extern "C" {
 
-int simulator_main(int argc, char *argv[])
+	int simulator_main(int argc, char *argv[])
-{
+	{
-	int ret = 0;
+		int ret = 0;
-	if (argc == 3 && strcmp(argv[1], "start") == 0) {
-		if (strcmp(argv[2], "-s") == 0 || strcmp(argv[2], "-p") == 0) {
-			if (g_sim_task >= 0) {
-				warnx("Simulator already started");
-				return 0;
-			}
-			g_sim_task = px4_task_spawn_cmd("Simulator",
-				SCHED_DEFAULT,
-				SCHED_PRIORITY_MAX - 5,
-				1500,
-				Simulator::start,
-				argv);
 
-			// now wait for the command to complete
+		if (argc == 3 && strcmp(argv[1], "start") == 0) {
-			while(true) {
+			if (strcmp(argv[2], "-s") == 0 || strcmp(argv[2], "-p") == 0) {
-				if (Simulator::getInstance() && Simulator::getInstance()->isInitialized()) {
+				if (g_sim_task >= 0) {
-					break;
+					warnx("Simulator already started");
-				} else {
+					return 0;
-					usleep(100000);
 				}
+
+				g_sim_task = px4_task_spawn_cmd("Simulator",
+								SCHED_DEFAULT,
+								SCHED_PRIORITY_MAX - 5,
+								1500,
+								Simulator::start,
+								argv);
+
+				// now wait for the command to complete
+				while (true) {
+					if (Simulator::getInstance() && Simulator::getInstance()->isInitialized()) {
+						break;
+
+					} else {
+						usleep(100000);
+					}
+				}
+
+			} else {
+				usage();
+				ret = -EINVAL;
 			}
-		}
+
-		else
+		} else if (argc == 2 && strcmp(argv[1], "stop") == 0) {
-		{
+			if (g_sim_task < 0) {
+				PX4_WARN("Simulator not running");
+
+			} else {
+				px4_task_delete(g_sim_task);
+				g_sim_task = -1;
+			}
+
+		} else {
 			usage();
 			ret = -EINVAL;
 		}
-	}
+
-	else if (argc == 2 && strcmp(argv[1], "stop") == 0) {
+		return ret;
-		if (g_sim_task < 0) {
-			PX4_WARN("Simulator not running");
-		}
-		else {
-			px4_task_delete(g_sim_task);
-			g_sim_task = -1;
-		}
-	}
-	else {
-		usage();
-		ret = -EINVAL;
 	}
 
-	return ret;
-}
-
 }

src/modules/simulator/simulator.h

@@ -53,24 +53,25 @@
 #include <uORB/uORB.h>
 #include <v1.0/mavlink_types.h>
 #include <v1.0/common/mavlink.h>
-namespace simulator {
+namespace simulator
+{
 
 // FIXME - what is the endianness of these on actual device?
 #pragma pack(push, 1)
 struct RawAccelData {
-		float temperature;
+	float temperature;
-        float x;
+	float x;
-        float y;
+	float y;
-        float z;
+	float z;
 };
 #pragma pack(pop)
 
 #pragma pack(push, 1)
 struct RawMagData {
-		float temperature;
+	float temperature;
-        float x;
+	float x;
-        float y;
+	float y;
-        float z;
+	float z;
 };
 #pragma pack(pop)
 
@@ -103,22 +104,23 @@ struct RawAirspeedData {
 
 #pragma pack(push, 1)
 struct RawGPSData {
- int32_t lat;
+	int32_t lat;
- int32_t lon;
+	int32_t lon;
- int32_t alt;
+	int32_t alt;
- uint16_t eph;
+	uint16_t eph;
- uint16_t epv;
+	uint16_t epv;
- uint16_t vel;
+	uint16_t vel;
- int16_t vn;
+	int16_t vn;
- int16_t ve;
+	int16_t ve;
- int16_t vd;
+	int16_t vd;
- uint16_t cog;
+	uint16_t cog;
- uint8_t fix_type;
+	uint8_t fix_type;
- uint8_t satellites_visible;
+	uint8_t satellites_visible;
 };
 #pragma pack(pop)
 
-template <typename RType> class Report {
+template <typename RType> class Report
+{
 public:
 	Report(int readers) :
 		_readidx(0),
@@ -135,6 +137,7 @@ public:
 		if (len != _report_len) {
 			return false;
 		}
+
 		read_lock();
 		memcpy(outbuf, &_buf[_readidx], _report_len);
 		read_unlock();
@@ -153,13 +156,13 @@ protected:
 	void read_unlock() { px4_sem_post(&_lock); }
 	void write_lock()
 	{
-		for (int i=0; i<_max_readers; i++) {
+		for (int i = 0; i < _max_readers; i++) {
 			px4_sem_wait(&_lock);
 		}
 	}
 	void write_unlock()
 	{
-		for (int i=0; i<_max_readers; i++) {
+		for (int i = 0; i < _max_readers; i++) {
 			px4_sem_post(&_lock);
 		}
 	}
@@ -173,7 +176,8 @@ protected:
 
 };
 
-class Simulator {
+class Simulator
+{
 public:
 	static Simulator *getInstance();
 
@@ -211,32 +215,32 @@ public:
 
 private:
 	Simulator() :
-	_accel(1),
+		_accel(1),
-	_mpu(1),
+		_mpu(1),
-	_baro(1),
+		_baro(1),
-	_mag(1),
+		_mag(1),
-	_gps(1),
+		_gps(1),
-	_airspeed(1),
+		_airspeed(1),
-	_accel_pub(nullptr),
+		_accel_pub(nullptr),
-	_baro_pub(nullptr),
+		_baro_pub(nullptr),
-	_gyro_pub(nullptr),
+		_gyro_pub(nullptr),
-	_mag_pub(nullptr),
+		_mag_pub(nullptr),
-	_initialized(false)
+		_initialized(false)
 #ifndef __PX4_QURT
-	,
+		,
-	_rc_channels_pub(nullptr),
+		_rc_channels_pub(nullptr),
-	_actuator_outputs_sub(-1),
+		_actuator_outputs_sub(-1),
-	_vehicle_attitude_sub(-1),
+		_vehicle_attitude_sub(-1),
-	_manual_sub(-1),
+		_manual_sub(-1),
-	_vehicle_status_sub(-1),
+		_vehicle_status_sub(-1),
-	_rc_input{},
+		_rc_input{},
-	_actuators{},
+		_actuators{},
-	_attitude{},
+		_attitude{},
-	_manual{},
+		_manual{},
-	_vehicle_status{}
+		_vehicle_status{}
 #endif
 	{}
-	~Simulator() { _instance=NULL; }
+	~Simulator() { _instance = NULL; }
 
 	void initializeSensorData();
 

src/modules/simulator/simulator_mavlink.cpp

@@ -71,7 +71,8 @@ static socklen_t _addrlen = sizeof(_srcaddr);
 
 using namespace simulator;
 
-void Simulator::pack_actuator_message(mavlink_hil_controls_t &actuator_msg) {
+void Simulator::pack_actuator_message(mavlink_hil_controls_t &actuator_msg)
+{
 	float out[8] = {};
 
 	const float pwm_center = (PWM_HIGHEST_MAX + PWM_LOWEST_MIN) / 2;
@@ -96,6 +97,7 @@ void Simulator::pack_actuator_message(mavlink_hil_controls_t &actuator_msg) {
 				out[i] = 0.0f;
 			}
 		}
+
 	} else {
 		// convert back to range [-1, 1]
 		for (unsigned i = 0; i < 8; i++) {
@@ -122,29 +124,31 @@ void Simulator::pack_actuator_message(mavlink_hil_controls_t &actuator_msg) {
 	actuator_msg.nav_mode = 0;
 }
 
-void Simulator::send_controls() {
+void Simulator::send_controls()
+{
 	mavlink_hil_controls_t msg;
 	pack_actuator_message(msg);
 	//PX4_WARN("Sending HIL_CONTROLS msg");
 	send_mavlink_message(MAVLINK_MSG_ID_HIL_CONTROLS, &msg, 200);
 }
 
-static void fill_rc_input_msg(struct rc_input_values *rc, mavlink_rc_channels_t *rc_channels) {
+static void fill_rc_input_msg(struct rc_input_values *rc, mavlink_rc_channels_t *rc_channels)
+{
 	rc->timestamp_publication = hrt_absolute_time();
 	rc->timestamp_last_signal = hrt_absolute_time();
 	rc->channel_count = rc_channels->chancount;
 	rc->rssi = rc_channels->rssi;
 
-/*	PX4_WARN("RC: %d, %d, %d, %d, %d, %d, %d, %d",
+	/*	PX4_WARN("RC: %d, %d, %d, %d, %d, %d, %d, %d",
-		 rc_channels->chan1_raw,
+			 rc_channels->chan1_raw,
-		 rc_channels->chan2_raw,
+			 rc_channels->chan2_raw,
-		 rc_channels->chan3_raw,
+			 rc_channels->chan3_raw,
-		 rc_channels->chan4_raw,
+			 rc_channels->chan4_raw,
-		 rc_channels->chan5_raw,
+			 rc_channels->chan5_raw,
-		 rc_channels->chan6_raw,
+			 rc_channels->chan6_raw,
-		 rc_channels->chan7_raw,
+			 rc_channels->chan7_raw,
-		 rc_channels->chan8_raw);
+			 rc_channels->chan8_raw);
-*/
+	*/
 
 	rc->values[0] = rc_channels->chan1_raw;
 	rc->values[1] = rc_channels->chan2_raw;
@@ -166,7 +170,8 @@ static void fill_rc_input_msg(struct rc_input_values *rc, mavlink_rc_channels_t
 	rc->values[17] = rc_channels->chan18_raw;
 }
 
-void Simulator::update_sensors(mavlink_hil_sensor_t *imu) {
+void Simulator::update_sensors(mavlink_hil_sensor_t *imu)
+{
 	// write sensor data to memory so that drivers can copy data from there
 	RawMPUData mpu;
 	mpu.accel_x = imu->xacc;
@@ -208,11 +213,12 @@ void Simulator::update_sensors(mavlink_hil_sensor_t *imu) {
 	write_airspeed_data((void *)&airspeed);
 }
 
-void Simulator::update_gps(mavlink_hil_gps_t *gps_sim) {
+void Simulator::update_gps(mavlink_hil_gps_t *gps_sim)
+{
 	RawGPSData gps;
-	gps.lat = gps_sim->lat; 
+	gps.lat = gps_sim->lat;
- 	gps.lon = gps_sim->lon;
+	gps.lon = gps_sim->lon;
- 	gps.alt = gps_sim->alt;
+	gps.alt = gps_sim->alt;
 	gps.eph = gps_sim->eph;
 	gps.epv = gps_sim->epv;
 	gps.vel = gps_sim->vel;
@@ -226,23 +232,28 @@ void Simulator::update_gps(mavlink_hil_gps_t *gps_sim) {
 	write_gps_data((void *)&gps);
 }
 
-void Simulator::handle_message(mavlink_message_t *msg, bool publish) {
+void Simulator::handle_message(mavlink_message_t *msg, bool publish)
-	switch(msg->msgid) {
+{
+	switch (msg->msgid) {
 	case MAVLINK_MSG_ID_HIL_SENSOR:
 		mavlink_hil_sensor_t imu;
 		mavlink_msg_hil_sensor_decode(msg, &imu);
+
 		if (publish) {
 			publish_sensor_topics(&imu);
 		}
+
 		update_sensors(&imu);
 		break;
 
 	case MAVLINK_MSG_ID_HIL_GPS:
 		mavlink_hil_gps_t gps_sim;
 		mavlink_msg_hil_gps_decode(msg, &gps_sim);
+
 		if (publish) {
 			//PX4_WARN("FIXME:  Need to publish GPS topic.  Not done yet.");
 		}
+
 		update_gps(&gps_sim);
 		break;
 
@@ -253,17 +264,20 @@ void Simulator::handle_message(mavlink_message_t *msg, bool publish) {
 
 		// publish message
 		if (publish) {
-			if(_rc_channels_pub == nullptr) {
+			if (_rc_channels_pub == nullptr) {
 				_rc_channels_pub = orb_advertise(ORB_ID(input_rc), &_rc_input);
+
 			} else {
 				orb_publish(ORB_ID(input_rc), _rc_channels_pub, &_rc_input);
 			}
 		}
+
 		break;
 	}
 }
 
-void Simulator::send_mavlink_message(const uint8_t msgid, const void *msg, uint8_t component_ID) {
+void Simulator::send_mavlink_message(const uint8_t msgid, const void *msg, uint8_t component_ID)
+{
 	component_ID = 0;
 	uint8_t payload_len = mavlink_message_lengths[msgid];
 	unsigned packet_len = payload_len + MAVLINK_NUM_NON_PAYLOAD_BYTES;
@@ -280,7 +294,7 @@ void Simulator::send_mavlink_message(const uint8_t msgid, const void *msg, uint8
 	buf[5] = msgid;
 
 	/* payload */
-	memcpy(&buf[MAVLINK_NUM_HEADER_BYTES],msg, payload_len);
+	memcpy(&buf[MAVLINK_NUM_HEADER_BYTES], msg, payload_len);
 
 	/* checksum */
 	uint16_t checksum;
@@ -292,44 +306,51 @@ void Simulator::send_mavlink_message(const uint8_t msgid, const void *msg, uint8
 	buf[MAVLINK_NUM_HEADER_BYTES + payload_len + 1] = (uint8_t)(checksum >> 8);
 
 	ssize_t len = sendto(_fd, buf, packet_len, 0, (struct sockaddr *)&_srcaddr, _addrlen);
+
 	if (len <= 0) {
 		PX4_WARN("Failed sending mavlink message");
 	}
 }
 
-void Simulator::poll_topics() {
+void Simulator::poll_topics()
+{
 	// copy new actuator data if available
 	bool updated;
 	orb_check(_actuator_outputs_sub, &updated);
+
 	if (updated) {
 		orb_copy(ORB_ID(actuator_outputs), _actuator_outputs_sub, &_actuators);
 	}
 
 	orb_check(_vehicle_status_sub, &updated);
+
 	if (updated) {
 		orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status);
 	}
 }
 
-void *Simulator::sending_trampoline(void *) {
+void *Simulator::sending_trampoline(void *)
+{
 	_instance->send();
 	return 0;	// why do I have to put this???
 }
 
-void Simulator::send() {
+void Simulator::send()
+{
 	px4_pollfd_struct_t fds[1];
 	fds[0].fd = _actuator_outputs_sub;
 	fds[0].events = POLLIN;
 
 	int pret;
 
-	while(true) {
+	while (true) {
 		// wait for up to 100ms for data
 		pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
 
 		//timed out
-		if (pret == 0)
+		if (pret == 0) {
 			continue;
+		}
 
 		// this is undesirable but not much we can do
 		if (pret < 0) {
@@ -349,23 +370,19 @@ void Simulator::send() {
 
 void Simulator::initializeSensorData()
 {
-	struct baro_report baro;
+	struct baro_report baro = {};
-	memset(&baro,0,sizeof(baro));
 	baro.pressure = 120000.0f;
 
 	// acceleration report
-	struct accel_report accel;
+	struct accel_report accel = {};
-	memset(&accel,0,sizeof(accel));
 	accel.z = 9.81f;
 	accel.range_m_s2 = 80.0f;
 
 	// gyro report
-	struct gyro_report gyro;
+	struct gyro_report gyro = {};
-	memset(&gyro, 0 ,sizeof(gyro));
 
 	// mag report
-	struct mag_report mag;
+	struct mag_report mag = {};
-	memset(&mag, 0 ,sizeof(mag));
 }
 
 void Simulator::pollForMAVLinkMessages(bool publish)
@@ -410,6 +427,7 @@ void Simulator::pollForMAVLinkMessages(bool publish)
 
 	if (serial_fd < 0) {
 		PX4_INFO("Not using %s for radio control input. Assuming joystick input via MAVLink.", PIXHAWK_DEVICE);
+
 	} else {
 
 		// tell the device to stream some messages
@@ -435,10 +453,12 @@ void Simulator::pollForMAVLinkMessages(bool publish)
 	// this is important for the UDP communication to work
 	int pret = -1;
 	PX4_INFO("Waiting for initial data on UDP. Please start the flight simulator to proceed..");
+
 	while (pret <= 0) {
-		pret = ::poll(&fds[0], (sizeof(fds[0])/sizeof(fds[0])), 100);
+		pret = ::poll(&fds[0], (sizeof(fds[0]) / sizeof(fds[0])), 100);
 	}
-	PX4_INFO("Found initial message, pret = %d",pret);
+
+	PX4_INFO("Found initial message, pret = %d", pret);
 	_initialized = true;
 	// reset system time
 	(void)hrt_reset();
@@ -460,11 +480,12 @@ void Simulator::pollForMAVLinkMessages(bool publish)
 	// wait for new mavlink messages to arrive
 	while (true) {
 
-		pret = ::poll(&fds[0], (sizeof(fds)/sizeof(fds[0])), 100);
+		pret = ::poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
 
 		//timed out
-		if (pret == 0)
+		if (pret == 0) {
 			continue;
+		}
 
 		// this is undesirable but not much we can do
 		if (pret < 0) {
@@ -477,13 +498,13 @@ void Simulator::pollForMAVLinkMessages(bool publish)
 		// got data from simulator
 		if (fds[0].revents & POLLIN) {
 			len = recvfrom(_fd, _buf, sizeof(_buf), 0, (struct sockaddr *)&_srcaddr, &_addrlen);
+
 			if (len > 0) {
 				mavlink_message_t msg;
 				mavlink_status_t status;
-				for (int i = 0; i < len; ++i)
+
-				{
+				for (int i = 0; i < len; ++i) {
-					if (mavlink_parse_char(MAVLINK_COMM_0, _buf[i], &msg, &status))
+					if (mavlink_parse_char(MAVLINK_COMM_0, _buf[i], &msg, &status)) {
-					{
 						// have a message, handle it
 						handle_message(&msg, publish);
 					}
@@ -494,13 +515,13 @@ void Simulator::pollForMAVLinkMessages(bool publish)
 		// got data from PIXHAWK
 		if (fds[1].revents & POLLIN) {
 			len = ::read(serial_fd, serial_buf, sizeof(serial_buf));
+
 			if (len > 0) {
 				mavlink_message_t msg;
 				mavlink_status_t status;
-				for (int i = 0; i < len; ++i)
+
-				{
+				for (int i = 0; i < len; ++i) {
-					if (mavlink_parse_char(MAVLINK_COMM_0, serial_buf[i], &msg, &status))
+					if (mavlink_parse_char(MAVLINK_COMM_0, serial_buf[i], &msg, &status)) {
-					{
 						// have a message, handle it
 						handle_message(&msg, publish);
 					}
@@ -612,15 +633,17 @@ int openUart(const char *uart_name, int baud)
 	return uart_fd;
 }
 
-int Simulator::publish_sensor_topics(mavlink_hil_sensor_t *imu) {
+int Simulator::publish_sensor_topics(mavlink_hil_sensor_t *imu)
+{
 
 
-        //uint64_t timestamp = imu->time_usec;
+	//uint64_t timestamp = imu->time_usec;
-        uint64_t timestamp = hrt_absolute_time();
+	uint64_t timestamp = hrt_absolute_time();
+
+	if ((imu->fields_updated & 0x1FFF) != 0x1FFF) {
+		PX4_DEBUG("All sensor fields in mavlink HIL_SENSOR packet not updated.  Got %08x", imu->fields_updated);
+	}
 
-        if((imu->fields_updated & 0x1FFF)!=0x1FFF) {
-	   	        PX4_DEBUG("All sensor fields in mavlink HIL_SENSOR packet not updated.  Got %08x",imu->fields_updated);
-        }
 	/*
 	  static int count=0;
 	  static uint64_t last_timestamp=0;
@@ -717,5 +740,5 @@ int Simulator::publish_sensor_topics(mavlink_hil_sensor_t *imu) {
 		}
 	}
 
-        return OK;
+	return OK;
 }

src/platforms/posix/drivers/accelsim/accelsim.cpp

@@ -493,7 +493,7 @@ ACCELSIM::init()
 	_accel_class_instance = register_class_devname(ACCEL_BASE_DEVICE_PATH);
 
 	/* advertise sensor topic, measure manually to initialize valid report */
-	struct accel_report arp;
+	struct accel_report arp = {};
 	_accel_reports->get(&arp);
 
 	/* measurement will have generated a report, publish */

src/platforms/posix/drivers/gyrosim/gyrosim.cpp

@@ -466,7 +466,7 @@ GYROSIM::init()
 	measure();
 
 	/* advertise sensor topic, measure manually to initialize valid report */
-	struct accel_report arp;
+	struct accel_report arp = {};
 	_accel_reports->get(&arp);
 
 	/* measurement will have generated a report, publish */
@@ -482,7 +482,7 @@ GYROSIM::init()
 
 
 	/* advertise sensor topic, measure manually to initialize valid report */
-	struct gyro_report grp;
+	struct gyro_report grp = {};
 	_gyro_reports->get(&grp);
 
 	_gyro->_gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &grp,