mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-04 15:08:28 -04:00
AP_Temperature: slow down temp driver thread and cleanup
This commit is contained in:
parent
4ee58c4496
commit
521b0f9d85
libraries/AP_TemperatureSensor
@ -124,6 +124,7 @@ void AP_TemperatureSensor::init()
|
||||
|
||||
// create each instance
|
||||
for (uint8_t instance = 0; instance < AP_TEMPERATURE_SENSOR_MAX_INSTANCES; instance++) {
|
||||
_state[instance].instance = instance;
|
||||
|
||||
switch (get_type(instance)) {
|
||||
#if AP_TEMPERATURE_SENSOR_TSYS01_ENABLED
|
||||
@ -148,7 +149,6 @@ void AP_TemperatureSensor::init()
|
||||
|
||||
// call init function for each backend
|
||||
if (drivers[instance] != nullptr) {
|
||||
_state[instance].instance = instance;
|
||||
drivers[instance]->init();
|
||||
// _num_instances is actually the index for looping over instances
|
||||
// the user may have TEMP_TYPE=0 and TEMP2_TYPE=7, in which case
|
||||
|
@ -71,7 +71,6 @@ private:
|
||||
|
||||
// The TemperatureSensor_State structure is filled in by the backend driver
|
||||
struct TemperatureSensor_State {
|
||||
const struct AP_Param::GroupInfo *var_info;
|
||||
uint32_t last_time_ms; // time when the sensor was last read in milliseconds
|
||||
float temperature; // temperature (deg C)
|
||||
uint8_t instance; // instance number
|
||||
|
@ -93,8 +93,8 @@ void AP_TemperatureSensor_MAX31865::init()
|
||||
|
||||
_dev->set_speed(AP_HAL::Device::SPEED_HIGH);
|
||||
|
||||
/* Request 10Hz update */
|
||||
_dev->register_periodic_callback(100 * AP_USEC_PER_MSEC,
|
||||
/* Request 5Hz update */
|
||||
_dev->register_periodic_callback(200 * AP_USEC_PER_MSEC,
|
||||
FUNCTOR_BIND_MEMBER(&AP_TemperatureSensor_MAX31865::thread_tick, void));
|
||||
}
|
||||
|
||||
|
@ -105,8 +105,8 @@ void AP_TemperatureSensor_MCP9600::init()
|
||||
// lower retries for run
|
||||
_dev->set_retries(3);
|
||||
|
||||
/* Request 10Hz update */
|
||||
_dev->register_periodic_callback(100 * AP_USEC_PER_MSEC,
|
||||
/* Request 5Hz update */
|
||||
_dev->register_periodic_callback(200 * AP_USEC_PER_MSEC,
|
||||
FUNCTOR_BIND_MEMBER(&AP_TemperatureSensor_MCP9600::_timer, void));
|
||||
}
|
||||
|
||||
|
Loading…
Reference in New Issue
Block a user