diff options
Diffstat (limited to 'apps')
| -rw-r--r-- | apps/mavlink/mavlink.c | 44 | ||||
| -rw-r--r-- | apps/sensors/sensors.cpp | 8 | ||||
| -rw-r--r-- | apps/uORB/topics/sensor_combined.h | 10 |
3 files changed, 47 insertions, 15 deletions
diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index 1d71b4019..0df881339 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -770,6 +770,38 @@ static void *uorb_receiveloop(void *arg) /* send scaled imu data (m/s^2 accelerations scaled back to milli-g) */ //mavlink_msg_scaled_imu_send(MAVLINK_COMM_0, buf.raw.timestamp, buf.raw.accelerometer_m_s2[0] * 101.936799f, buf.raw.accelerometer_m_s2[1] * 101.936799f, buf.raw.accelerometer_m_s2[2] * 101.936799f, buf.raw.gyro_rad_s[0] * 1000, buf.raw.gyro_rad_s[1] * 1000, buf.raw.gyro_rad_s[2] * 1000, buf.raw.magnetometer_ga[0] * 1000, buf.raw.magnetometer_ga[1] * 1000, buf.raw.magnetometer_ga[2] * 1000); /* send scaled imu data */ + + /* mark individual fields as changed */ + uint16_t fields_updated = 0; + static unsigned accel_counter = 0; + static unsigned gyro_counter = 0; + static unsigned mag_counter = 0; + static unsigned baro_counter = 0; + + if (accel_counter != buf.raw.accelerometer_counter) { + /* mark first three dimensions as changed */ + fields_updated |= (1 << 0) | (1 << 1) | (1 << 2); + accel_counter = buf.raw.accelerometer_counter; + } + + if (gyro_counter != buf.raw.gyro_counter) { + /* mark first three dimensions as changed */ + fields_updated |= (1 << 3) | (1 << 4) | (1 << 5); + gyro_counter = buf.raw.gyro_counter; + } + + if (mag_counter != buf.raw.magnetometer_counter) { + /* mark first three dimensions as changed */ + fields_updated |= (1 << 6) | (1 << 7) | (1 << 8); + mag_counter = buf.raw.magnetometer_counter; + } + + if (baro_counter != buf.raw.baro_counter) { + /* mark first three dimensions as changed */ + fields_updated |= (1 << 9) | (1 << 11) | (1 << 12); + baro_counter = buf.raw.baro_counter; + } + mavlink_msg_highres_imu_send(MAVLINK_COMM_0, buf.raw.timestamp, buf.raw.accelerometer_m_s2[0], buf.raw.accelerometer_m_s2[1], buf.raw.accelerometer_m_s2[2], buf.raw.gyro_rad_s[0], @@ -777,7 +809,8 @@ static void *uorb_receiveloop(void *arg) buf.raw.magnetometer_ga[0], buf.raw.magnetometer_ga[1],buf.raw.magnetometer_ga[2], buf.raw.baro_pres_mbar, 0 /* no diff pressure yet */, - buf.raw.baro_alt_meter, buf.raw.baro_temp_celcius); + buf.raw.baro_alt_meter, buf.raw.baro_temp_celcius, + fields_updated); /* send pressure */ //mavlink_msg_scaled_pressure_send(MAVLINK_COMM_0, buf.raw.timestamp / 1000, buf.raw.baro_pres_mbar, buf.raw.baro_alt_meter, buf.raw.baro_temp_celcius * 100); @@ -1528,13 +1561,12 @@ int mavlink_thread_main(int argc, char *argv[]) set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 200); } else if (baudrate >= 460800) { /* 200 Hz / 5 ms */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 5); + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 10); /* 100 Hz / 10 ms */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SCALED_IMU, 10); - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 10); - /* 66 Hz / 15 ms */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 15); + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SCALED_IMU, 20); + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 20); /* 20 Hz / 20 ms */ + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 50); set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 50); /* 2 Hz */ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 500); diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp index c5b9e483e..77dc0aa31 100644 --- a/apps/sensors/sensors.cpp +++ b/apps/sensors/sensors.cpp @@ -666,7 +666,7 @@ Sensors::accel_poll(struct sensor_combined_s &raw) raw.accelerometer_raw[1] = accel_report.y_raw; raw.accelerometer_raw[2] = accel_report.z_raw; - raw.accelerometer_raw_counter++; + raw.accelerometer_counter++; } void @@ -702,7 +702,7 @@ Sensors::gyro_poll(struct sensor_combined_s &raw) raw.gyro_raw[1] = gyro_report.y_raw; raw.gyro_raw[2] = gyro_report.z_raw; - raw.gyro_raw_counter++; + raw.gyro_counter++; } } @@ -721,7 +721,7 @@ Sensors::mag_poll(struct sensor_combined_s &raw) raw.magnetometer_raw[1] = mag_report.y_raw; raw.magnetometer_raw[2] = mag_report.z_raw; - raw.magnetometer_raw_counter++; + raw.magnetometer_counter++; } void @@ -735,7 +735,7 @@ Sensors::baro_poll(struct sensor_combined_s &raw) raw.baro_alt_meter = baro_report.altitude; // Altitude in meters raw.baro_temp_celcius = baro_report.temperature; // Temperature in degrees celcius - raw.baro_raw_counter++; + raw.baro_counter++; } void diff --git a/apps/uORB/topics/sensor_combined.h b/apps/uORB/topics/sensor_combined.h index 8fae78858..8c1847753 100644 --- a/apps/uORB/topics/sensor_combined.h +++ b/apps/uORB/topics/sensor_combined.h @@ -80,11 +80,11 @@ struct sensor_combined_s { uint64_t timestamp; /**< Timestamp in microseconds since boot LOGME */ int16_t gyro_raw[3]; /**< Raw sensor values of angular velocity LOGME */ - uint16_t gyro_raw_counter; /**< Number of raw measurments taken LOGME */ + uint16_t gyro_counter; /**< Number of raw measurments taken LOGME */ float gyro_rad_s[3]; /**< Angular velocity in radian per seconds LOGME */ int16_t accelerometer_raw[3]; /**< Raw acceleration in NED body frame LOGME */ - uint16_t accelerometer_raw_counter; /**< Number of raw acc measurements taken LOGME */ + uint32_t accelerometer_counter; /**< Number of raw acc measurements taken LOGME */ float accelerometer_m_s2[3]; /**< Acceleration in NED body frame, in m/s^2 LOGME */ int accelerometer_mode; /**< Accelerometer measurement mode */ float accelerometer_range_m_s2; /**< Accelerometer measurement range in m/s^2 */ @@ -94,15 +94,15 @@ struct sensor_combined_s { int magnetometer_mode; /**< Magnetometer measurement mode */ float magnetometer_range_ga; /**< ± measurement range in Gauss */ float magnetometer_cuttoff_freq_hz; /**< Internal analog low pass frequency of sensor */ - uint16_t magnetometer_raw_counter; /**< Number of raw mag measurements taken LOGME */ + uint32_t magnetometer_counter; /**< Number of raw mag measurements taken LOGME */ float baro_pres_mbar; /**< Barometric pressure, already temp. comp. LOGME */ float baro_alt_meter; /**< Altitude, already temp. comp. LOGME */ float baro_temp_celcius; /**< Temperature in degrees celsius LOGME */ float battery_voltage_v; /**< Battery voltage in volts, filtered LOGME */ float adc_voltage_v[3]; /**< ADC voltages of ADC Chan 11/12/13 or -1 LOGME */ - uint16_t baro_raw_counter; /**< Number of raw baro measurements taken LOGME */ - uint16_t battery_voltage_counter; /**< Number of voltage measurements taken LOGME */ + uint32_t baro_counter; /**< Number of raw baro measurements taken LOGME */ + uint32_t battery_voltage_counter; /**< Number of voltage measurements taken LOGME */ bool battery_voltage_valid; /**< True if battery voltage can be measured LOGME */ }; |