|
|
@ -129,7 +129,7 @@ void AP_Airspeed_UAVCAN::handle_airspeed(AP_UAVCAN* ap_uavcan, uint8_t node_id, |
|
|
|
if (driver != nullptr) { |
|
|
|
if (driver != nullptr) { |
|
|
|
WITH_SEMAPHORE(driver->_sem_airspeed); |
|
|
|
WITH_SEMAPHORE(driver->_sem_airspeed); |
|
|
|
driver->_pressure = cb.msg->differential_pressure; |
|
|
|
driver->_pressure = cb.msg->differential_pressure; |
|
|
|
driver->_temperature = cb.msg->static_air_temperature; |
|
|
|
driver->_temperature = cb.msg->static_air_temperature - C_TO_KELVIN; |
|
|
|
driver->_last_sample_time_ms = AP_HAL::millis(); |
|
|
|
driver->_last_sample_time_ms = AP_HAL::millis(); |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
@ -164,7 +164,7 @@ bool AP_Airspeed_UAVCAN::get_temperature(float &temperature) |
|
|
|
return false; |
|
|
|
return false; |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
temperature = _temperature - C_TO_KELVIN; |
|
|
|
temperature = _temperature; |
|
|
|
|
|
|
|
|
|
|
|
return true; |
|
|
|
return true; |
|
|
|
} |
|
|
|
} |
|
|
|