From 30d0dab8e67c339ab840014bd52a0f7e995f5d11 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 20 Sep 2015 11:35:52 +0200 Subject: [PATCH 1/2] Simulator: Fix code style --- src/modules/simulator/simulator.cpp | 109 ++++++++-------- src/modules/simulator/simulator.h | 100 ++++++++------- src/modules/simulator/simulator_mavlink.cpp | 133 ++++++++++++-------- 3 files changed, 190 insertions(+), 152 deletions(-) diff --git a/src/modules/simulator/simulator.cpp b/src/modules/simulator/simulator.cpp index 3ec3b305f8..1656a3451f 100644 --- a/src/modules/simulator/simulator.cpp +++ b/src/modules/simulator/simulator.cpp @@ -45,7 +45,7 @@ #include #include #include -#include +#include #include #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 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); + int simulator_main(int argc, char *argv[]) + { + int ret = 0; - // now wait for the command to complete - while(true) { - if (Simulator::getInstance() && Simulator::getInstance()->isInitialized()) { - break; - } else { - usleep(100000); + 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 + 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) { - 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; } - return ret; -} - } diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h index af4b8a746c..ca7d851270 100644 --- a/src/modules/simulator/simulator.h +++ b/src/modules/simulator/simulator.h @@ -53,24 +53,25 @@ #include #include #include -namespace simulator { +namespace simulator +{ // FIXME - what is the endianness of these on actual device? #pragma pack(push, 1) struct RawAccelData { - float temperature; - float x; - float y; - float z; + float temperature; + float x; + float y; + float z; }; #pragma pack(pop) #pragma pack(push, 1) struct RawMagData { - float temperature; - float x; - float y; - float z; + float temperature; + float x; + float y; + float z; }; #pragma pack(pop) @@ -103,22 +104,23 @@ struct RawAirspeedData { #pragma pack(push, 1) struct RawGPSData { - int32_t lat; - int32_t lon; - int32_t alt; - uint16_t eph; - uint16_t epv; - uint16_t vel; - int16_t vn; - int16_t ve; - int16_t vd; - uint16_t cog; - uint8_t fix_type; - uint8_t satellites_visible; + int32_t lat; + int32_t lon; + int32_t alt; + uint16_t eph; + uint16_t epv; + uint16_t vel; + int16_t vn; + int16_t ve; + int16_t vd; + uint16_t cog; + uint8_t fix_type; + uint8_t satellites_visible; }; #pragma pack(pop) -template class Report { +template 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() { sem_post(&_lock); } void write_lock() { - for (int i=0; i<_max_readers; i++) { + for (int i = 0; i < _max_readers; i++) { sem_wait(&_lock); } } void write_unlock() { - for (int i=0; i<_max_readers; i++) { + for (int i = 0; i < _max_readers; i++) { 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), - _mpu(1), - _baro(1), - _mag(1), - _gps(1), - _airspeed(1), - _accel_pub(nullptr), - _baro_pub(nullptr), - _gyro_pub(nullptr), - _mag_pub(nullptr), - _initialized(false) + _accel(1), + _mpu(1), + _baro(1), + _mag(1), + _gps(1), + _airspeed(1), + _accel_pub(nullptr), + _baro_pub(nullptr), + _gyro_pub(nullptr), + _mag_pub(nullptr), + _initialized(false) #ifndef __PX4_QURT - , - _rc_channels_pub(nullptr), - _actuator_outputs_sub(-1), - _vehicle_attitude_sub(-1), - _manual_sub(-1), - _vehicle_status_sub(-1), - _rc_input{}, - _actuators{}, - _attitude{}, - _manual{}, - _vehicle_status{} + , + _rc_channels_pub(nullptr), + _actuator_outputs_sub(-1), + _vehicle_attitude_sub(-1), + _manual_sub(-1), + _vehicle_status_sub(-1), + _rc_input{}, + _actuators{}, + _attitude{}, + _manual{}, + _vehicle_status{} #endif {} - ~Simulator() { _instance=NULL; } + ~Simulator() { _instance = NULL; } void initializeSensorData(); diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index 81f5bf39eb..c9833addda 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/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", - rc_channels->chan1_raw, - rc_channels->chan2_raw, - rc_channels->chan3_raw, - rc_channels->chan4_raw, - rc_channels->chan5_raw, - rc_channels->chan6_raw, - rc_channels->chan7_raw, - rc_channels->chan8_raw); -*/ + /* PX4_WARN("RC: %d, %d, %d, %d, %d, %d, %d, %d", + rc_channels->chan1_raw, + rc_channels->chan2_raw, + rc_channels->chan3_raw, + rc_channels->chan4_raw, + rc_channels->chan5_raw, + rc_channels->chan6_raw, + rc_channels->chan7_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.lon = gps_sim->lon; - gps.alt = gps_sim->alt; + gps.lat = gps_sim->lat; + gps.lon = gps_sim->lon; + 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) { - switch(msg->msgid) { +void Simulator::handle_message(mavlink_message_t *msg, bool publish) +{ + 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; - memset(&baro,0,sizeof(baro)); + struct baro_report baro = {}; baro.pressure = 120000.0f; // acceleration report - struct accel_report accel; - memset(&accel,0,sizeof(accel)); + struct accel_report accel = {}; accel.z = 9.81f; accel.range_m_s2 = 80.0f; // gyro report - struct gyro_report gyro; - memset(&gyro, 0 ,sizeof(gyro)); + struct gyro_report gyro = {}; // mag report - struct mag_report mag; - memset(&mag, 0 ,sizeof(mag)); + struct mag_report 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) - { - if (mavlink_parse_char(MAVLINK_COMM_0, _buf[i], &msg, &status)) - { + + for (int i = 0; i < len; ++i) { + 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) - { - if (mavlink_parse_char(MAVLINK_COMM_0, serial_buf[i], &msg, &status)) - { + + for (int i = 0; i < len; ++i) { + 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 = hrt_absolute_time(); + //uint64_t timestamp = imu->time_usec; + 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; } From ddd1b8224034442e38427758fa7a383b6fdde298 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 20 Sep 2015 11:36:11 +0200 Subject: [PATCH 2/2] Simulated sensors: Publish valid data initially --- src/platforms/posix/drivers/accelsim/accelsim.cpp | 2 +- src/platforms/posix/drivers/gyrosim/gyrosim.cpp | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/platforms/posix/drivers/accelsim/accelsim.cpp b/src/platforms/posix/drivers/accelsim/accelsim.cpp index 174c522383..cc299b5847 100644 --- a/src/platforms/posix/drivers/accelsim/accelsim.cpp +++ b/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 */ diff --git a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp index 5eb8ff7fea..850bf0f9d1 100644 --- a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp +++ b/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,