From c611749b4fe6a46b532dbe19f9512fd720a1f3a6 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 1 Jul 2015 11:38:19 -0700 Subject: [PATCH] Simulator: modified -p to publish individual sensor data The simulator was changed to publish the sensor data that is read by the sensors module when the -p flag is passed. Signed-off-by: Mark Charlebois --- msg/hil_sensor.msg | 100 ++++++++++ src/modules/simulator/simulator.cpp | 72 +------ src/modules/simulator/simulator.h | 51 ++--- src/modules/simulator/simulator_mavlink.cpp | 205 +++++++++++++++++--- 4 files changed, 298 insertions(+), 130 deletions(-) create mode 100644 msg/hil_sensor.msg diff --git a/msg/hil_sensor.msg b/msg/hil_sensor.msg new file mode 100644 index 0000000000..9317722db4 --- /dev/null +++ b/msg/hil_sensor.msg @@ -0,0 +1,100 @@ +# Definition of the hil_sensor uORB topic. + +int32 MAGNETOMETER_MODE_NORMAL = 0 +int32 MAGNETOMETER_MODE_POSITIVE_BIAS = 1 +int32 MAGNETOMETER_MODE_NEGATIVE_BIAS = 2 + +# Sensor readings in raw and SI-unit form. +# +# These values are read from the sensors. Raw values are in sensor-specific units, +# the scaled values are in SI-units, as visible from the ending of the variable +# or the comments. The use of the SI fields is in general advised, as these fields +# are scaled and offset-compensated where possible and do not change with board +# revisions and sensor updates. +# +# Actual data, this is specific to the type of data which is stored in this struct +# A line containing L0GME will be added by the Python logging code generator to the logged dataset. +# +# NOTE: Ordering of fields optimized to align to 32 bit / 4 bytes Change with consideration only + +uint64 timestamp # Timestamp in microseconds since boot, from gyro +# +int16[3] gyro_raw # Raw sensor values of angular velocity +float32[3] gyro_rad_s # Angular velocity in radian per seconds +uint32 gyro_errcount # Error counter for gyro 0 +float32 gyro_temp # Temperature of gyro 0 + +int16[3] accelerometer_raw # Raw acceleration in NED body frame +float32[3] accelerometer_m_s2 # Acceleration in NED body frame, in m/s^2 +int16 accelerometer_mode # Accelerometer measurement mode +float32 accelerometer_range_m_s2 # Accelerometer measurement range in m/s^2 +uint64 accelerometer_timestamp # Accelerometer timestamp +uint32 accelerometer_errcount # Error counter for accel 0 +float32 accelerometer_temp # Temperature of accel 0 + +int16[3] magnetometer_raw # Raw magnetic field in NED body frame +float32[3] magnetometer_ga # Magnetic field in NED body frame, in Gauss +int16 magnetometer_mode # Magnetometer measurement mode +float32 magnetometer_range_ga # measurement range in Gauss +float32 magnetometer_cuttoff_freq_hz # Internal analog low pass frequency of sensor +uint64 magnetometer_timestamp # Magnetometer timestamp +uint32 magnetometer_errcount # Error counter for mag 0 +float32 magnetometer_temp # Temperature of mag 0 + +int16[3] gyro1_raw # Raw sensor values of angular velocity +float32[3] gyro1_rad_s # Angular velocity in radian per seconds +uint64 gyro1_timestamp # Gyro timestamp +uint32 gyro1_errcount # Error counter for gyro 1 +float32 gyro1_temp # Temperature of gyro 1 + +int16[3] accelerometer1_raw # Raw acceleration in NED body frame +float32[3] accelerometer1_m_s2 # Acceleration in NED body frame, in m/s^2 +uint64 accelerometer1_timestamp # Accelerometer timestamp +uint32 accelerometer1_errcount # Error counter for accel 1 +float32 accelerometer1_temp # Temperature of accel 1 + +int16[3] magnetometer1_raw # Raw magnetic field in NED body frame +float32[3] magnetometer1_ga # Magnetic field in NED body frame, in Gauss +uint64 magnetometer1_timestamp # Magnetometer timestamp +uint32 magnetometer1_errcount # Error counter for mag 1 +float32 magnetometer1_temp # Temperature of mag 1 + +int16[3] gyro2_raw # Raw sensor values of angular velocity +float32[3] gyro2_rad_s # Angular velocity in radian per seconds +uint64 gyro2_timestamp # Gyro timestamp +uint32 gyro2_errcount # Error counter for gyro 1 +float32 gyro2_temp # Temperature of gyro 1 + +int16[3] accelerometer2_raw # Raw acceleration in NED body frame +float32[3] accelerometer2_m_s2 # Acceleration in NED body frame, in m/s^2 +uint64 accelerometer2_timestamp # Accelerometer timestamp +uint32 accelerometer2_errcount # Error counter for accel 2 +float32 accelerometer2_temp # Temperature of accel 2 + +int16[3] magnetometer2_raw # Raw magnetic field in NED body frame +float32[3] magnetometer2_ga # Magnetic field in NED body frame, in Gauss +uint64 magnetometer2_timestamp # Magnetometer timestamp +uint32 magnetometer2_errcount # Error counter for mag 2 +float32 magnetometer2_temp # Temperature of mag 2 + +float32 baro_pres_mbar # Barometric pressure, already temp. comp. +float32 baro_alt_meter # Altitude, already temp. comp. +float32 baro_temp_celcius # Temperature in degrees celsius +uint64 baro_timestamp # Barometer timestamp + +float32 baro1_pres_mbar # Barometric pressure, already temp. comp. +float32 baro1_alt_meter # Altitude, already temp. comp. +float32 baro1_temp_celcius # Temperature in degrees celsius +uint64 baro1_timestamp # Barometer timestamp + +float32[10] adc_voltage_v # ADC voltages of ADC Chan 10/11/12/13 or -1 +uint16[10] adc_mapping # Channel indices of each of these values +float32 mcu_temp_celcius # Internal temperature measurement of MCU + +float32 differential_pressure_pa # Airspeed sensor differential pressure +uint64 differential_pressure_timestamp # Last measurement timestamp +float32 differential_pressure_filtered_pa # Low pass filtered airspeed sensor differential pressure reading + +float32 differential_pressure1_pa # Airspeed sensor differential pressure +uint64 differential_pressure1_timestamp # Last measurement timestamp +float32 differential_pressure1_filtered_pa # Low pass filtered airspeed sensor differential pressure reading diff --git a/src/modules/simulator/simulator.cpp b/src/modules/simulator/simulator.cpp index 0c02da247f..9e75534236 100644 --- a/src/modules/simulator/simulator.cpp +++ b/src/modules/simulator/simulator.cpp @@ -114,11 +114,14 @@ int Simulator::start(int argc, char *argv[]) PX4_INFO("Simulator started"); drv_led_start(); if (argv[2][1] == 's') { + _instance->initializeSensorData(); #ifndef __PX4_QURT - _instance->updateSamples(); + // Update sensor data + _instance->pollForMAVLinkMessages(false); #endif } else { - _instance->publishSensorsCombined(); + // Update sensor data + _instance->pollForMAVLinkMessages(true); } } else { @@ -128,71 +131,6 @@ int Simulator::start(int argc, char *argv[]) return ret; } -void Simulator::publishSensorsCombined() { - - struct baro_report baro; - memset(&baro,0,sizeof(baro)); - baro.pressure = 120000.0f; - - // acceleration report - 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; - memset(&gyro, 0 ,sizeof(gyro)); - - // mag report - struct mag_report mag; - memset(&mag, 0 ,sizeof(mag)); - // init publishers - _baro_pub = orb_advertise(ORB_ID(sensor_baro), &baro); - _accel_pub = orb_advertise(ORB_ID(sensor_accel), &accel); - _gyro_pub = orb_advertise(ORB_ID(sensor_gyro), &gyro); - _mag_pub = orb_advertise(ORB_ID(sensor_mag), &mag); - - struct sensor_combined_s sensors; - memset(&sensors, 0, sizeof(sensors)); - // fill sensors with some data - sensors.accelerometer_m_s2[2] = 9.81f; - sensors.magnetometer_ga[0] = 0.2f; - sensors.timestamp = hrt_absolute_time(); - sensors.accelerometer_timestamp = hrt_absolute_time(); - sensors.magnetometer_timestamp = hrt_absolute_time(); - sensors.baro_timestamp = hrt_absolute_time(); - // advertise - _sensor_combined_pub = orb_advertise(ORB_ID(sensor_combined), &sensors); - - hrt_abstime time_last = hrt_absolute_time(); - uint64_t delta; - for(;;) { - delta = hrt_absolute_time() - time_last; - if(delta > (uint64_t)1000000) { - time_last = hrt_absolute_time(); - sensors.timestamp = time_last; - sensors.accelerometer_timestamp = time_last; - sensors.magnetometer_timestamp = time_last; - sensors.baro_timestamp = time_last; - baro.timestamp = time_last; - accel.timestamp = time_last; - gyro.timestamp = time_last; - mag.timestamp = time_last; - // publish the sensor values - //PX4_DEBUG("Publishing SensorsCombined\n"); - orb_publish(ORB_ID(sensor_combined), _sensor_combined_pub, &sensors); - orb_publish(ORB_ID(sensor_baro), _baro_pub, &baro); - orb_publish(ORB_ID(sensor_accel), _accel_pub, &baro); - orb_publish(ORB_ID(sensor_gyro), _gyro_pub, &baro); - orb_publish(ORB_ID(sensor_mag), _mag_pub, &mag); - } - else { - usleep(1000000-delta); - } - } -} - static void usage() { PX4_WARN("Usage: simulator {start -[sc] |stop}"); diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h index b2ebc880cd..5d9eaa1f4b 100644 --- a/src/modules/simulator/simulator.h +++ b/src/modules/simulator/simulator.h @@ -39,7 +39,7 @@ #pragma once #include -#include +#include #include #include #include @@ -52,11 +52,6 @@ #include #include #include -#ifndef __PX4_QURT -#include -#include -#endif - namespace simulator { // FIXME - what is the endianness of these on actual device? @@ -118,11 +113,11 @@ struct RawGPSData { template class Report { public: Report(int readers) : - _readidx(0), - _max_readers(readers), - _report_len(sizeof(RType)) + _readidx(0), + _max_readers(readers), + _report_len(sizeof(RType)) { - sem_init(&_lock, 0, _max_readers); + sem_init(&_lock, 0, _max_readers); } ~Report() {}; @@ -148,11 +143,11 @@ public: protected: void read_lock() { sem_wait(&_lock); } void read_unlock() { sem_post(&_lock); } - void write_lock() + void write_lock() { for (int i=0; i<_max_readers; i++) { - sem_wait(&_lock); - } + sem_wait(&_lock); + } } void write_unlock() { @@ -209,14 +204,15 @@ private: _baro(1), _mag(1), _gps(1), - _sensor_combined_pub(nullptr) #ifndef __PX4_QURT - , + _accel_pub(nullptr), + _baro_pub(nullptr), + _gyro_pub(nullptr), + _mag_pub(nullptr), _rc_channels_pub(nullptr), _actuator_outputs_sub(-1), _vehicle_attitude_sub(-1), _manual_sub(-1), - _sensor{}, _rc_input{}, _actuators{}, _attitude{}, @@ -225,17 +221,15 @@ private: {} ~Simulator() { _instance=NULL; } -#ifndef __PX4_QURT - void updateSamples(); -#endif + void initializeSensorData(); static Simulator *_instance; // simulated sensor instances - simulator::Report _accel; + simulator::Report _accel; simulator::Report _mpu; simulator::Report _baro; - simulator::Report _mag; + simulator::Report _mag; simulator::Report _gps; // uORB publisher handlers @@ -243,10 +237,9 @@ private: orb_advert_t _baro_pub; orb_advert_t _gyro_pub; orb_advert_t _mag_pub; - orb_advert_t _sensor_combined_pub; // class methods - void publishSensorsCombined(); + int publish_sensor_topics(mavlink_hil_sensor_t *imu); #ifndef __PX4_QURT // uORB publisher handlers @@ -258,23 +251,19 @@ private: int _manual_sub; // uORB data containers - struct sensor_combined_s _sensor; struct rc_input_values _rc_input; struct actuator_outputs_s _actuators; struct vehicle_attitude_s _attitude; struct manual_control_setpoint_s _manual; - int _fd; - unsigned char _buf[200]; - struct sockaddr_in _srcaddr; - socklen_t _addrlen = sizeof(_srcaddr); - void poll_actuators(); - void handle_message(mavlink_message_t *msg); + void handle_message(mavlink_message_t *msg, bool publish); void send_controls(); + void pollForMAVLinkMessages(bool publish); + void pack_actuator_message(mavlink_hil_controls_t &actuator_msg); void send_mavlink_message(const uint8_t msgid, const void *msg, uint8_t component_ID); - void update_sensors(struct sensor_combined_s *sensor, mavlink_hil_sensor_t *imu); + void update_sensors(mavlink_hil_sensor_t *imu); void update_gps(mavlink_hil_gps_t *gps_sim); static void *sending_trampoline(void *); void send(); diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index 0c411edca5..cc4b871f02 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -35,9 +35,10 @@ #include #include "simulator.h" #include "errno.h" +#include #include - -using namespace simulator; +#include +#include #define SEND_INTERVAL 20 #define UDP_PORT 14560 @@ -49,9 +50,17 @@ using namespace simulator; static const uint8_t mavlink_message_lengths[256] = MAVLINK_MESSAGE_LENGTHS; static const uint8_t mavlink_message_crcs[256] = MAVLINK_MESSAGE_CRCS; +static const float mg2ms2 = CONSTANTS_ONE_G / 1000.0f; static int openUart(const char *uart_name, int baud); +static int _fd; +static unsigned char _buf[200]; +sockaddr_in _srcaddr; +static socklen_t _addrlen = sizeof(_srcaddr); + +using namespace simulator; + void Simulator::pack_actuator_message(mavlink_hil_controls_t &actuator_msg) { float out[8]; @@ -93,6 +102,7 @@ void Simulator::pack_actuator_message(mavlink_hil_controls_t &actuator_msg) { 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); } @@ -102,6 +112,17 @@ static void fill_rc_input_msg(struct rc_input_values *rc, mavlink_rc_channels_t 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); +*/ + rc->values[0] = rc_channels->chan1_raw; rc->values[1] = rc_channels->chan2_raw; rc->values[2] = rc_channels->chan3_raw; @@ -122,7 +143,7 @@ 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(struct sensor_combined_s *sensor, 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; @@ -174,36 +195,42 @@ void Simulator::update_gps(mavlink_hil_gps_t *gps_sim) { gps.satellites_visible = gps_sim->satellites_visible; write_gps_data((void *)&gps); - } -void Simulator::handle_message(mavlink_message_t *msg) { +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); - update_sensors(&_sensor, &imu); - break; + 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); - update_gps(&gps_sim); - 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; - case MAVLINK_MSG_ID_RC_CHANNELS: + case MAVLINK_MSG_ID_RC_CHANNELS: + mavlink_rc_channels_t rc_channels; + mavlink_msg_rc_channels_decode(msg, &rc_channels); + fill_rc_input_msg(&_rc_input, &rc_channels); - mavlink_rc_channels_t rc_channels; - mavlink_msg_rc_channels_decode(msg, &rc_channels); - fill_rc_input_msg(&_rc_input, &rc_channels); - - // publish message + // publish message + if (publish) { 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; + } + break; } } @@ -246,6 +273,7 @@ void Simulator::poll_actuators() { bool updated; orb_check(_actuator_outputs_sub, &updated); if(updated) { + //PX4_WARN("Received actuator_output0 orb_topic"); orb_copy(ORB_ID(actuator_outputs), _actuator_outputs_sub, &_actuators); } } @@ -286,12 +314,8 @@ void Simulator::send() { } } -void Simulator::updateSamples() +void Simulator::initializeSensorData() { - // udp socket data - struct sockaddr_in _myaddr; - const int _port = UDP_PORT; - struct baro_report baro; memset(&baro,0,sizeof(baro)); baro.pressure = 120000.0f; @@ -309,6 +333,13 @@ void Simulator::updateSamples() // mag report struct mag_report mag; memset(&mag, 0 ,sizeof(mag)); +} + +void Simulator::pollForMAVLinkMessages(bool publish) +{ + // udp socket data + struct sockaddr_in _myaddr; + const int _port = UDP_PORT; // try to setup udp socket for communcation with simulator memset((char *)&_myaddr, 0, sizeof(_myaddr)); @@ -372,9 +403,11 @@ void Simulator::updateSamples() while (pret <= 0) { pret = ::poll(&fds[0], (sizeof(fds[0])/sizeof(fds[0])), 100); } + PX4_WARN("Found initial message, pret = %d",pret); if (fds[0].revents & POLLIN) { len = recvfrom(_fd, _buf, sizeof(_buf), 0, (struct sockaddr *)&_srcaddr, &_addrlen); + PX4_WARN("Sending initial controls message to jMAVSim."); send_controls(); } @@ -413,7 +446,7 @@ void Simulator::updateSamples() if (mavlink_parse_char(MAVLINK_COMM_0, _buf[i], &msg, &status)) { // have a message, handle it - handle_message(&msg); + handle_message(&msg, publish); } } } @@ -430,7 +463,7 @@ void Simulator::updateSamples() if (mavlink_parse_char(MAVLINK_COMM_0, serial_buf[i], &msg, &status)) { // have a message, handle it - handle_message(&msg); + handle_message(&msg, publish); } } } @@ -528,8 +561,8 @@ int openUart(const char *uart_name, int baud) } - // Make raw - cfmakeraw(&uart_config); + // Make raw + cfmakeraw(&uart_config); if ((termios_state = tcsetattr(uart_fd, TCSANOW, &uart_config)) < 0) { warnx("ERR SET CONF %s\n", uart_name); @@ -537,5 +570,113 @@ int openUart(const char *uart_name, int baud) return -1; } - return uart_fd; + return uart_fd; +} + +int Simulator::publish_sensor_topics(mavlink_hil_sensor_t *imu) { + + + //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); + } + /* + static int count=0; + static uint64_t last_timestamp=0; + count++; + if (!(count % 200)) { + PX4_WARN("TIME : %lu, dt: %lu", + (unsigned long) timestamp,(unsigned long) timestamp - (unsigned long) last_timestamp); + PX4_WARN("IMU : %f %f %f",imu->xgyro,imu->ygyro,imu->zgyro); + PX4_WARN("ACCEL: %f %f %f",imu->xacc,imu->yacc,imu->zacc); + PX4_WARN("MAG : %f %f %f",imu->xmag,imu->ymag,imu->zmag); + PX4_WARN("BARO : %f %f %f",imu->abs_pressure,imu->pressure_alt,imu->temperature); + } + last_timestamp = timestamp; + */ + /* gyro */ + { + struct gyro_report gyro; + memset(&gyro, 0, sizeof(gyro)); + + gyro.timestamp = timestamp; + gyro.x_raw = imu->xgyro * 1000.0f; + gyro.y_raw = imu->ygyro * 1000.0f; + gyro.z_raw = imu->zgyro * 1000.0f; + gyro.x = imu->xgyro; + gyro.y = imu->ygyro; + gyro.z = imu->zgyro; + + if (_gyro_pub == nullptr) { + _gyro_pub = orb_advertise(ORB_ID(sensor_gyro), &gyro); + + } else { + orb_publish(ORB_ID(sensor_gyro), _gyro_pub, &gyro); + } + } + + /* accelerometer */ + { + struct accel_report accel; + memset(&accel, 0, sizeof(accel)); + + accel.timestamp = timestamp; + accel.x_raw = imu->xacc / mg2ms2; + accel.y_raw = imu->yacc / mg2ms2; + accel.z_raw = imu->zacc / mg2ms2; + accel.x = imu->xacc; + accel.y = imu->yacc; + accel.z = imu->zacc; + + if (_accel_pub == nullptr) { + _accel_pub = orb_advertise(ORB_ID(sensor_accel), &accel); + + } else { + orb_publish(ORB_ID(sensor_accel), _accel_pub, &accel); + } + } + + /* magnetometer */ + { + struct mag_report mag; + memset(&mag, 0, sizeof(mag)); + + mag.timestamp = timestamp; + mag.x_raw = imu->xmag * 1000.0f; + mag.y_raw = imu->ymag * 1000.0f; + mag.z_raw = imu->zmag * 1000.0f; + mag.x = imu->xmag; + mag.y = imu->ymag; + mag.z = imu->zmag; + + if (_mag_pub == nullptr) { + /* publish to the first mag topic */ + _mag_pub = orb_advertise(ORB_ID(sensor_mag), &mag); + + } else { + orb_publish(ORB_ID(sensor_mag), _mag_pub, &mag); + } + } + + /* baro */ + { + struct baro_report baro; + memset(&baro, 0, sizeof(baro)); + + baro.timestamp = timestamp; + baro.pressure = imu->abs_pressure; + baro.altitude = imu->pressure_alt; + baro.temperature = imu->temperature; + + if (_baro_pub == nullptr) { + _baro_pub = orb_advertise(ORB_ID(sensor_baro), &baro); + + } else { + orb_publish(ORB_ID(sensor_baro), _baro_pub, &baro); + } + } + + return OK; }