forked from Archive/PX4-Autopilot
Merge branch 'beta_mavlink2' of github.com:PX4/Firmware into mavlink2_hil
This commit is contained in:
commit
7a4efc81b9
|
@ -474,7 +474,7 @@ then
|
|||
sh /etc/init.d/rc.interface
|
||||
|
||||
# Start standard fixedwing apps
|
||||
if [ LOAD_DEFAULT_APPS == yes ]
|
||||
if [ $LOAD_DEFAULT_APPS == yes ]
|
||||
then
|
||||
sh /etc/init.d/rc.fw_apps
|
||||
fi
|
||||
|
@ -533,7 +533,7 @@ then
|
|||
sh /etc/init.d/rc.interface
|
||||
|
||||
# Start standard multicopter apps
|
||||
if [ LOAD_DEFAULT_APPS == yes ]
|
||||
if [ $LOAD_DEFAULT_APPS == yes ]
|
||||
then
|
||||
sh /etc/init.d/rc.mc_apps
|
||||
fi
|
||||
|
|
|
@ -277,7 +277,6 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
|
|||
// XXX write this out to perf regs
|
||||
|
||||
/* keep track of sensor updates */
|
||||
uint32_t sensor_last_count[3] = {0, 0, 0};
|
||||
uint64_t sensor_last_timestamp[3] = {0, 0, 0};
|
||||
|
||||
struct attitude_estimator_ekf_params ekf_params;
|
||||
|
@ -380,9 +379,8 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
|
|||
uint8_t update_vect[3] = {0, 0, 0};
|
||||
|
||||
/* Fill in gyro measurements */
|
||||
if (sensor_last_count[0] != raw.gyro_counter) {
|
||||
if (sensor_last_timestamp[0] != raw.timestamp) {
|
||||
update_vect[0] = 1;
|
||||
sensor_last_count[0] = raw.gyro_counter;
|
||||
sensor_update_hz[0] = 1e6f / (raw.timestamp - sensor_last_timestamp[0]);
|
||||
sensor_last_timestamp[0] = raw.timestamp;
|
||||
}
|
||||
|
@ -392,9 +390,8 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
|
|||
z_k[2] = raw.gyro_rad_s[2] - gyro_offsets[2];
|
||||
|
||||
/* update accelerometer measurements */
|
||||
if (sensor_last_count[1] != raw.accelerometer_counter) {
|
||||
if (sensor_last_timestamp[1] != raw.accelerometer_timestamp) {
|
||||
update_vect[1] = 1;
|
||||
sensor_last_count[1] = raw.accelerometer_counter;
|
||||
sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]);
|
||||
sensor_last_timestamp[1] = raw.timestamp;
|
||||
}
|
||||
|
@ -445,9 +442,8 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
|
|||
z_k[5] = raw.accelerometer_m_s2[2] - acc(2);
|
||||
|
||||
/* update magnetometer measurements */
|
||||
if (sensor_last_count[2] != raw.magnetometer_counter) {
|
||||
if (sensor_last_timestamp[2] != raw.magnetometer_timestamp) {
|
||||
update_vect[2] = 1;
|
||||
sensor_last_count[2] = raw.magnetometer_counter;
|
||||
sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]);
|
||||
sensor_last_timestamp[2] = raw.timestamp;
|
||||
}
|
||||
|
|
|
@ -445,7 +445,6 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[])
|
|||
// XXX write this out to perf regs
|
||||
|
||||
/* keep track of sensor updates */
|
||||
uint32_t sensor_last_count[3] = {0, 0, 0};
|
||||
uint64_t sensor_last_timestamp[3] = {0, 0, 0};
|
||||
|
||||
struct attitude_estimator_so3_params so3_comp_params;
|
||||
|
@ -526,9 +525,8 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[])
|
|||
uint8_t update_vect[3] = {0, 0, 0};
|
||||
|
||||
/* Fill in gyro measurements */
|
||||
if (sensor_last_count[0] != raw.gyro_counter) {
|
||||
if (sensor_last_timestamp[0] != raw.timestamp) {
|
||||
update_vect[0] = 1;
|
||||
sensor_last_count[0] = raw.gyro_counter;
|
||||
sensor_update_hz[0] = 1e6f / (raw.timestamp - sensor_last_timestamp[0]);
|
||||
sensor_last_timestamp[0] = raw.timestamp;
|
||||
}
|
||||
|
@ -538,9 +536,8 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[])
|
|||
gyro[2] = raw.gyro_rad_s[2] - gyro_offsets[2];
|
||||
|
||||
/* update accelerometer measurements */
|
||||
if (sensor_last_count[1] != raw.accelerometer_counter) {
|
||||
if (sensor_last_timestamp[1] != raw.accelerometer_timestamp) {
|
||||
update_vect[1] = 1;
|
||||
sensor_last_count[1] = raw.accelerometer_counter;
|
||||
sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]);
|
||||
sensor_last_timestamp[1] = raw.timestamp;
|
||||
}
|
||||
|
@ -550,9 +547,8 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[])
|
|||
acc[2] = raw.accelerometer_m_s2[2];
|
||||
|
||||
/* update magnetometer measurements */
|
||||
if (sensor_last_count[2] != raw.magnetometer_counter) {
|
||||
if (sensor_last_timestamp[2] != raw.magnetometer_timestamp) {
|
||||
update_vect[2] = 1;
|
||||
sensor_last_count[2] = raw.magnetometer_counter;
|
||||
sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]);
|
||||
sensor_last_timestamp[2] = raw.timestamp;
|
||||
}
|
||||
|
|
|
@ -216,8 +216,8 @@ protected:
|
|||
|
||||
void send(const hrt_abstime t)
|
||||
{
|
||||
status_sub->update(t);
|
||||
pos_sp_triplet_sub->update(t);
|
||||
(void)status_sub->update(t);
|
||||
(void)pos_sp_triplet_sub->update(t);
|
||||
|
||||
uint8_t mavlink_state = 0;
|
||||
uint8_t mavlink_base_mode = 0;
|
||||
|
@ -261,22 +261,23 @@ protected:
|
|||
|
||||
void send(const hrt_abstime t)
|
||||
{
|
||||
status_sub->update(t);
|
||||
if (status_sub->update(t)) {
|
||||
|
||||
mavlink_msg_sys_status_send(_channel,
|
||||
status->onboard_control_sensors_present,
|
||||
status->onboard_control_sensors_enabled,
|
||||
status->onboard_control_sensors_health,
|
||||
status->load * 1000.0f,
|
||||
status->battery_voltage * 1000.0f,
|
||||
status->battery_current * 1000.0f,
|
||||
status->battery_remaining,
|
||||
status->drop_rate_comm,
|
||||
status->errors_comm,
|
||||
status->errors_count1,
|
||||
status->errors_count2,
|
||||
status->errors_count3,
|
||||
status->errors_count4);
|
||||
mavlink_msg_sys_status_send(_channel,
|
||||
status->onboard_control_sensors_present,
|
||||
status->onboard_control_sensors_enabled,
|
||||
status->onboard_control_sensors_health,
|
||||
status->load * 1000.0f,
|
||||
status->battery_voltage * 1000.0f,
|
||||
status->battery_current * 1000.0f,
|
||||
status->battery_remaining,
|
||||
status->drop_rate_comm,
|
||||
status->errors_comm,
|
||||
status->errors_count1,
|
||||
status->errors_count2,
|
||||
status->errors_count3,
|
||||
status->errors_count4);
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
|
@ -284,7 +285,7 @@ protected:
|
|||
class MavlinkStreamHighresIMU : public MavlinkStream
|
||||
{
|
||||
public:
|
||||
MavlinkStreamHighresIMU() : MavlinkStream(), accel_counter(0), gyro_counter(0), mag_counter(0), baro_counter(0)
|
||||
MavlinkStreamHighresIMU() : MavlinkStream(), accel_timestamp(0), gyro_timestamp(0), mag_timestamp(0), baro_timestamp(0)
|
||||
{
|
||||
}
|
||||
|
||||
|
@ -302,10 +303,10 @@ private:
|
|||
MavlinkOrbSubscription *sensor_sub;
|
||||
struct sensor_combined_s *sensor;
|
||||
|
||||
uint32_t accel_counter;
|
||||
uint32_t gyro_counter;
|
||||
uint32_t mag_counter;
|
||||
uint32_t baro_counter;
|
||||
uint64_t accel_timestamp;
|
||||
uint64_t gyro_timestamp;
|
||||
uint64_t mag_timestamp;
|
||||
uint64_t baro_timestamp;
|
||||
|
||||
protected:
|
||||
void subscribe(Mavlink *mavlink)
|
||||
|
@ -316,42 +317,43 @@ protected:
|
|||
|
||||
void send(const hrt_abstime t)
|
||||
{
|
||||
sensor_sub->update(t);
|
||||
if (sensor_sub->update(t)) {
|
||||
|
||||
uint16_t fields_updated = 0;
|
||||
uint16_t fields_updated = 0;
|
||||
|
||||
if (accel_counter != sensor->accelerometer_counter) {
|
||||
/* mark first three dimensions as changed */
|
||||
fields_updated |= (1 << 0) | (1 << 1) | (1 << 2);
|
||||
accel_counter = sensor->accelerometer_counter;
|
||||
if (accel_timestamp != sensor->accelerometer_timestamp) {
|
||||
/* mark first three dimensions as changed */
|
||||
fields_updated |= (1 << 0) | (1 << 1) | (1 << 2);
|
||||
accel_timestamp = sensor->accelerometer_timestamp;
|
||||
}
|
||||
|
||||
if (gyro_timestamp != sensor->timestamp) {
|
||||
/* mark second group dimensions as changed */
|
||||
fields_updated |= (1 << 3) | (1 << 4) | (1 << 5);
|
||||
gyro_timestamp = sensor->timestamp;
|
||||
}
|
||||
|
||||
if (mag_timestamp != sensor->magnetometer_timestamp) {
|
||||
/* mark third group dimensions as changed */
|
||||
fields_updated |= (1 << 6) | (1 << 7) | (1 << 8);
|
||||
mag_timestamp = sensor->magnetometer_timestamp;
|
||||
}
|
||||
|
||||
if (baro_timestamp != sensor->baro_timestamp) {
|
||||
/* mark last group dimensions as changed */
|
||||
fields_updated |= (1 << 9) | (1 << 11) | (1 << 12);
|
||||
baro_timestamp = sensor->baro_timestamp;
|
||||
}
|
||||
|
||||
mavlink_msg_highres_imu_send(_channel,
|
||||
sensor->timestamp,
|
||||
sensor->accelerometer_m_s2[0], sensor->accelerometer_m_s2[1], sensor->accelerometer_m_s2[2],
|
||||
sensor->gyro_rad_s[0], sensor->gyro_rad_s[1], sensor->gyro_rad_s[2],
|
||||
sensor->magnetometer_ga[0], sensor->magnetometer_ga[1], sensor->magnetometer_ga[2],
|
||||
sensor->baro_pres_mbar, sensor->differential_pressure_pa,
|
||||
sensor->baro_alt_meter, sensor->baro_temp_celcius,
|
||||
fields_updated);
|
||||
}
|
||||
|
||||
if (gyro_counter != sensor->gyro_counter) {
|
||||
/* mark second group dimensions as changed */
|
||||
fields_updated |= (1 << 3) | (1 << 4) | (1 << 5);
|
||||
gyro_counter = sensor->gyro_counter;
|
||||
}
|
||||
|
||||
if (mag_counter != sensor->magnetometer_counter) {
|
||||
/* mark third group dimensions as changed */
|
||||
fields_updated |= (1 << 6) | (1 << 7) | (1 << 8);
|
||||
mag_counter = sensor->magnetometer_counter;
|
||||
}
|
||||
|
||||
if (baro_counter != sensor->baro_counter) {
|
||||
/* mark last group dimensions as changed */
|
||||
fields_updated |= (1 << 9) | (1 << 11) | (1 << 12);
|
||||
baro_counter = sensor->baro_counter;
|
||||
}
|
||||
|
||||
mavlink_msg_highres_imu_send(_channel,
|
||||
sensor->timestamp,
|
||||
sensor->accelerometer_m_s2[0], sensor->accelerometer_m_s2[1], sensor->accelerometer_m_s2[2],
|
||||
sensor->gyro_rad_s[0], sensor->gyro_rad_s[1], sensor->gyro_rad_s[2],
|
||||
sensor->magnetometer_ga[0], sensor->magnetometer_ga[1], sensor->magnetometer_ga[2],
|
||||
sensor->baro_pres_mbar, sensor->differential_pressure_pa,
|
||||
sensor->baro_alt_meter, sensor->baro_temp_celcius,
|
||||
fields_updated);
|
||||
}
|
||||
};
|
||||
|
||||
|
@ -382,12 +384,13 @@ protected:
|
|||
|
||||
void send(const hrt_abstime t)
|
||||
{
|
||||
att_sub->update(t);
|
||||
if (att_sub->update(t)) {
|
||||
|
||||
mavlink_msg_attitude_send(_channel,
|
||||
att->timestamp / 1000,
|
||||
att->roll, att->pitch, att->yaw,
|
||||
att->rollspeed, att->pitchspeed, att->yawspeed);
|
||||
mavlink_msg_attitude_send(_channel,
|
||||
att->timestamp / 1000,
|
||||
att->roll, att->pitch, att->yaw,
|
||||
att->rollspeed, att->pitchspeed, att->yawspeed);
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
|
@ -418,17 +421,18 @@ protected:
|
|||
|
||||
void send(const hrt_abstime t)
|
||||
{
|
||||
att_sub->update(t);
|
||||
if (att_sub->update(t)) {
|
||||
|
||||
mavlink_msg_attitude_quaternion_send(_channel,
|
||||
att->timestamp / 1000,
|
||||
att->q[0],
|
||||
att->q[1],
|
||||
att->q[2],
|
||||
att->q[3],
|
||||
att->rollspeed,
|
||||
att->pitchspeed,
|
||||
att->yawspeed);
|
||||
mavlink_msg_attitude_quaternion_send(_channel,
|
||||
att->timestamp / 1000,
|
||||
att->q[0],
|
||||
att->q[1],
|
||||
att->q[2],
|
||||
att->q[3],
|
||||
att->rollspeed,
|
||||
att->pitchspeed,
|
||||
att->yawspeed);
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
|
@ -483,23 +487,26 @@ protected:
|
|||
|
||||
void send(const hrt_abstime t)
|
||||
{
|
||||
att_sub->update(t);
|
||||
pos_sub->update(t);
|
||||
armed_sub->update(t);
|
||||
act_sub->update(t);
|
||||
airspeed_sub->update(t);
|
||||
bool updated = att_sub->update(t);
|
||||
updated |= pos_sub->update(t);
|
||||
updated |= armed_sub->update(t);
|
||||
updated |= act_sub->update(t);
|
||||
updated |= airspeed_sub->update(t);
|
||||
|
||||
float groundspeed = sqrtf(pos->vel_n * pos->vel_n + pos->vel_e * pos->vel_e);
|
||||
uint16_t heading = _wrap_2pi(att->yaw) * M_RAD_TO_DEG_F;
|
||||
float throttle = armed->armed ? act->control[3] * 100.0f : 0.0f;
|
||||
if (updated) {
|
||||
|
||||
mavlink_msg_vfr_hud_send(_channel,
|
||||
airspeed->true_airspeed_m_s,
|
||||
groundspeed,
|
||||
heading,
|
||||
throttle,
|
||||
pos->alt,
|
||||
-pos->vel_d);
|
||||
float groundspeed = sqrtf(pos->vel_n * pos->vel_n + pos->vel_e * pos->vel_e);
|
||||
uint16_t heading = _wrap_2pi(att->yaw) * M_RAD_TO_DEG_F;
|
||||
float throttle = armed->armed ? act->control[3] * 100.0f : 0.0f;
|
||||
|
||||
mavlink_msg_vfr_hud_send(_channel,
|
||||
airspeed->true_airspeed_m_s,
|
||||
groundspeed,
|
||||
heading,
|
||||
throttle,
|
||||
pos->alt,
|
||||
-pos->vel_d);
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
|
@ -530,19 +537,20 @@ protected:
|
|||
|
||||
void send(const hrt_abstime t)
|
||||
{
|
||||
gps_sub->update(t);
|
||||
if (gps_sub->update(t)) {
|
||||
|
||||
mavlink_msg_gps_raw_int_send(_channel,
|
||||
gps->timestamp_position,
|
||||
gps->fix_type,
|
||||
gps->lat,
|
||||
gps->lon,
|
||||
gps->alt,
|
||||
cm_uint16_from_m_float(gps->eph_m),
|
||||
cm_uint16_from_m_float(gps->epv_m),
|
||||
gps->vel_m_s * 100.0f,
|
||||
_wrap_2pi(gps->cog_rad) * M_RAD_TO_DEG_F * 1e2f,
|
||||
gps->satellites_visible);
|
||||
mavlink_msg_gps_raw_int_send(_channel,
|
||||
gps->timestamp_position,
|
||||
gps->fix_type,
|
||||
gps->lat,
|
||||
gps->lon,
|
||||
gps->alt,
|
||||
cm_uint16_from_m_float(gps->eph_m),
|
||||
cm_uint16_from_m_float(gps->epv_m),
|
||||
gps->vel_m_s * 100.0f,
|
||||
_wrap_2pi(gps->cog_rad) * M_RAD_TO_DEG_F * 1e2f,
|
||||
gps->satellites_visible);
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
|
@ -579,10 +587,11 @@ protected:
|
|||
|
||||
void send(const hrt_abstime t)
|
||||
{
|
||||
pos_sub->update(t);
|
||||
home_sub->update(t);
|
||||
bool updated = pos_sub->update(t);
|
||||
updated |= home_sub->update(t);
|
||||
|
||||
mavlink_msg_global_position_int_send(_channel,
|
||||
if (updated) {
|
||||
mavlink_msg_global_position_int_send(_channel,
|
||||
pos->timestamp / 1000,
|
||||
pos->lat * 1e7,
|
||||
pos->lon * 1e7,
|
||||
|
@ -592,6 +601,7 @@ protected:
|
|||
pos->vel_e * 100.0f,
|
||||
pos->vel_d * 100.0f,
|
||||
_wrap_2pi(pos->yaw) * M_RAD_TO_DEG_F * 100.0f);
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
|
@ -622,16 +632,17 @@ protected:
|
|||
|
||||
void send(const hrt_abstime t)
|
||||
{
|
||||
pos_sub->update(t);
|
||||
if (pos_sub->update(t)) {
|
||||
|
||||
mavlink_msg_local_position_ned_send(_channel,
|
||||
pos->timestamp / 1000,
|
||||
pos->x,
|
||||
pos->y,
|
||||
pos->z,
|
||||
pos->vx,
|
||||
pos->vy,
|
||||
pos->vz);
|
||||
mavlink_msg_local_position_ned_send(_channel,
|
||||
pos->timestamp / 1000,
|
||||
pos->x,
|
||||
pos->y,
|
||||
pos->z,
|
||||
pos->vx,
|
||||
pos->vy,
|
||||
pos->vz);
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
|
@ -662,12 +673,17 @@ protected:
|
|||
|
||||
void send(const hrt_abstime t)
|
||||
{
|
||||
home_sub->update(t);
|
||||
|
||||
mavlink_msg_gps_global_origin_send(_channel,
|
||||
(int32_t)(home->lat * 1e7),
|
||||
(int32_t)(home->lon * 1e7),
|
||||
(int32_t)(home->alt) * 1000.0f);
|
||||
/* we're sending the GPS home periodically to ensure the
|
||||
* the GCS does pick it up at one point */
|
||||
if (home_sub->is_published()) {
|
||||
home_sub->update(t);
|
||||
|
||||
mavlink_msg_gps_global_origin_send(_channel,
|
||||
(int32_t)(home->lat * 1e7),
|
||||
(int32_t)(home->lon * 1e7),
|
||||
(int32_t)(home->alt) * 1000.0f);
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
|
@ -713,19 +729,20 @@ protected:
|
|||
|
||||
void send(const hrt_abstime t)
|
||||
{
|
||||
act_sub->update(t);
|
||||
if (act_sub->update(t)) {
|
||||
|
||||
mavlink_msg_servo_output_raw_send(_channel,
|
||||
act->timestamp / 1000,
|
||||
_n,
|
||||
act->output[0],
|
||||
act->output[1],
|
||||
act->output[2],
|
||||
act->output[3],
|
||||
act->output[4],
|
||||
act->output[5],
|
||||
act->output[6],
|
||||
act->output[7]);
|
||||
mavlink_msg_servo_output_raw_send(_channel,
|
||||
act->timestamp / 1000,
|
||||
_n,
|
||||
act->output[0],
|
||||
act->output[1],
|
||||
act->output[2],
|
||||
act->output[3],
|
||||
act->output[4],
|
||||
act->output[5],
|
||||
act->output[6],
|
||||
act->output[7]);
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
|
@ -768,57 +785,60 @@ protected:
|
|||
|
||||
void send(const hrt_abstime t)
|
||||
{
|
||||
status_sub->update(t);
|
||||
pos_sp_triplet_sub->update(t);
|
||||
act_sub->update(t);
|
||||
bool updated = status_sub->update(t);
|
||||
updated |= pos_sp_triplet_sub->update(t);
|
||||
updated |= act_sub->update(t);
|
||||
|
||||
/* translate the current syste state to mavlink state and mode */
|
||||
uint8_t mavlink_state;
|
||||
uint8_t mavlink_base_mode;
|
||||
uint32_t mavlink_custom_mode;
|
||||
get_mavlink_mode_state(status, pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
|
||||
if (updated) {
|
||||
|
||||
/* set number of valid outputs depending on vehicle type */
|
||||
unsigned n;
|
||||
/* translate the current syste state to mavlink state and mode */
|
||||
uint8_t mavlink_state;
|
||||
uint8_t mavlink_base_mode;
|
||||
uint32_t mavlink_custom_mode;
|
||||
get_mavlink_mode_state(status, pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
|
||||
|
||||
switch (mavlink_system.type) {
|
||||
case MAV_TYPE_QUADROTOR:
|
||||
n = 4;
|
||||
break;
|
||||
/* set number of valid outputs depending on vehicle type */
|
||||
unsigned n;
|
||||
|
||||
case MAV_TYPE_HEXAROTOR:
|
||||
n = 6;
|
||||
break;
|
||||
switch (mavlink_system.type) {
|
||||
case MAV_TYPE_QUADROTOR:
|
||||
n = 4;
|
||||
break;
|
||||
|
||||
default:
|
||||
n = 8;
|
||||
break;
|
||||
}
|
||||
case MAV_TYPE_HEXAROTOR:
|
||||
n = 6;
|
||||
break;
|
||||
|
||||
/* scale / assign outputs depending on system type */
|
||||
float out[8];
|
||||
default:
|
||||
n = 8;
|
||||
break;
|
||||
}
|
||||
|
||||
for (unsigned i = 0; i < 8; i++) {
|
||||
if (i < n) {
|
||||
if (mavlink_base_mode & MAV_MODE_FLAG_SAFETY_ARMED) {
|
||||
/* scale fake PWM out 900..1200 us to 0..1*/
|
||||
out[i] = (act->output[i] - 900.0f) / 1200.0f;
|
||||
/* scale / assign outputs depending on system type */
|
||||
float out[8];
|
||||
|
||||
for (unsigned i = 0; i < 8; i++) {
|
||||
if (i < n) {
|
||||
if (mavlink_base_mode & MAV_MODE_FLAG_SAFETY_ARMED) {
|
||||
/* scale fake PWM out 900..1200 us to 0..1*/
|
||||
out[i] = (act->output[i] - 900.0f) / 1200.0f;
|
||||
|
||||
} else {
|
||||
/* send 0 when disarmed */
|
||||
out[i] = 0.0f;
|
||||
}
|
||||
|
||||
} else {
|
||||
/* send 0 when disarmed */
|
||||
out[i] = 0.0f;
|
||||
out[i] = -1.0f;
|
||||
}
|
||||
|
||||
} else {
|
||||
out[i] = -1.0f;
|
||||
}
|
||||
}
|
||||
|
||||
mavlink_msg_hil_controls_send(_channel,
|
||||
hrt_absolute_time(),
|
||||
out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7],
|
||||
mavlink_base_mode,
|
||||
0);
|
||||
mavlink_msg_hil_controls_send(_channel,
|
||||
hrt_absolute_time(),
|
||||
out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7],
|
||||
mavlink_base_mode,
|
||||
0);
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
|
@ -849,14 +869,15 @@ protected:
|
|||
|
||||
void send(const hrt_abstime t)
|
||||
{
|
||||
pos_sp_triplet_sub->update(t);
|
||||
if (pos_sp_triplet_sub->update(t)) {
|
||||
|
||||
mavlink_msg_global_position_setpoint_int_send(_channel,
|
||||
MAV_FRAME_GLOBAL,
|
||||
(int32_t)(pos_sp_triplet->current.lat * 1e7),
|
||||
(int32_t)(pos_sp_triplet->current.lon * 1e7),
|
||||
(int32_t)(pos_sp_triplet->current.alt * 1000),
|
||||
(int16_t)(pos_sp_triplet->current.yaw * M_RAD_TO_DEG_F * 100.0f));
|
||||
mavlink_msg_global_position_setpoint_int_send(_channel,
|
||||
MAV_FRAME_GLOBAL,
|
||||
(int32_t)(pos_sp_triplet->current.lat * 1e7),
|
||||
(int32_t)(pos_sp_triplet->current.lon * 1e7),
|
||||
(int32_t)(pos_sp_triplet->current.alt * 1000),
|
||||
(int16_t)(pos_sp_triplet->current.yaw * M_RAD_TO_DEG_F * 100.0f));
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
|
@ -925,14 +946,15 @@ protected:
|
|||
|
||||
void send(const hrt_abstime t)
|
||||
{
|
||||
att_sp_sub->update(t);
|
||||
if (att_sp_sub->update(t)) {
|
||||
|
||||
mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(_channel,
|
||||
att_sp->timestamp / 1000,
|
||||
att_sp->roll_body,
|
||||
att_sp->pitch_body,
|
||||
att_sp->yaw_body,
|
||||
att_sp->thrust);
|
||||
mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(_channel,
|
||||
att_sp->timestamp / 1000,
|
||||
att_sp->roll_body,
|
||||
att_sp->pitch_body,
|
||||
att_sp->yaw_body,
|
||||
att_sp->thrust);
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
|
@ -963,14 +985,15 @@ protected:
|
|||
|
||||
void send(const hrt_abstime t)
|
||||
{
|
||||
att_rates_sp_sub->update(t);
|
||||
if (att_rates_sp_sub->update(t)) {
|
||||
|
||||
mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send(_channel,
|
||||
att_rates_sp->timestamp / 1000,
|
||||
att_rates_sp->roll,
|
||||
att_rates_sp->pitch,
|
||||
att_rates_sp->yaw,
|
||||
att_rates_sp->thrust);
|
||||
mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send(_channel,
|
||||
att_rates_sp->timestamp / 1000,
|
||||
att_rates_sp->roll,
|
||||
att_rates_sp->pitch,
|
||||
att_rates_sp->yaw,
|
||||
att_rates_sp->thrust);
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
|
@ -1001,24 +1024,25 @@ protected:
|
|||
|
||||
void send(const hrt_abstime t)
|
||||
{
|
||||
rc_sub->update(t);
|
||||
if (rc_sub->update(t)) {
|
||||
|
||||
const unsigned port_width = 8;
|
||||
const unsigned port_width = 8;
|
||||
|
||||
for (unsigned i = 0; (i * port_width) < rc->channel_count; i++) {
|
||||
/* Channels are sent in MAVLink main loop at a fixed interval */
|
||||
mavlink_msg_rc_channels_raw_send(_channel,
|
||||
rc->timestamp_publication / 1000,
|
||||
i,
|
||||
(rc->channel_count > (i * port_width) + 0) ? rc->values[(i * port_width) + 0] : UINT16_MAX,
|
||||
(rc->channel_count > (i * port_width) + 1) ? rc->values[(i * port_width) + 1] : UINT16_MAX,
|
||||
(rc->channel_count > (i * port_width) + 2) ? rc->values[(i * port_width) + 2] : UINT16_MAX,
|
||||
(rc->channel_count > (i * port_width) + 3) ? rc->values[(i * port_width) + 3] : UINT16_MAX,
|
||||
(rc->channel_count > (i * port_width) + 4) ? rc->values[(i * port_width) + 4] : UINT16_MAX,
|
||||
(rc->channel_count > (i * port_width) + 5) ? rc->values[(i * port_width) + 5] : UINT16_MAX,
|
||||
(rc->channel_count > (i * port_width) + 6) ? rc->values[(i * port_width) + 6] : UINT16_MAX,
|
||||
(rc->channel_count > (i * port_width) + 7) ? rc->values[(i * port_width) + 7] : UINT16_MAX,
|
||||
rc->rssi);
|
||||
for (unsigned i = 0; (i * port_width) < rc->channel_count; i++) {
|
||||
/* Channels are sent in MAVLink main loop at a fixed interval */
|
||||
mavlink_msg_rc_channels_raw_send(_channel,
|
||||
rc->timestamp_publication / 1000,
|
||||
i,
|
||||
(rc->channel_count > (i * port_width) + 0) ? rc->values[(i * port_width) + 0] : UINT16_MAX,
|
||||
(rc->channel_count > (i * port_width) + 1) ? rc->values[(i * port_width) + 1] : UINT16_MAX,
|
||||
(rc->channel_count > (i * port_width) + 2) ? rc->values[(i * port_width) + 2] : UINT16_MAX,
|
||||
(rc->channel_count > (i * port_width) + 3) ? rc->values[(i * port_width) + 3] : UINT16_MAX,
|
||||
(rc->channel_count > (i * port_width) + 4) ? rc->values[(i * port_width) + 4] : UINT16_MAX,
|
||||
(rc->channel_count > (i * port_width) + 5) ? rc->values[(i * port_width) + 5] : UINT16_MAX,
|
||||
(rc->channel_count > (i * port_width) + 6) ? rc->values[(i * port_width) + 6] : UINT16_MAX,
|
||||
(rc->channel_count > (i * port_width) + 7) ? rc->values[(i * port_width) + 7] : UINT16_MAX,
|
||||
rc->rssi);
|
||||
}
|
||||
}
|
||||
}
|
||||
};
|
||||
|
@ -1050,15 +1074,16 @@ protected:
|
|||
|
||||
void send(const hrt_abstime t)
|
||||
{
|
||||
manual_sub->update(t);
|
||||
if (manual_sub->update(t)) {
|
||||
|
||||
mavlink_msg_manual_control_send(_channel,
|
||||
mavlink_system.sysid,
|
||||
manual->roll * 1000,
|
||||
manual->pitch * 1000,
|
||||
manual->yaw * 1000,
|
||||
manual->throttle * 1000,
|
||||
0);
|
||||
mavlink_msg_manual_control_send(_channel,
|
||||
mavlink_system.sysid,
|
||||
manual->roll * 1000,
|
||||
manual->pitch * 1000,
|
||||
manual->yaw * 1000,
|
||||
manual->throttle * 1000,
|
||||
0);
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
|
@ -1089,15 +1114,16 @@ protected:
|
|||
|
||||
void send(const hrt_abstime t)
|
||||
{
|
||||
flow_sub->update(t);
|
||||
if (flow_sub->update(t)) {
|
||||
|
||||
mavlink_msg_optical_flow_send(_channel,
|
||||
flow->timestamp,
|
||||
flow->sensor_id,
|
||||
flow->flow_raw_x, flow->flow_raw_y,
|
||||
flow->flow_comp_x_m, flow->flow_comp_y_m,
|
||||
flow->quality,
|
||||
flow->ground_distance_m);
|
||||
mavlink_msg_optical_flow_send(_channel,
|
||||
flow->timestamp,
|
||||
flow->sensor_id,
|
||||
flow->flow_raw_x, flow->flow_raw_y,
|
||||
flow->flow_comp_x_m, flow->flow_comp_y_m,
|
||||
flow->quality,
|
||||
flow->ground_distance_m);
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
|
|
|
@ -46,11 +46,15 @@
|
|||
|
||||
#include "mavlink_orb_subscription.h"
|
||||
|
||||
MavlinkOrbSubscription::MavlinkOrbSubscription(const orb_id_t topic) : _topic(topic), _last_check(0), next(nullptr)
|
||||
MavlinkOrbSubscription::MavlinkOrbSubscription(const orb_id_t topic) :
|
||||
_fd(orb_subscribe(_topic)),
|
||||
_published(false),
|
||||
_topic(topic),
|
||||
_last_check(0),
|
||||
next(nullptr)
|
||||
{
|
||||
_data = malloc(topic->o_size);
|
||||
memset(_data, 0, topic->o_size);
|
||||
_fd = orb_subscribe(_topic);
|
||||
}
|
||||
|
||||
MavlinkOrbSubscription::~MavlinkOrbSubscription()
|
||||
|
@ -87,3 +91,16 @@ MavlinkOrbSubscription::update(const hrt_abstime t)
|
|||
|
||||
return false;
|
||||
}
|
||||
|
||||
bool
|
||||
MavlinkOrbSubscription::is_published()
|
||||
{
|
||||
bool updated;
|
||||
orb_check(_fd, &updated);
|
||||
|
||||
if (updated) {
|
||||
_published = true;
|
||||
}
|
||||
|
||||
return _published;
|
||||
}
|
||||
|
|
|
@ -54,12 +54,21 @@ public:
|
|||
~MavlinkOrbSubscription();
|
||||
|
||||
bool update(const hrt_abstime t);
|
||||
|
||||
/**
|
||||
* Check if the topic has been published.
|
||||
*
|
||||
* This call will return true if the topic was ever published.
|
||||
* @param true if the topic has been published at least once.
|
||||
*/
|
||||
bool is_published();
|
||||
void *get_data();
|
||||
const orb_id_t get_topic();
|
||||
|
||||
private:
|
||||
const orb_id_t _topic;
|
||||
int _fd;
|
||||
bool _published;
|
||||
void *_data;
|
||||
hrt_abstime _last_check;
|
||||
};
|
||||
|
|
|
@ -586,7 +586,6 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
|
|||
hil_sensors.gyro_rad_s[0] = imu.xgyro;
|
||||
hil_sensors.gyro_rad_s[1] = imu.ygyro;
|
||||
hil_sensors.gyro_rad_s[2] = imu.zgyro;
|
||||
hil_sensors.gyro_counter = _hil_counter;
|
||||
|
||||
hil_sensors.accelerometer_raw[0] = imu.xacc / mg2ms2;
|
||||
hil_sensors.accelerometer_raw[1] = imu.yacc / mg2ms2;
|
||||
|
@ -596,7 +595,7 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
|
|||
hil_sensors.accelerometer_m_s2[2] = imu.zacc;
|
||||
hil_sensors.accelerometer_mode = 0; // TODO what is this?
|
||||
hil_sensors.accelerometer_range_m_s2 = 32.7f; // int16
|
||||
hil_sensors.accelerometer_counter = _hil_counter;
|
||||
hil_sensors.accelerometer_timestamp = timestamp;
|
||||
|
||||
hil_sensors.adc_voltage_v[0] = 0.0f;
|
||||
hil_sensors.adc_voltage_v[1] = 0.0f;
|
||||
|
@ -611,15 +610,15 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
|
|||
hil_sensors.magnetometer_range_ga = 32.7f; // int16
|
||||
hil_sensors.magnetometer_mode = 0; // TODO what is this
|
||||
hil_sensors.magnetometer_cuttoff_freq_hz = 50.0f;
|
||||
hil_sensors.magnetometer_counter = _hil_counter;
|
||||
hil_sensors.magnetometer_timestamp = timestamp;
|
||||
|
||||
hil_sensors.baro_pres_mbar = imu.abs_pressure;
|
||||
hil_sensors.baro_alt_meter = imu.pressure_alt;
|
||||
hil_sensors.baro_temp_celcius = imu.temperature;
|
||||
hil_sensors.baro_counter = _hil_counter;
|
||||
hil_sensors.baro_timestamp = timestamp;
|
||||
|
||||
hil_sensors.differential_pressure_pa = imu.diff_pressure * 1e2f; //from hPa to Pa
|
||||
hil_sensors.differential_pressure_counter = _hil_counter;
|
||||
hil_sensors.differential_pressure_timestamp = timestamp;
|
||||
|
||||
/* publish combined sensor topic */
|
||||
if (_sensors_pub > 0) {
|
||||
|
|
|
@ -200,8 +200,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
|
|||
bool landed = true;
|
||||
hrt_abstime landed_time = 0;
|
||||
|
||||
uint32_t accel_counter = 0;
|
||||
uint32_t baro_counter = 0;
|
||||
hrt_abstime accel_timestamp = 0;
|
||||
hrt_abstime baro_timestamp = 0;
|
||||
|
||||
bool ref_inited = false;
|
||||
hrt_abstime ref_init_start = 0;
|
||||
|
@ -310,8 +310,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
|
|||
if (fds_init[0].revents & POLLIN) {
|
||||
orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);
|
||||
|
||||
if (wait_baro && sensor.baro_counter != baro_counter) {
|
||||
baro_counter = sensor.baro_counter;
|
||||
if (wait_baro && sensor.baro_timestamp != baro_timestamp) {
|
||||
baro_timestamp = sensor.baro_timestamp;
|
||||
|
||||
/* mean calculation over several measurements */
|
||||
if (baro_init_cnt < baro_init_num) {
|
||||
|
@ -384,7 +384,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
|
|||
if (updated) {
|
||||
orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);
|
||||
|
||||
if (sensor.accelerometer_counter != accel_counter) {
|
||||
if (sensor.accelerometer_timestamp != accel_timestamp) {
|
||||
if (att.R_valid) {
|
||||
/* correct accel bias */
|
||||
sensor.accelerometer_m_s2[0] -= acc_bias[0];
|
||||
|
@ -408,13 +408,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
|
|||
memset(corr_acc, 0, sizeof(corr_acc));
|
||||
}
|
||||
|
||||
accel_counter = sensor.accelerometer_counter;
|
||||
accel_timestamp = sensor.accelerometer_timestamp;
|
||||
accel_updates++;
|
||||
}
|
||||
|
||||
if (sensor.baro_counter != baro_counter) {
|
||||
if (sensor.baro_timestamp != baro_timestamp) {
|
||||
corr_baro = baro_offset - sensor.baro_alt_meter - z_est[0];
|
||||
baro_counter = sensor.baro_counter;
|
||||
baro_timestamp = sensor.baro_timestamp;
|
||||
baro_updates++;
|
||||
}
|
||||
}
|
||||
|
|
|
@ -887,11 +887,11 @@ int sdlog2_thread_main(int argc, char *argv[])
|
|||
pthread_cond_init(&logbuffer_cond, NULL);
|
||||
|
||||
/* track changes in sensor_combined topic */
|
||||
uint16_t gyro_counter = 0;
|
||||
uint16_t accelerometer_counter = 0;
|
||||
uint16_t magnetometer_counter = 0;
|
||||
uint16_t baro_counter = 0;
|
||||
uint16_t differential_pressure_counter = 0;
|
||||
hrt_abstime gyro_timestamp = 0;
|
||||
hrt_abstime accelerometer_timestamp = 0;
|
||||
hrt_abstime magnetometer_timestamp = 0;
|
||||
hrt_abstime barometer_timestamp = 0;
|
||||
hrt_abstime differential_pressure_timestamp = 0;
|
||||
|
||||
/* track changes in distance status */
|
||||
bool dist_bottom_present = false;
|
||||
|
@ -976,28 +976,28 @@ int sdlog2_thread_main(int argc, char *argv[])
|
|||
bool write_IMU = false;
|
||||
bool write_SENS = false;
|
||||
|
||||
if (buf.sensor.gyro_counter != gyro_counter) {
|
||||
gyro_counter = buf.sensor.gyro_counter;
|
||||
if (buf.sensor.timestamp != gyro_timestamp) {
|
||||
gyro_timestamp = buf.sensor.timestamp;
|
||||
write_IMU = true;
|
||||
}
|
||||
|
||||
if (buf.sensor.accelerometer_counter != accelerometer_counter) {
|
||||
accelerometer_counter = buf.sensor.accelerometer_counter;
|
||||
if (buf.sensor.accelerometer_timestamp != accelerometer_timestamp) {
|
||||
accelerometer_timestamp = buf.sensor.accelerometer_timestamp;
|
||||
write_IMU = true;
|
||||
}
|
||||
|
||||
if (buf.sensor.magnetometer_counter != magnetometer_counter) {
|
||||
magnetometer_counter = buf.sensor.magnetometer_counter;
|
||||
if (buf.sensor.magnetometer_timestamp != magnetometer_timestamp) {
|
||||
magnetometer_timestamp = buf.sensor.magnetometer_timestamp;
|
||||
write_IMU = true;
|
||||
}
|
||||
|
||||
if (buf.sensor.baro_counter != baro_counter) {
|
||||
baro_counter = buf.sensor.baro_counter;
|
||||
if (buf.sensor.baro_timestamp != barometer_timestamp) {
|
||||
barometer_timestamp = buf.sensor.baro_timestamp;
|
||||
write_SENS = true;
|
||||
}
|
||||
|
||||
if (buf.sensor.differential_pressure_counter != differential_pressure_counter) {
|
||||
differential_pressure_counter = buf.sensor.differential_pressure_counter;
|
||||
if (buf.sensor.differential_pressure_timestamp != differential_pressure_timestamp) {
|
||||
differential_pressure_timestamp = buf.sensor.differential_pressure_timestamp;
|
||||
write_SENS = true;
|
||||
}
|
||||
|
||||
|
@ -1023,6 +1023,7 @@ int sdlog2_thread_main(int argc, char *argv[])
|
|||
log_msg.body.log_SENS.diff_pres = buf.sensor.differential_pressure_pa;
|
||||
LOGBUFFER_WRITE_AND_COUNT(SENS);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
/* --- ATTITUDE --- */
|
||||
|
|
|
@ -940,7 +940,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_counter++;
|
||||
raw.accelerometer_timestamp = accel_report.timestamp;
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -966,7 +966,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_counter++;
|
||||
raw.timestamp = gyro_report.timestamp;
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -996,7 +996,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_counter++;
|
||||
raw.magnetometer_timestamp = mag_report.timestamp;
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -1014,7 +1014,7 @@ Sensors::baro_poll(struct sensor_combined_s &raw)
|
|||
raw.baro_alt_meter = _barometer.altitude; // Altitude in meters
|
||||
raw.baro_temp_celcius = _barometer.temperature; // Temperature in degrees celcius
|
||||
|
||||
raw.baro_counter++;
|
||||
raw.baro_timestamp = _barometer.timestamp;
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -1028,7 +1028,7 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw)
|
|||
orb_copy(ORB_ID(differential_pressure), _diff_pres_sub, &_diff_pres);
|
||||
|
||||
raw.differential_pressure_pa = _diff_pres.differential_pressure_pa;
|
||||
raw.differential_pressure_counter++;
|
||||
raw.differential_pressure_timestamp = _diff_pres.timestamp;
|
||||
|
||||
_airspeed.indicated_airspeed_m_s = calc_indicated_airspeed(_diff_pres.differential_pressure_pa);
|
||||
_airspeed.true_airspeed_m_s = calc_true_airspeed(_diff_pres.differential_pressure_pa + raw.baro_pres_mbar * 1e2f,
|
||||
|
|
|
@ -77,34 +77,33 @@ struct sensor_combined_s {
|
|||
|
||||
/* NOTE: Ordering of fields optimized to align to 32 bit / 4 bytes Change with consideration only */
|
||||
|
||||
uint64_t timestamp; /**< Timestamp in microseconds since boot */
|
||||
uint64_t timestamp; /**< Timestamp in microseconds since boot, from gyro */
|
||||
|
||||
int16_t gyro_raw[3]; /**< Raw sensor values of angular velocity */
|
||||
uint16_t gyro_counter; /**< Number of raw measurments taken */
|
||||
float gyro_rad_s[3]; /**< Angular velocity in radian per seconds */
|
||||
|
||||
|
||||
int16_t accelerometer_raw[3]; /**< Raw acceleration in NED body frame */
|
||||
uint32_t accelerometer_counter; /**< Number of raw acc measurements taken */
|
||||
float accelerometer_m_s2[3]; /**< Acceleration in NED body frame, in m/s^2 */
|
||||
int accelerometer_mode; /**< Accelerometer measurement mode */
|
||||
float accelerometer_range_m_s2; /**< Accelerometer measurement range in m/s^2 */
|
||||
uint64_t accelerometer_timestamp; /**< Accelerometer timestamp */
|
||||
|
||||
int16_t magnetometer_raw[3]; /**< Raw magnetic field in NED body frame */
|
||||
float magnetometer_ga[3]; /**< Magnetic field in NED body frame, in Gauss */
|
||||
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 */
|
||||
uint32_t magnetometer_counter; /**< Number of raw mag measurements taken */
|
||||
|
||||
uint64_t magnetometer_timestamp; /**< Magnetometer timestamp */
|
||||
|
||||
float baro_pres_mbar; /**< Barometric pressure, already temp. comp. */
|
||||
float baro_alt_meter; /**< Altitude, already temp. comp. */
|
||||
float baro_temp_celcius; /**< Temperature in degrees celsius */
|
||||
float adc_voltage_v[4]; /**< ADC voltages of ADC Chan 10/11/12/13 or -1 */
|
||||
float mcu_temp_celcius; /**< Internal temperature measurement of MCU */
|
||||
uint32_t baro_counter; /**< Number of raw baro measurements taken */
|
||||
uint64_t baro_timestamp; /**< Barometer timestamp */
|
||||
|
||||
float differential_pressure_pa; /**< Airspeed sensor differential pressure */
|
||||
uint32_t differential_pressure_counter; /**< Number of raw differential pressure measurements taken */
|
||||
float differential_pressure_pa; /**< Airspeed sensor differential pressure */
|
||||
uint64_t differential_pressure_timestamp; /**< Last measurement timestamp */
|
||||
};
|
||||
|
||||
/**
|
||||
|
|
Loading…
Reference in New Issue