forked from Archive/PX4-Autopilot
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
This commit is contained in:
tumbili
Mark Charlebois
parent
e7abd78051
commit
f0a3210e94
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -48,6 +48,7 @@
|
|||
#include <drivers/drv_baro.h>
|
||||
#include <drivers/drv_mag.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <drivers/drv_rc_input.h>
|
||||
#include <uORB/uORB.h>
|
||||
#include <v1.0/mavlink_types.h>
|
||||
#include <v1.0/common/mavlink.h>
|
||||
|
|
@ -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 <typename RType> 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<simulator::RawAccelData> _accel;
|
||||
simulator::Report<simulator::RawMPUData> _mpu;
|
||||
simulator::Report<simulator::RawBaroData> _baro;
|
||||
simulator::Report<simulator::RawMagData> _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
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
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;
|
||||
|
||||
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;
|
||||
write_MPU_data((void *)&mpu);
|
||||
|
||||
sensor->adc_voltage_v[0] = 0.0f;
|
||||
sensor->adc_voltage_v[1] = 0.0f;
|
||||
sensor->adc_voltage_v[2] = 0.0f;
|
||||
RawAccelData accel;
|
||||
accel.x = imu->xacc;
|
||||
accel.y = imu->yacc;
|
||||
accel.z = imu->zacc;
|
||||
|
||||
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;
|
||||
write_accel_data((void *)&accel);
|
||||
|
||||
sensor->baro_pres_mbar = imu->abs_pressure;
|
||||
sensor->baro_alt_meter = imu->pressure_alt;
|
||||
sensor->baro_temp_celcius = imu->temperature;
|
||||
sensor->baro_timestamp = timestamp;
|
||||
RawMagData mag;
|
||||
mag.x = imu->xmag;
|
||||
mag.y = imu->ymag;
|
||||
mag.z = imu->zmag;
|
||||
|
||||
sensor->differential_pressure_pa = imu->diff_pressure * 1e2f; //from hPa to Pa
|
||||
sensor->differential_pressure_timestamp = timestamp;
|
||||
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);
|
||||
}
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue