mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-11-20 08:51:35 +08:00
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:
@@ -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);
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user