From f0a3210e94a8385d0a1868695cf442c42c076d4f Mon Sep 17 00:00:00 2001 From: tumbili Date: Sun, 31 May 2015 18:15:56 +0200 Subject: [PATCH] major simulator rework: - wait for first message from jMAVSim before sending data - publish raw rc data coming from PIXHAWK (temporary) - send some interesting messages to jMAVSim - prepare sensor data for sim drivers to read --- src/modules/simulator/simulator.cpp | 21 ++ src/modules/simulator/simulator.h | 65 ++++-- src/modules/simulator/simulator_mavlink.cpp | 227 ++++++++++++-------- 3 files changed, 204 insertions(+), 109 deletions(-) diff --git a/src/modules/simulator/simulator.cpp b/src/modules/simulator/simulator.cpp index 83e3fd8d76..da0b41589d 100644 --- a/src/modules/simulator/simulator.cpp +++ b/src/modules/simulator/simulator.cpp @@ -71,11 +71,32 @@ bool Simulator::getRawAccelReport(uint8_t *buf, int len) return _accel.copyData(buf, len); } +bool Simulator::getMagReport(uint8_t *buf, int len) +{ + return _mag.copyData(buf, len); +} + bool Simulator::getBaroSample(uint8_t *buf, int len) { return _baro.copyData(buf, len); } +void Simulator::write_MPU_data(void *buf) { + _mpu.writeData(buf); +} + +void Simulator::write_accel_data(void *buf) { + _accel.writeData(buf); +} + +void Simulator::write_mag_data(void *buf) { + _mag.writeData(buf); +} + +void Simulator::write_baro_data(void *buf) { + _baro.writeData(buf); +} + int Simulator::start(int argc, char *argv[]) { int ret = 0; diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h index a95fa15731..109c9f301c 100644 --- a/src/modules/simulator/simulator.h +++ b/src/modules/simulator/simulator.h @@ -48,6 +48,7 @@ #include #include #include +#include #include #include #include @@ -61,31 +62,46 @@ namespace simulator { // FIXME - what is the endianness of these on actual device? #pragma pack(push, 1) struct RawAccelData { - int16_t x; - int16_t y; - int16_t 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; }; #pragma pack(pop) #pragma pack(push, 1) struct RawMPUData { - uint8_t accel_x[2]; - uint8_t accel_y[2]; - uint8_t accel_z[2]; - uint8_t temp[2]; - uint8_t gyro_x[2]; - uint8_t gyro_y[2]; - uint8_t gyro_z[2]; + float accel_x; + float accel_y; + float accel_z; + float temp; + float gyro_x; + float gyro_y; + float gyro_z; }; #pragma pack(pop) +#pragma pack(push, 1) struct RawBaroData { - uint8_t d[3]; + float pressure; + float altitude; + float temperature; }; +#pragma pack(pop) template class Report { public: Report(int readers) : + _readidx(0), _max_readers(readers), _report_len(sizeof(RType)) { @@ -158,23 +174,33 @@ public: static int start(int argc, char *argv[]); bool getRawAccelReport(uint8_t *buf, int len); + bool getMagReport(uint8_t *buf, int len); bool getMPUReport(uint8_t *buf, int len); bool getBaroSample(uint8_t *buf, int len); + + void write_MPU_data(void *buf); + void write_accel_data(void *buf); + void write_mag_data(void *buf); + void write_baro_data(void *buf); + private: Simulator() : _accel(1), _mpu(1), _baro(1), + _mag(1), _sensor_combined_pub(nullptr) #ifndef __PX4_QURT , - _manual_control_sp_pub(nullptr), + _rc_channels_pub(nullptr), _actuator_outputs_sub(-1), _vehicle_attitude_sub(-1), + _manual_sub(-1), _sensor{}, - _manual_control_sp{}, + _rc_input{}, _actuators{}, - _attitude{} + _attitude{}, + _manual{} #endif {} ~Simulator() { _instance=NULL; } @@ -189,6 +215,7 @@ private: simulator::Report _accel; simulator::Report _mpu; simulator::Report _baro; + simulator::Report _mag; // uORB publisher handlers orb_advert_t _accel_pub; @@ -202,21 +229,26 @@ private: #ifndef __PX4_QURT // uORB publisher handlers - orb_advert_t _manual_control_sp_pub; + orb_advert_t _rc_channels_pub; // uORB subscription handlers int _actuator_outputs_sub; int _vehicle_attitude_sub; + int _manual_sub; // uORB data containers struct sensor_combined_s _sensor; - struct manual_control_setpoint_s _manual_control_sp; + 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]; hrt_abstime _time_last; + hrt_abstime _heartbeat_last; + hrt_abstime _attitude_last; + hrt_abstime _manual_last; struct sockaddr_in _srcaddr; socklen_t _addrlen = sizeof(_srcaddr); @@ -225,6 +257,7 @@ private: void send_data(); 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); static void *sending_trampoline(void *); void send(); #endif diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index de24fb8626..5c9ceae0cc 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -85,68 +85,107 @@ void Simulator::pack_actuator_message(mavlink_hil_controls_t &actuator_msg) { } void Simulator::send_data() { - // check if it's time to send new data - hrt_abstime time_now = hrt_absolute_time(); - if (true) {//time_now - _time_last >= (hrt_abstime)(SEND_INTERVAL * 1000)) { - _time_last = time_now; - mavlink_hil_controls_t msg; - pack_actuator_message(msg); - send_mavlink_message(MAVLINK_MSG_ID_HIL_CONTROLS, &msg, 200); - // can add more messages here, can also setup different timings + mavlink_hil_controls_t msg; + pack_actuator_message(msg); + send_mavlink_message(MAVLINK_MSG_ID_HIL_CONTROLS, &msg, 200); + + // send heartbeat + if (hrt_elapsed_time(&_heartbeat_last) >= 1e6) { + _heartbeat_last = hrt_absolute_time(); + mavlink_heartbeat_t msg; + msg.base_mode = 0; + msg.custom_mode = 0; + msg.autopilot = MAV_AUTOPILOT_PX4; + msg.mavlink_version = 3; + send_mavlink_message(MAVLINK_MSG_ID_HEARTBEAT, &msg, 200); + } + + // send attitude message + if (hrt_elapsed_time(&_attitude_last) > 20000) { + _attitude_last = hrt_absolute_time(); + mavlink_attitude_t msg; + msg.time_boot_ms = _attitude.timestamp / 1000; + msg.roll = _attitude.roll; + msg.pitch = _attitude.pitch; + msg.yaw = _attitude.yaw; + msg.rollspeed = _attitude.rollspeed; + msg.pitchspeed = _attitude.pitchspeed; + msg.yawspeed = _attitude.yawspeed; + send_mavlink_message(MAVLINK_MSG_ID_ATTITUDE, &msg, 200); + } + + // send manual control setpoint + if (hrt_elapsed_time(&_manual_last) > 200000) { + _manual_last = hrt_absolute_time(); + mavlink_manual_control_t msg; + msg.x = _manual.x * 1000; + msg.y = _manual.y * 1000; + msg.z = _manual.z * 1000; + msg.r = _manual.r * 1000; + msg.buttons = 0; + send_mavlink_message(MAVLINK_MSG_ID_MANUAL_CONTROL, &msg, 200); } } -static void fill_manual_control_sp_msg(struct manual_control_setpoint_s *manual, mavlink_manual_control_t *man_msg) { - manual->timestamp = hrt_absolute_time(); - manual->x = man_msg->x / 1000.0f; - manual->y = man_msg->y / 1000.0f; - manual->r = man_msg->r / 1000.0f; - manual->z = man_msg->z / 1000.0f; +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; + + rc->values[0] = rc_channels->chan1_raw; + rc->values[1] = rc_channels->chan2_raw; + rc->values[2] = rc_channels->chan3_raw; + rc->values[3] = rc_channels->chan4_raw; + rc->values[4] = rc_channels->chan5_raw; + rc->values[5] = rc_channels->chan6_raw; + rc->values[6] = rc_channels->chan7_raw; + rc->values[7] = rc_channels->chan8_raw; + rc->values[8] = rc_channels->chan9_raw; + rc->values[9] = rc_channels->chan10_raw; + rc->values[10] = rc_channels->chan11_raw; + rc->values[11] = rc_channels->chan12_raw; + rc->values[12] = rc_channels->chan13_raw; + rc->values[13] = rc_channels->chan14_raw; + rc->values[14] = rc_channels->chan15_raw; + rc->values[15] = rc_channels->chan16_raw; + rc->values[16] = rc_channels->chan17_raw; + rc->values[17] = rc_channels->chan18_raw; } -static void fill_sensors_from_imu_msg(struct sensor_combined_s *sensor, mavlink_hil_sensor_t *imu) { - hrt_abstime timestamp = hrt_absolute_time(); - sensor->timestamp = timestamp; - sensor->gyro_raw[0] = imu->xgyro * 1000.0f; - sensor->gyro_raw[1] = imu->ygyro * 1000.0f; - sensor->gyro_raw[2] = imu->zgyro * 1000.0f; - sensor->gyro_rad_s[0] = imu->xgyro; - sensor->gyro_rad_s[1] = imu->ygyro; - sensor->gyro_rad_s[2] = imu->zgyro; - - sensor->accelerometer_raw[0] = imu->xacc; // mg2ms2; - sensor->accelerometer_raw[1] = imu->yacc; // mg2ms2; - sensor->accelerometer_raw[2] = imu->zacc; // mg2ms2; - sensor->accelerometer_m_s2[0] = imu->xacc; - sensor->accelerometer_m_s2[1] = imu->yacc; - sensor->accelerometer_m_s2[2] = imu->zacc; - sensor->accelerometer_mode = 0; // TODO what is this? - sensor->accelerometer_range_m_s2 = 32.7f; // int16 - sensor->accelerometer_timestamp = timestamp; - sensor->timestamp = timestamp; - - sensor->adc_voltage_v[0] = 0.0f; - sensor->adc_voltage_v[1] = 0.0f; - sensor->adc_voltage_v[2] = 0.0f; - - sensor->magnetometer_raw[0] = imu->xmag * 1000.0f; - sensor->magnetometer_raw[1] = imu->ymag * 1000.0f; - sensor->magnetometer_raw[2] = imu->zmag * 1000.0f; - sensor->magnetometer_ga[0] = imu->xmag; - sensor->magnetometer_ga[1] = imu->ymag; - sensor->magnetometer_ga[2] = imu->zmag; - sensor->magnetometer_range_ga = 32.7f; // int16 - sensor->magnetometer_mode = 0; // TODO what is this - sensor->magnetometer_cuttoff_freq_hz = 50.0f; - sensor->magnetometer_timestamp = timestamp; - - sensor->baro_pres_mbar = imu->abs_pressure; - sensor->baro_alt_meter = imu->pressure_alt; - sensor->baro_temp_celcius = imu->temperature; - sensor->baro_timestamp = timestamp; - - sensor->differential_pressure_pa = imu->diff_pressure * 1e2f; //from hPa to Pa - sensor->differential_pressure_timestamp = timestamp; +void Simulator::update_sensors(struct sensor_combined_s *sensor, 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; + mpu.accel_y = imu->yacc; + mpu.accel_z = imu->zacc; + mpu.temp = imu->temperature; + mpu.gyro_x = imu->xgyro; + mpu.gyro_y = imu->ygyro; + mpu.gyro_z = imu->zgyro; + + write_MPU_data((void *)&mpu); + + RawAccelData accel; + accel.x = imu->xacc; + accel.y = imu->yacc; + accel.z = imu->zacc; + + write_accel_data((void *)&accel); + + RawMagData mag; + mag.x = imu->xmag; + mag.y = imu->ymag; + mag.z = imu->zmag; + + write_mag_data((void *)&mag); + + RawBaroData baro; + baro.pressure = imu->abs_pressure; + baro.altitude = imu->pressure_alt; + baro.temperature = imu->temperature; + + write_baro_data((void *)&baro); } void Simulator::handle_message(mavlink_message_t *msg) { @@ -154,27 +193,20 @@ void Simulator::handle_message(mavlink_message_t *msg) { case MAVLINK_MSG_ID_HIL_SENSOR: mavlink_hil_sensor_t imu; mavlink_msg_hil_sensor_decode(msg, &imu); - fill_sensors_from_imu_msg(&_sensor, &imu); - - // publish message - if(_sensor_combined_pub == nullptr) { - _sensor_combined_pub = orb_advertise(ORB_ID(sensor_combined), &_sensor); - } else { - orb_publish(ORB_ID(sensor_combined), _sensor_combined_pub, &_sensor); - } + update_sensors(&_sensor, &imu); break; - case MAVLINK_MSG_ID_MANUAL_CONTROL: + case MAVLINK_MSG_ID_RC_CHANNELS: - mavlink_manual_control_t man_ctrl_sp; - mavlink_msg_manual_control_decode(msg, &man_ctrl_sp); - fill_manual_control_sp_msg(&_manual_control_sp, &man_ctrl_sp); + mavlink_rc_channels_t rc_channels; + mavlink_msg_rc_channels_decode(msg, &rc_channels); + fill_rc_input_msg(&_rc_input, &rc_channels); // publish message - if(_manual_control_sp_pub == nullptr) { - _manual_control_sp_pub = orb_advertise(ORB_ID(manual_control_setpoint), &_manual_control_sp); + if(_rc_channels_pub == nullptr) { + _rc_channels_pub = orb_advertise(ORB_ID(input_rc), &_rc_input); } else { - orb_publish(ORB_ID(manual_control_setpoint), _manual_control_sp_pub, &_manual_control_sp); + orb_publish(ORB_ID(input_rc), _rc_channels_pub, &_rc_input); } break; } @@ -226,6 +258,12 @@ void Simulator::poll_topics() { if(updated) { orb_copy(ORB_ID(vehicle_attitude), _vehicle_attitude_sub, &_attitude); } + + orb_check(_manual_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(manual_control_setpoint), _manual_sub, &_manual); + } } void *Simulator::sending_trampoline(void *) { @@ -238,11 +276,18 @@ void Simulator::send() { fds[0].fd = _actuator_outputs_sub; fds[0].events = POLLIN; - _time_last = hrt_absolute_time(); + _time_last = 0; + _heartbeat_last = 0; + _attitude_last = 0; + _manual_last = 0; + int pret = -1; + while(pret <= 0) { + pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 1000); + } while(true) { // wait for up to 100ms for data - int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); + pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); //timed out if (pret == 0) @@ -288,15 +333,10 @@ void Simulator::updateSamples() 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); - // subscribe to topics _actuator_outputs_sub = orb_subscribe(ORB_ID(actuator_outputs)); _vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + _manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); // try to setup udp socket for communcation with simulator memset((char *)&_myaddr, 0, sizeof(_myaddr)); @@ -355,10 +395,23 @@ void Simulator::updateSamples() fds[1].events = POLLIN; int len = 0; + + // wait for first data from simulator and respond with first controls + // this is important for the UDP communication to work + int pret = -1; + while (pret <= 0) { + pret = ::poll(&fds[0], (sizeof(fds[0])/sizeof(fds[0])), 100); + } + + if (fds[0].revents & POLLIN) { + len = recvfrom(_fd, _buf, sizeof(_buf), 0, (struct sockaddr *)&_srcaddr, &_addrlen); + send_data(); + } + // wait for new mavlink messages to arrive while (true) { - int 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) @@ -405,17 +458,5 @@ void Simulator::updateSamples() } } } - - // publish these messages so that attitude estimator does not complain - hrt_abstime time_last = hrt_absolute_time(); - baro.timestamp = time_last; - accel.timestamp = time_last; - gyro.timestamp = time_last; - mag.timestamp = time_last; - // publish the sensor values - 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); } }