|
|
@ -24,7 +24,6 @@ AP_BattMonitor_UAVCAN::AP_BattMonitor_UAVCAN(AP_BattMonitor &mon, AP_BattMonitor |
|
|
|
{ |
|
|
|
{ |
|
|
|
// starts with not healthy
|
|
|
|
// starts with not healthy
|
|
|
|
_state.healthy = false; |
|
|
|
_state.healthy = false; |
|
|
|
_params._serial_number = AP_BATT_SERIAL_NUMBER_DEFAULT; |
|
|
|
|
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
void AP_BattMonitor_UAVCAN::subscribe_msgs(AP_UAVCAN* ap_uavcan) |
|
|
|
void AP_BattMonitor_UAVCAN::subscribe_msgs(AP_UAVCAN* ap_uavcan) |
|
|
@ -45,7 +44,7 @@ void AP_BattMonitor_UAVCAN::subscribe_msgs(AP_UAVCAN* ap_uavcan) |
|
|
|
} |
|
|
|
} |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
AP_BattMonitor_UAVCAN* AP_BattMonitor_UAVCAN::get_uavcan_backend(AP_UAVCAN* ap_uavcan, uint8_t node_id,uint8_t battery_id, bool create_new) |
|
|
|
AP_BattMonitor_UAVCAN* AP_BattMonitor_UAVCAN::get_uavcan_backend(AP_UAVCAN* ap_uavcan, uint8_t node_id,uint8_t battery_id) |
|
|
|
{ |
|
|
|
{ |
|
|
|
if (ap_uavcan == nullptr) { |
|
|
|
if (ap_uavcan == nullptr) { |
|
|
|
return nullptr; |
|
|
|
return nullptr; |
|
|
@ -82,10 +81,9 @@ AP_BattMonitor_UAVCAN* AP_BattMonitor_UAVCAN::get_uavcan_backend(AP_UAVCAN* ap_u |
|
|
|
return nullptr; |
|
|
|
return nullptr; |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
void AP_BattMonitor_UAVCAN::handle_battery_info(const BattInfoCb &cb,uint8_t index) |
|
|
|
void AP_BattMonitor_UAVCAN::handle_battery_info(const BattInfoCb &cb) |
|
|
|
{ |
|
|
|
{ |
|
|
|
WITH_SEMAPHORE(_sem_battmon); |
|
|
|
WITH_SEMAPHORE(_sem_battmon); |
|
|
|
_params._serial_number.set_and_notify(index); |
|
|
|
|
|
|
|
_interim_state.temperature = cb.msg->temperature; |
|
|
|
_interim_state.temperature = cb.msg->temperature; |
|
|
|
_interim_state.voltage = cb.msg->voltage; |
|
|
|
_interim_state.voltage = cb.msg->voltage; |
|
|
|
_interim_state.current_amps = cb.msg->current; |
|
|
|
_interim_state.current_amps = cb.msg->current; |
|
|
@ -108,12 +106,12 @@ void AP_BattMonitor_UAVCAN::handle_battery_info(const BattInfoCb &cb,uint8_t ind |
|
|
|
|
|
|
|
|
|
|
|
void AP_BattMonitor_UAVCAN::handle_battery_info_trampoline(AP_UAVCAN* ap_uavcan, uint8_t node_id, const BattInfoCb &cb) |
|
|
|
void AP_BattMonitor_UAVCAN::handle_battery_info_trampoline(AP_UAVCAN* ap_uavcan, uint8_t node_id, const BattInfoCb &cb) |
|
|
|
{ |
|
|
|
{ |
|
|
|
AP_BattMonitor_UAVCAN* driver = get_uavcan_backend(ap_uavcan, node_id,ap_uavcan->get_driver_index(),true); |
|
|
|
AP_BattMonitor_UAVCAN* driver = get_uavcan_backend(ap_uavcan, node_id,ap_uavcan->get_driver_index()); |
|
|
|
|
|
|
|
|
|
|
|
if (driver == nullptr) { |
|
|
|
if (driver == nullptr) { |
|
|
|
return; |
|
|
|
return; |
|
|
|
} |
|
|
|
} |
|
|
|
driver->handle_battery_info(cb,ap_uavcan->get_driver_index()); |
|
|
|
driver->handle_battery_info(cb); |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
// read - read the voltage and current
|
|
|
|
// read - read the voltage and current
|
|
|
|