From 17267a7f66c8392ab1a96e8c0921499b4a9ef473 Mon Sep 17 00:00:00 2001 From: tumbili Date: Sun, 3 May 2015 16:38:55 +0200 Subject: [PATCH 1/2] enable receiving mavlink highres imu message (via udp) from external simulator --- src/modules/simulator/module.mk | 8 +- src/modules/simulator/simulator.cpp | 142 +++++++++++++++++++++++++++- src/modules/simulator/simulator.h | 20 +++- 3 files changed, 167 insertions(+), 3 deletions(-) diff --git a/src/modules/simulator/module.mk b/src/modules/simulator/module.mk index 483204d5e4..0555c5dbd0 100644 --- a/src/modules/simulator/module.mk +++ b/src/modules/simulator/module.mk @@ -37,4 +37,10 @@ MODULE_COMMAND = simulator -SRCS = simulator.cpp +SRCS = simulator.cpp + +INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink + +EXTRACXXFLAGS = -Weffc++ -Wno-attributes -Wno-packed + +EXTRACFLAGS = -Wno-packed diff --git a/src/modules/simulator/simulator.cpp b/src/modules/simulator/simulator.cpp index 3f6f763193..2520f8a28a 100644 --- a/src/modules/simulator/simulator.cpp +++ b/src/modules/simulator/simulator.cpp @@ -99,6 +99,90 @@ int Simulator::start(int argc, char *argv[]) return ret; } +void Simulator::fill_sensors_from_imu_msg(struct sensor_combined_s *sensor, mavlink_highres_imu_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::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; +} + +void Simulator::handle_message(mavlink_message_t *msg) { + switch(msg->msgid) { + case MAVLINK_MSG_ID_HIGHRES_IMU: + mavlink_highres_imu_t imu; + mavlink_msg_highres_imu_decode(msg, &imu); + fill_sensors_from_imu_msg(&_sensor, &imu); + + // publish message + if(_sensor_combined_pub < 0) { + _sensor_combined_pub = orb_advertise(ORB_ID(sensor_combined), &_sensor); + } else { + orb_publish(ORB_ID(sensor_combined), _sensor_combined_pub, &_sensor); + } + break; + + case MAVLINK_MSG_ID_MANUAL_CONTROL: + + 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); + + // publish message + if(_manual_control_sp_pub < 0) { + _manual_control_sp_pub = orb_advertise(ORB_ID(manual_control_setpoint), &_manual_control_sp); + } else { + orb_publish(ORB_ID(manual_control_setpoint), _manual_control_sp_pub, &_manual_control_sp); + } + break; + } +} + void Simulator::publishSensorsCombined() { struct baro_report baro; @@ -168,13 +252,36 @@ void Simulator::publishSensorsCombined() { #ifndef __PX4_QURT void Simulator::updateSamples() { + 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); + // get samples from external provider struct sockaddr_in myaddr; struct sockaddr_in srcaddr; socklen_t addrlen = sizeof(srcaddr); int len, fd; const int buflen = 200; - const int port = 9876; + const int port = 14550; unsigned char buf[buflen]; if ((fd = socket(AF_INET, SOCK_DGRAM, 0)) < 0) { @@ -192,6 +299,38 @@ void Simulator::updateSamples() return; } + // wait for new mavlink messages to arrive + for (;;) { + len = 0; + len = recvfrom(fd, buf, buflen, 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)) + { + // have a message, handle it + handle_message(&msg); + } + } + } + + // 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 + //printf("Publishing SensorsCombined\n"); + 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); + } + + /* for (;;) { len = recvfrom(fd, buf, buflen, 0, (struct sockaddr *)&srcaddr, &addrlen); if (len > 0) { @@ -212,6 +351,7 @@ void Simulator::updateSamples() } } } + */ } #endif diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h index d2ca2fa8ab..97b2420b2a 100644 --- a/src/modules/simulator/simulator.h +++ b/src/modules/simulator/simulator.h @@ -40,11 +40,14 @@ #include #include +#include #include #include #include #include #include +#include +#include namespace simulator { @@ -151,13 +154,24 @@ public: bool getMPUReport(uint8_t *buf, int len); bool getBaroSample(uint8_t *buf, int len); private: - Simulator() : _accel(1), _mpu(1), _baro(1) {} + Simulator() : + _accel(1), + _mpu(1), + _baro(1), + _sensor_combined_pub(-1), + _manual_control_sp_pub(-1), + _sensor{}, + _manual_control_sp{} + {} ~Simulator() { _instance=NULL; } #ifndef __PX4_QURT void updateSamples(); #endif void publishSensorsCombined(); + void fill_sensors_from_imu_msg(struct sensor_combined_s *sensor, mavlink_highres_imu_t *imu); + void fill_manual_control_sp_msg(struct manual_control_setpoint_s *manual, mavlink_manual_control_t *man_msg); + void handle_message(mavlink_message_t *msg); static Simulator *_instance; @@ -170,5 +184,9 @@ private: orb_advert_t _gyro_pub; orb_advert_t _mag_pub; orb_advert_t _sensor_combined_pub; + orb_advert_t _manual_control_sp_pub; + + struct sensor_combined_s _sensor; + struct manual_control_setpoint_s _manual_control_sp; }; From 4b34d0c297b2953eff7af65e0e71e351fdadffda Mon Sep 17 00:00:00 2001 From: tumbili Date: Sun, 3 May 2015 23:30:18 +0200 Subject: [PATCH 2/2] improved topic listener --- Tools/generate_listener.py | 45 +++++++++++++++++++++++++++++++++----- 1 file changed, 39 insertions(+), 6 deletions(-) diff --git a/Tools/generate_listener.py b/Tools/generate_listener.py index 805b390901..50fc17267c 100755 --- a/Tools/generate_listener.py +++ b/Tools/generate_listener.py @@ -9,19 +9,33 @@ import sys raw_messages = glob.glob(sys.argv[1]+"/msg/*.msg") messages = [] -message_floats = [] +message_elements = [] for index,m in enumerate(raw_messages): - temp_list = [] + temp_list_floats = [] + temp_list_uint64 = [] + temp_list_bool = [] if("actuator_control" not in m and "pwm_input" not in m and "position_setpoint" not in m): + temp_list = [] f = open(m,'r') for line in f.readlines(): if(line.split(' ')[0] == "float32"): - temp_list.append(line.split(' ')[1].split('\t')[0].split('\n')[0]) + temp_list.append(("float",line.split(' ')[1].split('\t')[0].split('\n')[0])) + elif(line.split(' ')[0] == "uint64"): + temp_list.append(("uint64",line.split(' ')[1].split('\t')[0].split('\n')[0])) + elif (line.split(' ')[0] == "bool"): + temp_list.append(("bool",line.split(' ')[1].split('\t')[0].split('\n')[0])) + elif (line.split(' ')[0] == "uint8") and len(line.split('=')) == 1: + temp_list.append(("uint8",line.split(' ')[1].split('\t')[0].split('\n')[0])) + elif ('float32[' in line.split(' ')[0]): + num_floats = int(line.split(" ")[0].split("[")[1].split("]")[0]) + temp_list.append(("float_array",line.split(' ')[1].split('\t')[0].split('\n')[0],num_floats)) + f.close() messages.append(m.split('/')[-1].split('.')[0]) - message_floats.append(temp_list) + message_elements.append(temp_list) + num_messages = len(messages); print @@ -76,6 +90,7 @@ print """ #include #include #include +#include """ for m in messages: print "#include " % m @@ -104,10 +119,28 @@ for index,m in enumerate(messages[1:]): print "\t\tID = ORB_ID(%s);" % m print "\t\tstruct %s_s container;" % m print "\t\tmemset(&container, 0, sizeof(container));" + print "\t\tbool updated;" print "\t\tfor(uint32_t i = 0;i