Browse Source

Code style in MAVLink simulator

sbg
Lorenz Meier 9 years ago
parent
commit
155a0d7f18
  1. 31
      src/modules/simulator/simulator_mavlink.cpp

31
src/modules/simulator/simulator_mavlink.cpp

@ -182,7 +182,7 @@ void Simulator::update_sensors(mavlink_hil_sensor_t *imu)
mpu.gyro_y = imu->ygyro; mpu.gyro_y = imu->ygyro;
mpu.gyro_z = imu->zgyro; mpu.gyro_z = imu->zgyro;
write_MPU_data((void *)&mpu); write_MPU_data(&mpu);
perf_begin(_perf_mpu); perf_begin(_perf_mpu);
RawAccelData accel = {}; RawAccelData accel = {};
@ -190,7 +190,7 @@ void Simulator::update_sensors(mavlink_hil_sensor_t *imu)
accel.y = imu->yacc; accel.y = imu->yacc;
accel.z = imu->zacc; accel.z = imu->zacc;
write_accel_data((void *)&accel); write_accel_data(&accel);
perf_begin(_perf_accel); perf_begin(_perf_accel);
RawMagData mag = {}; RawMagData mag = {};
@ -198,7 +198,7 @@ void Simulator::update_sensors(mavlink_hil_sensor_t *imu)
mag.y = imu->ymag; mag.y = imu->ymag;
mag.z = imu->zmag; mag.z = imu->zmag;
write_mag_data((void *)&mag); write_mag_data(&mag);
perf_begin(_perf_mag); perf_begin(_perf_mag);
RawBaroData baro = {}; RawBaroData baro = {};
@ -207,13 +207,13 @@ void Simulator::update_sensors(mavlink_hil_sensor_t *imu)
baro.altitude = imu->pressure_alt; baro.altitude = imu->pressure_alt;
baro.temperature = imu->temperature; baro.temperature = imu->temperature;
write_baro_data((void *)&baro); write_baro_data(&baro);
RawAirspeedData airspeed = {}; RawAirspeedData airspeed = {};
airspeed.temperature = imu->temperature; airspeed.temperature = imu->temperature;
airspeed.diff_pressure = imu->diff_pressure; airspeed.diff_pressure = imu->diff_pressure;
write_airspeed_data((void *)&airspeed); write_airspeed_data(&airspeed);
} }
void Simulator::update_gps(mavlink_hil_gps_t *gps_sim) void Simulator::update_gps(mavlink_hil_gps_t *gps_sim)
@ -242,6 +242,9 @@ void Simulator::handle_message(mavlink_message_t *msg, bool publish)
mavlink_hil_sensor_t imu; mavlink_hil_sensor_t imu;
mavlink_msg_hil_sensor_decode(msg, &imu); mavlink_msg_hil_sensor_decode(msg, &imu);
// set temperature to a decent value
imu.temperature = 32.0f;
uint64_t sim_timestamp = imu.time_usec; uint64_t sim_timestamp = imu.time_usec;
struct timespec ts; struct timespec ts;
px4_clock_gettime(CLOCK_REALTIME, &ts); px4_clock_gettime(CLOCK_REALTIME, &ts);
@ -781,8 +784,7 @@ int Simulator::publish_sensor_topics(mavlink_hil_sensor_t *imu)
*/ */
/* gyro */ /* gyro */
{ {
struct gyro_report gyro; struct gyro_report gyro = {};
memset(&gyro, 0, sizeof(gyro));
gyro.timestamp = timestamp; gyro.timestamp = timestamp;
gyro.x_raw = imu->xgyro * 1000.0f; gyro.x_raw = imu->xgyro * 1000.0f;
@ -792,6 +794,8 @@ int Simulator::publish_sensor_topics(mavlink_hil_sensor_t *imu)
gyro.y = imu->ygyro; gyro.y = imu->ygyro;
gyro.z = imu->zgyro; gyro.z = imu->zgyro;
gyro.temperature = imu->temperature;
if (_gyro_pub == nullptr) { if (_gyro_pub == nullptr) {
_gyro_pub = orb_advertise(ORB_ID(sensor_gyro), &gyro); _gyro_pub = orb_advertise(ORB_ID(sensor_gyro), &gyro);
@ -802,8 +806,7 @@ int Simulator::publish_sensor_topics(mavlink_hil_sensor_t *imu)
/* accelerometer */ /* accelerometer */
{ {
struct accel_report accel; struct accel_report accel = {};
memset(&accel, 0, sizeof(accel));
accel.timestamp = timestamp; accel.timestamp = timestamp;
accel.x_raw = imu->xacc / mg2ms2; accel.x_raw = imu->xacc / mg2ms2;
@ -813,6 +816,8 @@ int Simulator::publish_sensor_topics(mavlink_hil_sensor_t *imu)
accel.y = imu->yacc; accel.y = imu->yacc;
accel.z = imu->zacc; accel.z = imu->zacc;
accel.temperature = imu->temperature;
if (_accel_pub == nullptr) { if (_accel_pub == nullptr) {
_accel_pub = orb_advertise(ORB_ID(sensor_accel), &accel); _accel_pub = orb_advertise(ORB_ID(sensor_accel), &accel);
@ -823,8 +828,7 @@ int Simulator::publish_sensor_topics(mavlink_hil_sensor_t *imu)
/* magnetometer */ /* magnetometer */
{ {
struct mag_report mag; struct mag_report mag = {};
memset(&mag, 0, sizeof(mag));
mag.timestamp = timestamp; mag.timestamp = timestamp;
mag.x_raw = imu->xmag * 1000.0f; mag.x_raw = imu->xmag * 1000.0f;
@ -834,6 +838,8 @@ int Simulator::publish_sensor_topics(mavlink_hil_sensor_t *imu)
mag.y = imu->ymag; mag.y = imu->ymag;
mag.z = imu->zmag; mag.z = imu->zmag;
mag.temperature = imu->temperature;
if (_mag_pub == nullptr) { if (_mag_pub == nullptr) {
/* publish to the first mag topic */ /* publish to the first mag topic */
_mag_pub = orb_advertise(ORB_ID(sensor_mag), &mag); _mag_pub = orb_advertise(ORB_ID(sensor_mag), &mag);
@ -845,8 +851,7 @@ int Simulator::publish_sensor_topics(mavlink_hil_sensor_t *imu)
/* baro */ /* baro */
{ {
struct baro_report baro; struct baro_report baro = {};
memset(&baro, 0, sizeof(baro));
baro.timestamp = timestamp; baro.timestamp = timestamp;
baro.pressure = imu->abs_pressure; baro.pressure = imu->abs_pressure;

Loading…
Cancel
Save