diff --git a/src/modules/simulator/module.mk b/src/modules/simulator/module.mk index 0555c5dbd0..80f619477d 100644 --- a/src/modules/simulator/module.mk +++ b/src/modules/simulator/module.mk @@ -39,6 +39,10 @@ MODULE_COMMAND = simulator SRCS = simulator.cpp +ifneq ($(PX4_TARGET_OS), qurt) +SRCS += simulator_mavlink.cpp +endif + INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink EXTRACXXFLAGS = -Weffc++ -Wno-attributes -Wno-packed diff --git a/src/modules/simulator/simulator.cpp b/src/modules/simulator/simulator.cpp index e712234f1b..1920724fdf 100644 --- a/src/modules/simulator/simulator.cpp +++ b/src/modules/simulator/simulator.cpp @@ -56,9 +56,6 @@ static px4_task_t g_sim_task = -1; Simulator *Simulator::_instance = NULL; -static const uint8_t mavlink_message_lengths[256] = MAVLINK_MESSAGE_LENGTHS; -static const uint8_t mavlink_message_crcs[256] = MAVLINK_MESSAGE_CRCS; - Simulator *Simulator::getInstance() { return _instance; @@ -101,167 +98,6 @@ int Simulator::start(int argc, char *argv[]) return ret; } -void Simulator::poll_topics() { - // copy new 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_attitude_sub, &updated); - if(updated) { - orb_copy(ORB_ID(vehicle_attitude), _vehicle_attitude_sub, &_attitude); - } -} - -void Simulator::send_data() { - // check if it's time to send new data - hrt_abstime time_now = hrt_absolute_time(); - if (time_now - _time_last >= (hrt_abstime)(_interval * 1000)) { - _time_last = time_now; - mavlink_message_t msg; - pack_actuator_message(&msg); - send_mavlink_message(MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, &msg, 200); - // can add more messages here, can also setup different timings - } -} - -void Simulator::pack_actuator_message(mavlink_message_t *msg) { - // pack message and send - mavlink_servo_output_raw_t actuator_msg; - - actuator_msg.time_usec = hrt_absolute_time(); - actuator_msg.port = 8; // hardcoded for now - actuator_msg.servo1_raw = _actuators.output[0]; - actuator_msg.servo2_raw = _actuators.output[1]; - actuator_msg.servo3_raw = _actuators.output[2]; - actuator_msg.servo4_raw = _actuators.output[3]; - actuator_msg.servo5_raw = _actuators.output[4]; - actuator_msg.servo6_raw = _actuators.output[5]; - actuator_msg.servo7_raw = _actuators.output[6]; - actuator_msg.servo8_raw = _actuators.output[7]; - - // encode the message - mavlink_msg_servo_output_raw_encode(1, 100, msg, &actuator_msg); -} - -void Simulator::send_mavlink_message(const uint8_t msgid, const void *msg, uint8_t component_ID) { - uint8_t payload_len = mavlink_message_lengths[msgid]; - - uint8_t buf[MAVLINK_MAX_PACKET_LEN]; - - /* header */ - buf[0] = MAVLINK_STX; - buf[1] = payload_len; - /* no idea which numbers should be here*/ - buf[2] = 100; - buf[3] = 1; - buf[4] = component_ID; - buf[5] = msgid; - - /* payload */ - memcpy(&buf[MAVLINK_NUM_HEADER_BYTES],&msg, payload_len); - - /* checksum */ - uint16_t checksum; - crc_init(&checksum); - crc_accumulate_buffer(&checksum, (const char *) &buf[1], MAVLINK_CORE_HEADER_LEN + payload_len); - crc_accumulate(mavlink_message_crcs[msgid], &checksum); - - buf[MAVLINK_NUM_HEADER_BYTES + payload_len] = (uint8_t)(checksum & 0xFF); - buf[MAVLINK_NUM_HEADER_BYTES + payload_len + 1] = (uint8_t)(checksum >> 8); - - ssize_t len = sendto(_fd, buf, sizeof(buf), 0, (struct sockaddr *)&_srcaddr, _addrlen); - if (len <= 0) { - PX4_WARN("Failed sending mavlink message"); - } -} - -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; @@ -327,160 +163,6 @@ void Simulator::publishSensorsCombined() { } } -#ifndef __PX4_QURT - -void *Simulator::sending_trampoline(void *) { - _instance->send(); - return 0; // why do I have to put this??? -} - -void Simulator::send() { - px4_pollfd_struct_t fds[1]; - fds[0].fd = _actuator_outputs_sub; - fds[0].events = POLLIN; - - _time_last = hrt_absolute_time(); - - while(true) { - // wait for up to 100ms for data - int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); - - //timed out - if (pret == 0) - continue; - - // this is undesirable but not much we can do - if (pret < 0) { - PX4_WARN("poll error %d, %d", pret, errno); - // sleep a bit before next try - usleep(100000); - continue; - } - - if (fds[0].revents & POLLIN) { - // got new data to read, update all topics - poll_topics(); - send_data(); - } - } -} - -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); - - // subscribe to topics - _actuator_outputs_sub = orb_subscribe(ORB_ID(actuator_outputs)); - _vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - - // try to setup udp socket for communcation with simulator - memset((char *)&_myaddr, 0, sizeof(_myaddr)); - _myaddr.sin_family = AF_INET; - _myaddr.sin_addr.s_addr = htonl(INADDR_ANY); - _myaddr.sin_port = htons(_port); - - if ((_fd = socket(AF_INET, SOCK_DGRAM, 0)) < 0) { - PX4_WARN("create socket failed\n"); - return; - } - - if (bind(_fd, (struct sockaddr *)&_myaddr, sizeof(_myaddr)) < 0) { - PX4_WARN("bind failed\n"); - return; - } - - // create a thread for sending data to the simulator - pthread_t sender_thread; - - // initialize threads - pthread_attr_t sender_thread_attr; - pthread_attr_init(&sender_thread_attr); - pthread_attr_setstacksize(&sender_thread_attr, 1000); - - struct sched_param param; - (void)pthread_attr_getschedparam(&sender_thread_attr, ¶m); - - /* low priority */ - param.sched_priority = SCHED_PRIORITY_DEFAULT - 30; - (void)pthread_attr_setschedparam(&sender_thread_attr, ¶m); - pthread_create(&sender_thread, &sender_thread_attr, Simulator::sending_trampoline, NULL); - pthread_attr_destroy(&sender_thread_attr); - - struct pollfd socket_fds; - socket_fds.fd = _fd; - socket_fds.events = POLLIN; - - int len = 0; - // wait for new mavlink messages to arrive - while (true) { - - int socket_pret = ::poll(&socket_fds, (size_t)1, 100); - - //timed out - if (socket_pret == 0) - continue; - - // this is undesirable but not much we can do - if (socket_pret < 0) { - PX4_WARN("poll error %d, %d", socket_pret, errno); - // sleep a bit before next try - usleep(100000); - continue; - } - - if (socket_fds.revents & POLLIN) { - 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 - 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); - } -} -#endif - static void usage() { PX4_WARN("Usage: simulator {start -[sc] |stop}"); @@ -502,19 +184,7 @@ int simulator_main(int argc, char *argv[]) { int ret = 0; if (argc == 3 && strcmp(argv[1], "start") == 0) { - if (strcmp(argv[2], "-s") == 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); - } - else if (strcmp(argv[2], "-p") == 0) { + if (strcmp(argv[2], "-s") == 0 || strcmp(argv[2], "-p") == 0) { if (g_sim_task >= 0) { warnx("Simulator already started"); return 0; diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h index fe2e466c69..93f5327e83 100644 --- a/src/modules/simulator/simulator.h +++ b/src/modules/simulator/simulator.h @@ -51,8 +51,8 @@ #include #include #include -#include #ifndef __PX4_QURT +#include #include #endif @@ -165,18 +165,18 @@ private: _accel(1), _mpu(1), _baro(1), - _sensor_combined_pub(-1), + _sensor_combined_pub(-1) +#ifndef __PX4_QURT + , _manual_control_sp_pub(-1), _actuator_outputs_sub(-1), _vehicle_attitude_sub(-1), _sensor{}, _manual_control_sp{}, _actuators{}, - _attitude{}, - _interval(1000) - { - _buf = new unsigned char [_buflen]; - } + _attitude{} +#endif + {} ~Simulator() { _instance=NULL; } #ifndef __PX4_QURT @@ -196,6 +196,12 @@ private: orb_advert_t _gyro_pub; orb_advert_t _mag_pub; orb_advert_t _sensor_combined_pub; + + // class methods + void publishSensorsCombined(); + +#ifndef __PX4_QURT + // uORB publisher handlers orb_advert_t _manual_control_sp_pub; // uORB subscription handlers @@ -208,29 +214,18 @@ private: struct actuator_outputs_s _actuators; struct vehicle_attitude_s _attitude; - // udp socket data - struct sockaddr_in _myaddr; + int _fd; + unsigned char _buf[200]; + hrt_abstime _time_last; struct sockaddr_in _srcaddr; socklen_t _addrlen = sizeof(_srcaddr); - int _fd; - const int _buflen = 200; - const int _port = 14550; - unsigned char *_buf; - hrt_abstime _time_last; - int _interval; - - // class methods - int setup_udp_socket(); - void do_subscriptions(); void poll_topics(); - 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); void send_data(); - void send_mavlink_message(const uint8_t msgid, const void *msg, uint8_t component_ID); void pack_actuator_message(mavlink_message_t *msg); + void send_mavlink_message(const uint8_t msgid, const void *msg, uint8_t component_ID); static void *sending_trampoline(void *); void send(); +#endif }; diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp new file mode 100644 index 0000000000..a5aea30cd6 --- /dev/null +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -0,0 +1,361 @@ +/**************************************************************************** + * + * Copyright (c) 2015 Mark Charlebois. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include +#include +#include "simulator.h" +#include "errno.h" + +using namespace simulator; + +#define SEND_INTERVAL 1000 +#define UDP_PORT 14550 + +static const uint8_t mavlink_message_lengths[256] = MAVLINK_MESSAGE_LENGTHS; +static const uint8_t mavlink_message_crcs[256] = MAVLINK_MESSAGE_CRCS; + +void Simulator::pack_actuator_message(mavlink_message_t *msg) { + // pack message and send + mavlink_servo_output_raw_t actuator_msg; + + actuator_msg.time_usec = hrt_absolute_time(); + actuator_msg.port = 8; // hardcoded for now + actuator_msg.servo1_raw = _actuators.output[0]; + actuator_msg.servo2_raw = _actuators.output[1]; + actuator_msg.servo3_raw = _actuators.output[2]; + actuator_msg.servo4_raw = _actuators.output[3]; + actuator_msg.servo5_raw = _actuators.output[4]; + actuator_msg.servo6_raw = _actuators.output[5]; + actuator_msg.servo7_raw = _actuators.output[6]; + actuator_msg.servo8_raw = _actuators.output[7]; + + // encode the message + mavlink_msg_servo_output_raw_encode(1, 100, msg, &actuator_msg); +} + +void Simulator::send_data() { + // check if it's time to send new data + hrt_abstime time_now = hrt_absolute_time(); + if (time_now - _time_last >= (hrt_abstime)(SEND_INTERVAL * 1000)) { + _time_last = time_now; + mavlink_message_t msg; + pack_actuator_message(&msg); + send_mavlink_message(MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, &msg, 200); + // can add more messages here, can also setup different timings + } +} + +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; +} + +void 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::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::send_mavlink_message(const uint8_t msgid, const void *msg, uint8_t component_ID) { + uint8_t payload_len = mavlink_message_lengths[msgid]; + + uint8_t buf[MAVLINK_MAX_PACKET_LEN]; + + /* header */ + buf[0] = MAVLINK_STX; + buf[1] = payload_len; + /* no idea which numbers should be here*/ + buf[2] = 100; + buf[3] = 1; + buf[4] = component_ID; + buf[5] = msgid; + + /* payload */ + memcpy(&buf[MAVLINK_NUM_HEADER_BYTES],&msg, payload_len); + + /* checksum */ + uint16_t checksum; + crc_init(&checksum); + crc_accumulate_buffer(&checksum, (const char *) &buf[1], MAVLINK_CORE_HEADER_LEN + payload_len); + crc_accumulate(mavlink_message_crcs[msgid], &checksum); + + buf[MAVLINK_NUM_HEADER_BYTES + payload_len] = (uint8_t)(checksum & 0xFF); + buf[MAVLINK_NUM_HEADER_BYTES + payload_len + 1] = (uint8_t)(checksum >> 8); + + ssize_t len = sendto(_fd, buf, sizeof(buf), 0, (struct sockaddr *)&_srcaddr, _addrlen); + if (len <= 0) { + PX4_WARN("Failed sending mavlink message"); + } +} + +void Simulator::poll_topics() { + // copy new 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_attitude_sub, &updated); + if(updated) { + orb_copy(ORB_ID(vehicle_attitude), _vehicle_attitude_sub, &_attitude); + } +} + +void *Simulator::sending_trampoline(void *) { + _instance->send(); + return 0; // why do I have to put this??? +} + +void Simulator::send() { + px4_pollfd_struct_t fds[1]; + fds[0].fd = _actuator_outputs_sub; + fds[0].events = POLLIN; + + _time_last = hrt_absolute_time(); + + while(true) { + // wait for up to 100ms for data + int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); + + //timed out + if (pret == 0) + continue; + + // this is undesirable but not much we can do + if (pret < 0) { + PX4_WARN("poll error %d, %d", pret, errno); + // sleep a bit before next try + usleep(100000); + continue; + } + + if (fds[0].revents & POLLIN) { + // got new data to read, update all topics + poll_topics(); + send_data(); + } + } +} + +void Simulator::updateSamples() +{ + // 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; + + // 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); + + // subscribe to topics + _actuator_outputs_sub = orb_subscribe(ORB_ID(actuator_outputs)); + _vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + + // try to setup udp socket for communcation with simulator + memset((char *)&_myaddr, 0, sizeof(_myaddr)); + _myaddr.sin_family = AF_INET; + _myaddr.sin_addr.s_addr = htonl(INADDR_ANY); + _myaddr.sin_port = htons(_port); + + if ((_fd = socket(AF_INET, SOCK_DGRAM, 0)) < 0) { + PX4_WARN("create socket failed\n"); + return; + } + + if (bind(_fd, (struct sockaddr *)&_myaddr, sizeof(_myaddr)) < 0) { + PX4_WARN("bind failed\n"); + return; + } + + // create a thread for sending data to the simulator + pthread_t sender_thread; + + // initialize threads + pthread_attr_t sender_thread_attr; + pthread_attr_init(&sender_thread_attr); + pthread_attr_setstacksize(&sender_thread_attr, 1000); + + struct sched_param param; + (void)pthread_attr_getschedparam(&sender_thread_attr, ¶m); + + /* low priority */ + param.sched_priority = SCHED_PRIORITY_DEFAULT - 30; + (void)pthread_attr_setschedparam(&sender_thread_attr, ¶m); + pthread_create(&sender_thread, &sender_thread_attr, Simulator::sending_trampoline, NULL); + pthread_attr_destroy(&sender_thread_attr); + + struct pollfd socket_fds; + socket_fds.fd = _fd; + socket_fds.events = POLLIN; + + int len = 0; + // wait for new mavlink messages to arrive + while (true) { + + int socket_pret = ::poll(&socket_fds, (size_t)1, 100); + + //timed out + if (socket_pret == 0) + continue; + + // this is undesirable but not much we can do + if (socket_pret < 0) { + PX4_WARN("poll error %d, %d", socket_pret, errno); + // sleep a bit before next try + usleep(100000); + continue; + } + + if (socket_fds.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)) + { + // 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 + 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); + } +}