zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

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
sbg
tumbili 10 years ago committed by Mark Charlebois
parent
e7abd78051
commit
f0a3210e94
3 changed files with 204 additions and 109 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 21
      src/modules/simulator/simulator.cpp
  2. 65
      src/modules/simulator/simulator.h
  3. 231
      src/modules/simulator/simulator_mavlink.cpp

21
src/modules/simulator/simulator.cpp
Unescape View File

@ -71,11 +71,32 @@ bool Simulator::getRawAccelReport(uint8_t *buf, int len)
return _accel.copyData(buf, 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) bool Simulator::getBaroSample(uint8_t *buf, int len)
{ {
return _baro.copyData(buf, 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 Simulator::start(int argc, char *argv[])
{ {
int ret = 0; int ret = 0;

65
src/modules/simulator/simulator.h
Unescape View File

@ -48,6 +48,7 @@
#include <drivers/drv_baro.h> #include <drivers/drv_baro.h>
#include <drivers/drv_mag.h> #include <drivers/drv_mag.h>
#include <drivers/drv_hrt.h> #include <drivers/drv_hrt.h>
#include <drivers/drv_rc_input.h>
#include <uORB/uORB.h> #include <uORB/uORB.h>
#include <v1.0/mavlink_types.h> #include <v1.0/mavlink_types.h>
#include <v1.0/common/mavlink.h> #include <v1.0/common/mavlink.h>
@ -61,31 +62,46 @@ namespace simulator {
// FIXME - what is the endianness of these on actual device? // FIXME - what is the endianness of these on actual device?
#pragma pack(push, 1) #pragma pack(push, 1)
struct RawAccelData { struct RawAccelData {
int16_t x; float temperature;
int16_t y; float x;
int16_t z; 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(pop)
#pragma pack(push, 1) #pragma pack(push, 1)
struct RawMPUData { struct RawMPUData {
uint8_t accel_x[2]; float accel_x;
uint8_t accel_y[2]; float accel_y;
uint8_t accel_z[2]; float accel_z;
uint8_t temp[2]; float temp;
uint8_t gyro_x[2]; float gyro_x;
uint8_t gyro_y[2]; float gyro_y;
uint8_t gyro_z[2]; float gyro_z;
}; };
#pragma pack(pop) #pragma pack(pop)
#pragma pack(push, 1)
struct RawBaroData { struct RawBaroData {
uint8_t d[3]; float pressure;
float altitude;
float temperature;
}; };
#pragma pack(pop)
template <typename RType> class Report { template <typename RType> class Report {
public: public:
Report(int readers) : Report(int readers) :
_readidx(0),
_max_readers(readers), _max_readers(readers),
_report_len(sizeof(RType)) _report_len(sizeof(RType))
{ {
@ -158,23 +174,33 @@ public:
static int start(int argc, char *argv[]); static int start(int argc, char *argv[]);
bool getRawAccelReport(uint8_t *buf, int len); bool getRawAccelReport(uint8_t *buf, int len);
bool getMagReport(uint8_t *buf, int len);
bool getMPUReport(uint8_t *buf, int len); bool getMPUReport(uint8_t *buf, int len);
bool getBaroSample(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: private:
Simulator() : Simulator() :
_accel(1), _accel(1),
_mpu(1), _mpu(1),
_baro(1), _baro(1),
_mag(1),
_sensor_combined_pub(nullptr) _sensor_combined_pub(nullptr)
#ifndef __PX4_QURT #ifndef __PX4_QURT
, ,
_manual_control_sp_pub(nullptr), _rc_channels_pub(nullptr),
_actuator_outputs_sub(-1), _actuator_outputs_sub(-1),
_vehicle_attitude_sub(-1), _vehicle_attitude_sub(-1),
_manual_sub(-1),
_sensor{}, _sensor{},
_manual_control_sp{}, _rc_input{},
_actuators{}, _actuators{},
_attitude{} _attitude{},
_manual{}
#endif #endif
{} {}
~Simulator() { _instance=NULL; } ~Simulator() { _instance=NULL; }
@ -189,6 +215,7 @@ private:
simulator::Report<simulator::RawAccelData> _accel; simulator::Report<simulator::RawAccelData> _accel;
simulator::Report<simulator::RawMPUData> _mpu; simulator::Report<simulator::RawMPUData> _mpu;
simulator::Report<simulator::RawBaroData> _baro; simulator::Report<simulator::RawBaroData> _baro;
simulator::Report<simulator::RawMagData> _mag;
// uORB publisher handlers // uORB publisher handlers
orb_advert_t _accel_pub; orb_advert_t _accel_pub;
@ -202,21 +229,26 @@ private:
#ifndef __PX4_QURT #ifndef __PX4_QURT
// uORB publisher handlers // uORB publisher handlers
orb_advert_t _manual_control_sp_pub; orb_advert_t _rc_channels_pub;
// uORB subscription handlers // uORB subscription handlers
int _actuator_outputs_sub; int _actuator_outputs_sub;
int _vehicle_attitude_sub; int _vehicle_attitude_sub;
int _manual_sub;
// uORB data containers // uORB data containers
struct sensor_combined_s _sensor; 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 actuator_outputs_s _actuators;
struct vehicle_attitude_s _attitude; struct vehicle_attitude_s _attitude;
struct manual_control_setpoint_s _manual;
int _fd; int _fd;
unsigned char _buf[200]; unsigned char _buf[200];
hrt_abstime _time_last; hrt_abstime _time_last;
hrt_abstime _heartbeat_last;
hrt_abstime _attitude_last;
hrt_abstime _manual_last;
struct sockaddr_in _srcaddr; struct sockaddr_in _srcaddr;
socklen_t _addrlen = sizeof(_srcaddr); socklen_t _addrlen = sizeof(_srcaddr);
@ -225,6 +257,7 @@ private:
void send_data(); void send_data();
void pack_actuator_message(mavlink_hil_controls_t &actuator_msg); 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 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 *); static void *sending_trampoline(void *);
void send(); void send();
#endif #endif

231
src/modules/simulator/simulator_mavlink.cpp
Unescape View File

@ -85,68 +85,107 @@ void Simulator::pack_actuator_message(mavlink_hil_controls_t &actuator_msg) {
} }
void Simulator::send_data() { 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; mavlink_hil_controls_t msg;
pack_actuator_message(msg); pack_actuator_message(msg);
send_mavlink_message(MAVLINK_MSG_ID_HIL_CONTROLS, &msg, 200); send_mavlink_message(MAVLINK_MSG_ID_HIL_CONTROLS, &msg, 200);
// can add more messages here, can also setup different timings
} // send heartbeat
} if (hrt_elapsed_time(&_heartbeat_last) >= 1e6) {
_heartbeat_last = hrt_absolute_time();
static void fill_manual_control_sp_msg(struct manual_control_setpoint_s *manual, mavlink_manual_control_t *man_msg) { mavlink_heartbeat_t msg;
manual->timestamp = hrt_absolute_time(); msg.base_mode = 0;
manual->x = man_msg->x / 1000.0f; msg.custom_mode = 0;
manual->y = man_msg->y / 1000.0f; msg.autopilot = MAV_AUTOPILOT_PX4;
manual->r = man_msg->r / 1000.0f; msg.mavlink_version = 3;
manual->z = man_msg->z / 1000.0f; send_mavlink_message(MAVLINK_MSG_ID_HEARTBEAT, &msg, 200);
} }
static void fill_sensors_from_imu_msg(struct sensor_combined_s *sensor, mavlink_hil_sensor_t *imu) { // send attitude message
hrt_abstime timestamp = hrt_absolute_time(); if (hrt_elapsed_time(&_attitude_last) > 20000) {
sensor->timestamp = timestamp; _attitude_last = hrt_absolute_time();
sensor->gyro_raw[0] = imu->xgyro * 1000.0f; mavlink_attitude_t msg;
sensor->gyro_raw[1] = imu->ygyro * 1000.0f; msg.time_boot_ms = _attitude.timestamp / 1000;
sensor->gyro_raw[2] = imu->zgyro * 1000.0f; msg.roll = _attitude.roll;
sensor->gyro_rad_s[0] = imu->xgyro; msg.pitch = _attitude.pitch;
sensor->gyro_rad_s[1] = imu->ygyro; msg.yaw = _attitude.yaw;
sensor->gyro_rad_s[2] = imu->zgyro; msg.rollspeed = _attitude.rollspeed;
msg.pitchspeed = _attitude.pitchspeed;
sensor->accelerometer_raw[0] = imu->xacc; // mg2ms2; msg.yawspeed = _attitude.yawspeed;
sensor->accelerometer_raw[1] = imu->yacc; // mg2ms2; send_mavlink_message(MAVLINK_MSG_ID_ATTITUDE, &msg, 200);
sensor->accelerometer_raw[2] = imu->zacc; // mg2ms2; }
sensor->accelerometer_m_s2[0] = imu->xacc;
sensor->accelerometer_m_s2[1] = imu->yacc; // send manual control setpoint
sensor->accelerometer_m_s2[2] = imu->zacc; if (hrt_elapsed_time(&_manual_last) > 200000) {
sensor->accelerometer_mode = 0; // TODO what is this? _manual_last = hrt_absolute_time();
sensor->accelerometer_range_m_s2 = 32.7f; // int16 mavlink_manual_control_t msg;
sensor->accelerometer_timestamp = timestamp; msg.x = _manual.x * 1000;
sensor->timestamp = timestamp; msg.y = _manual.y * 1000;
msg.z = _manual.z * 1000;
sensor->adc_voltage_v[0] = 0.0f; msg.r = _manual.r * 1000;
sensor->adc_voltage_v[1] = 0.0f; msg.buttons = 0;
sensor->adc_voltage_v[2] = 0.0f; send_mavlink_message(MAVLINK_MSG_ID_MANUAL_CONTROL, &msg, 200);
}
sensor->magnetometer_raw[0] = imu->xmag * 1000.0f; }
sensor->magnetometer_raw[1] = imu->ymag * 1000.0f;
sensor->magnetometer_raw[2] = imu->zmag * 1000.0f; static void fill_rc_input_msg(struct rc_input_values *rc, mavlink_rc_channels_t *rc_channels) {
sensor->magnetometer_ga[0] = imu->xmag; rc->timestamp_publication = hrt_absolute_time();
sensor->magnetometer_ga[1] = imu->ymag; rc->timestamp_last_signal = hrt_absolute_time();
sensor->magnetometer_ga[2] = imu->zmag; rc->channel_count = rc_channels->chancount;
sensor->magnetometer_range_ga = 32.7f; // int16 rc->rssi = rc_channels->rssi;
sensor->magnetometer_mode = 0; // TODO what is this
sensor->magnetometer_cuttoff_freq_hz = 50.0f; rc->values[0] = rc_channels->chan1_raw;
sensor->magnetometer_timestamp = timestamp; rc->values[1] = rc_channels->chan2_raw;
rc->values[2] = rc_channels->chan3_raw;
sensor->baro_pres_mbar = imu->abs_pressure; rc->values[3] = rc_channels->chan4_raw;
sensor->baro_alt_meter = imu->pressure_alt; rc->values[4] = rc_channels->chan5_raw;
sensor->baro_temp_celcius = imu->temperature; rc->values[5] = rc_channels->chan6_raw;
sensor->baro_timestamp = timestamp; rc->values[6] = rc_channels->chan7_raw;
rc->values[7] = rc_channels->chan8_raw;
sensor->differential_pressure_pa = imu->diff_pressure * 1e2f; //from hPa to Pa rc->values[8] = rc_channels->chan9_raw;
sensor->differential_pressure_timestamp = timestamp; 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;
}
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) { 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: case MAVLINK_MSG_ID_HIL_SENSOR:
mavlink_hil_sensor_t imu; mavlink_hil_sensor_t imu;
mavlink_msg_hil_sensor_decode(msg, &imu); mavlink_msg_hil_sensor_decode(msg, &imu);
fill_sensors_from_imu_msg(&_sensor, &imu); update_sensors(&_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);
}
break; break;
case MAVLINK_MSG_ID_MANUAL_CONTROL: case MAVLINK_MSG_ID_RC_CHANNELS:
mavlink_manual_control_t man_ctrl_sp; mavlink_rc_channels_t rc_channels;
mavlink_msg_manual_control_decode(msg, &man_ctrl_sp); mavlink_msg_rc_channels_decode(msg, &rc_channels);
fill_manual_control_sp_msg(&_manual_control_sp, &man_ctrl_sp); fill_rc_input_msg(&_rc_input, &rc_channels);
// publish message // publish message
if(_manual_control_sp_pub == nullptr) { if(_rc_channels_pub == nullptr) {
_manual_control_sp_pub = orb_advertise(ORB_ID(manual_control_setpoint), &_manual_control_sp); _rc_channels_pub = orb_advertise(ORB_ID(input_rc), &_rc_input);
} else { } 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; break;
} }
@ -226,6 +258,12 @@ void Simulator::poll_topics() {
if(updated) { if(updated) {
orb_copy(ORB_ID(vehicle_attitude), _vehicle_attitude_sub, &_attitude); 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 *) { void *Simulator::sending_trampoline(void *) {
@ -238,11 +276,18 @@ void Simulator::send() {
fds[0].fd = _actuator_outputs_sub; fds[0].fd = _actuator_outputs_sub;
fds[0].events = POLLIN; 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) { while(true) {
// wait for up to 100ms for data // 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 //timed out
if (pret == 0) if (pret == 0)
@ -288,15 +333,10 @@ void Simulator::updateSamples()
struct mag_report mag; struct mag_report mag;
memset(&mag, 0 ,sizeof(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 // subscribe to topics
_actuator_outputs_sub = orb_subscribe(ORB_ID(actuator_outputs)); _actuator_outputs_sub = orb_subscribe(ORB_ID(actuator_outputs));
_vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); _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 // try to setup udp socket for communcation with simulator
memset((char *)&_myaddr, 0, sizeof(_myaddr)); memset((char *)&_myaddr, 0, sizeof(_myaddr));
@ -355,10 +395,23 @@ void Simulator::updateSamples()
fds[1].events = POLLIN; fds[1].events = POLLIN;
int len = 0; 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 // wait for new mavlink messages to arrive
while (true) { 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 //timed out
if (pret == 0) 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);
} }
} }

Write Preview
Loading…
Cancel
Save