fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-20 02:23:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Added bitfield to encode updated dimensions

Browse Source
This commit is contained in:
Lorenz Meier
2012-09-06 20:47:22 +02:00
parent 925f143433
commit e8307aba17
15 changed files with 109 additions and 53 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/mavlink/mavlink.c
+38 -6
View File
@@ -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);
apps/sensors/sensors.cpp
+4 -4
View File
@@ -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
apps/uORB/topics/sensor_combined.h
+5 -5
View File
@@ -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 */
};