forked from Archive/PX4-Autopilot
Merge branch 'master' of github.com:PX4/Firmware into px4io-i2c
This commit is contained in:
commit
7c8942f46c
|
@ -33,7 +33,7 @@ end
|
|||
% float vbat; //battery voltage in [volt]
|
||||
% float bat_current - current drawn from battery at this time instant
|
||||
% float bat_discharged - discharged energy in mAh
|
||||
% float adc[3]; //remaining auxiliary ADC ports [volt]
|
||||
% float adc[4]; //ADC ports [volt]
|
||||
% float local_position[3]; //tangent plane mapping into x,y,z [m]
|
||||
% int32_t gps_raw_position[3]; //latitude [degrees] north, longitude [degrees] east, altitude above MSL [millimeter]
|
||||
% float attitude[3]; //pitch, roll, yaw [rad]
|
||||
|
@ -57,7 +57,7 @@ logFormat{9} = struct('name', 'actuators', 'bytes', 4, 'array', 8, '
|
|||
logFormat{10} = struct('name', 'vbat', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le');
|
||||
logFormat{11} = struct('name', 'bat_current', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le');
|
||||
logFormat{12} = struct('name', 'bat_discharged', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le');
|
||||
logFormat{13} = struct('name', 'adc', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le');
|
||||
logFormat{13} = struct('name', 'adc', 'bytes', 4, 'array', 4, 'precision', 'float', 'machineformat', 'ieee-le');
|
||||
logFormat{14} = struct('name', 'local_position', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le');
|
||||
logFormat{15} = struct('name', 'gps_raw_position', 'bytes', 4, 'array', 3, 'precision', 'uint32', 'machineformat', 'ieee-le');
|
||||
logFormat{16} = struct('name', 'attitude', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le');
|
||||
|
|
|
@ -683,7 +683,7 @@ int sdlog_thread_main(int argc, char *argv[])
|
|||
.vbat = buf.batt.voltage_v,
|
||||
.bat_current = buf.batt.current_a,
|
||||
.bat_discharged = buf.batt.discharged_mah,
|
||||
.adc = {buf.raw.adc_voltage_v[0], buf.raw.adc_voltage_v[1], buf.raw.adc_voltage_v[2]},
|
||||
.adc = {buf.raw.adc_voltage_v[0], buf.raw.adc_voltage_v[1], buf.raw.adc_voltage_v[2], buf.raw.adc_voltage_v[3]},
|
||||
.local_position = {buf.local_pos.x, buf.local_pos.y, buf.local_pos.z},
|
||||
.gps_raw_position = {buf.gps_pos.lat, buf.gps_pos.lon, buf.gps_pos.alt},
|
||||
.attitude = {buf.att.pitch, buf.att.roll, buf.att.yaw},
|
||||
|
|
|
@ -56,7 +56,7 @@ struct sdlog_sysvector {
|
|||
float vbat; /**< battery voltage in [volt] */
|
||||
float bat_current; /**< battery discharge current */
|
||||
float bat_discharged; /**< discharged energy in mAh */
|
||||
float adc[3]; /**< remaining auxiliary ADC ports [volt] */
|
||||
float adc[4]; /**< ADC ports [volt] */
|
||||
float local_position[3]; /**< tangent plane mapping into x,y,z [m] */
|
||||
int32_t gps_raw_position[3]; /**< latitude [degrees] north, longitude [degrees] east, altitude above MSL [millimeter] */
|
||||
float attitude[3]; /**< roll, pitch, yaw [rad] */
|
||||
|
@ -88,4 +88,4 @@ void sdlog_logbuffer_write(struct sdlog_logbuffer *lb, const struct sdlog_sysvec
|
|||
|
||||
int sdlog_logbuffer_read(struct sdlog_logbuffer *lb, struct sdlog_sysvector *elem);
|
||||
|
||||
#endif
|
||||
#endif
|
||||
|
|
|
@ -998,12 +998,16 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
|
|||
/* read all channels available */
|
||||
int ret = read(_fd_adc, &buf_adc, sizeof(buf_adc));
|
||||
|
||||
/* look for battery channel */
|
||||
|
||||
for (unsigned i = 0; i < sizeof(buf_adc) / sizeof(buf_adc[0]); i++) {
|
||||
|
||||
if (ret >= (int)sizeof(buf_adc[0])) {
|
||||
|
||||
/* Save raw voltage values */
|
||||
if (i < (sizeof(raw.adc_voltage_v)) / sizeof(raw.adc_voltage_v[0])) {
|
||||
raw.adc_voltage_v[i] = buf_adc[i].am_data / (4096.0f / 3.3f);
|
||||
}
|
||||
|
||||
/* look for specific channels and process the raw voltage to measurement data */
|
||||
if (ADC_BATTERY_VOLTAGE_CHANNEL == buf_adc[i].am_channel) {
|
||||
/* Voltage in volts */
|
||||
float voltage = (buf_adc[i].am_data * _parameters.battery_voltage_scaling);
|
||||
|
|
|
@ -429,6 +429,12 @@ ORBDevNode::appears_updated(SubscriberData *sd)
|
|||
/* avoid racing between interrupt and non-interrupt context calls */
|
||||
irqstate_t state = irqsave();
|
||||
|
||||
/* check if this topic has been published yet, if not bail out */
|
||||
if (_data == nullptr) {
|
||||
ret = false;
|
||||
goto out;
|
||||
}
|
||||
|
||||
/*
|
||||
* If the subscriber's generation count matches the update generation
|
||||
* count, there has been no update from their perspective; if they
|
||||
|
@ -485,6 +491,7 @@ ORBDevNode::appears_updated(SubscriberData *sd)
|
|||
break;
|
||||
}
|
||||
|
||||
out:
|
||||
irqrestore(state);
|
||||
|
||||
/* consider it updated */
|
||||
|
|
Loading…
Reference in New Issue