Skip to content

Commit 6efa3fa

Browse files
sdomoszlai13sdomoszlai13
committed
Remove filter option; read serial number
1 parent e2bba22 commit 6efa3fa

File tree

3 files changed

+5
-81
lines changed

3 files changed

+5
-81
lines changed

‎src/drivers/uavcan/sensors/battery.cpp

+5-64
Original file line numberDiff line numberDiff line change
@@ -65,9 +65,6 @@ int UavcanBatteryBridge::init()
6565

6666
} else if (uavcan_sub_bat == RAW_AUX_CBAT_DATA) {
6767
_batt_update_mod[instance] = BatteryDataType::RawAuxCBAT;
68-
69-
} else if (uavcan_sub_bat == FILTER_DATA) {
70-
_batt_update_mod[instance] = BatteryDataType::Filter;
7168
}
7269
}
7370

@@ -85,8 +82,6 @@ int UavcanBatteryBridge::init()
8582
return res;
8683
}
8784

88-
return 0;
89-
9085
res = _sub_cbat.start(CBATCbBinder(this, &UavcanBatteryBridge::cbat_sub_cb));
9186

9287
if (res < 0) {
@@ -100,8 +95,6 @@ int UavcanBatteryBridge::init()
10095
void
10196
UavcanBatteryBridge::battery_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::power::BatteryInfo> &msg)
10297
{
103-
PX4_INFO("Battery Sub Cb called");
104-
10598
uint8_t instance = 0;
10699

107100
for (instance = 0; instance < battery_status_s::MAX_INSTANCES; instance++) {
@@ -114,12 +107,6 @@ UavcanBatteryBridge::battery_sub_cb(const uavcan::ReceivedDataStructure<uavcan::
114107
return;
115108
}
116109

117-
if (_batt_update_mod[instance] == BatteryDataType::Filter) {
118-
119-
filterData(msg, instance);
120-
return;
121-
}
122-
123110
_battery_status[instance].timestamp = hrt_absolute_time();
124111
_battery_status[instance].voltage_v = msg.voltage;
125112
_battery_status[instance].current_a = msg.current;
@@ -130,7 +117,7 @@ UavcanBatteryBridge::battery_sub_cb(const uavcan::ReceivedDataStructure<uavcan::
130117
_battery_status[instance].discharged_mah = _discharged_mah;
131118
}
132119

133-
_battery_status[instance].remaining = msg.state_of_charge_pct / 100.0f; // between 0 and 1
120+
_battery_status[instance].remaining = msg.state_of_charge_pct / 100.0f; // Between 0 and 1
134121
// _battery_status[instance].scale = msg.; // Power scaling factor, >= 1, or -1 if unknown
135122
_battery_status[instance].temperature = msg.temperature + atmosphere::kAbsoluteNullCelsius; // Kelvin to Celsius
136123
// _battery_status[instance].cell_count = msg.;
@@ -163,17 +150,13 @@ UavcanBatteryBridge::battery_sub_cb(const uavcan::ReceivedDataStructure<uavcan::
163150

164151
if (_batt_update_mod[instance] == BatteryDataType::Raw) {
165152
publish(msg.getSrcNodeID().get(), &_battery_status[instance]);
166-
PX4_INFO("Battery Sub Cb published");
167-
168153
}
169154
}
170155

171156
void
172157
UavcanBatteryBridge::battery_aux_sub_cb(const uavcan::ReceivedDataStructure<ardupilot::equipment::power::BatteryInfoAux>
173158
&msg)
174159
{
175-
PX4_INFO("Battery Aux Sub Cb called");
176-
177160
uint8_t instance = 0;
178161

179162
for (instance = 0; instance < battery_status_s::MAX_INSTANCES; instance++) {
@@ -186,10 +169,6 @@ UavcanBatteryBridge::battery_aux_sub_cb(const uavcan::ReceivedDataStructure<ardu
186169
return;
187170
}
188171

189-
if (_batt_update_mod[instance] == BatteryDataType::Filter) {
190-
return;
191-
}
192-
193172
_battery_status[instance].discharged_mah = (_battery_status[instance].full_charge_capacity_wh -
194173
_battery_status[instance].remaining_capacity_wh) / msg.nominal_voltage *
195174
1000;
@@ -208,7 +187,6 @@ UavcanBatteryBridge::battery_aux_sub_cb(const uavcan::ReceivedDataStructure<ardu
208187

209188
if (_batt_update_mod[instance] == BatteryDataType::RawAux) {
210189
publish(msg.getSrcNodeID().get(), &_battery_status[instance]);
211-
PX4_INFO("Battery Aux Sub Cb published");
212190
}
213191

214192
}
@@ -217,18 +195,6 @@ UavcanBatteryBridge::battery_aux_sub_cb(const uavcan::ReceivedDataStructure<ardu
217195
void
218196
UavcanBatteryBridge::cbat_sub_cb(const uavcan::ReceivedDataStructure<cuav::equipment::power::CBAT> &msg)
219197
{
220-
/*
221-
if (msg.serial_number != 0){
222-
PX4_ERR("CBAT message received");
223-
}
224-
225-
else{
226-
PX4_ERR("No CBAT message received");
227-
}
228-
*/
229-
230-
PX4_INFO("Battery CBAT Sub Cb called");
231-
232198
uint8_t instance = 0;
233199

234200
for (instance = 0; instance < battery_status_s::MAX_INSTANCES; instance++) {
@@ -242,24 +208,17 @@ UavcanBatteryBridge::cbat_sub_cb(const uavcan::ReceivedDataStructure<cuav::equip
242208
return;
243209
}
244210

245-
if (_batt_update_mod[instance] == BatteryDataType::Filter) {
246-
return;
247-
}
248-
249211
determineWarning(_battery_status[instance].remaining);
250212
_battery_status[instance].warning = _warning;
251213

252214
if (_batt_update_mod[instance] == BatteryDataType::RawAuxCBAT) {
253-
// char ser_num[32] = {0};
254-
// snprintf(ser_num, 32, "%hu", msg.serial_number);
255-
// strncpy(_battery_status[instance].serial_number, ser_num, sizeof(_battery_status[instance].serial_number) - 1);
256-
strncpy(_battery_status[instance].serial_number, "123456", sizeof(_battery_status[instance].serial_number));
215+
char ser_num[32] = {0};
216+
snprintf(ser_num, 32, "%hu", msg.serial_number);
217+
strncpy(_battery_status[instance].serial_number, ser_num, sizeof(_battery_status[instance].serial_number) - 1);
257218

258219
_battery_status[instance].serial_number[sizeof(_battery_status[instance].serial_number) - 1] = '\0';
259220

260221
publish(msg.getSrcNodeID().get(), &_battery_status[instance]);
261-
PX4_INFO("CBAT Sub Cb published");
262-
263222
}
264223
}
265224

@@ -288,7 +247,7 @@ UavcanBatteryBridge::sumDischarged(hrt_abstime timestamp, float current_a)
288247
void
289248
UavcanBatteryBridge::determineWarning(float remaining)
290249
{
291-
// propagate warning state only if the state is higher, otherwise remain in current warning state
250+
// Propagate warning state only if new state is higher, otherwise remain in current warning state
292251
if (remaining < _param_bat_emergen_thr.get() || (_warning == battery_status_s::WARNING_EMERGENCY)) {
293252
_warning = battery_status_s::WARNING_EMERGENCY;
294253

@@ -299,21 +258,3 @@ UavcanBatteryBridge::determineWarning(float remaining)
299258
_warning = battery_status_s::WARNING_LOW;
300259
}
301260
}
302-
303-
void
304-
UavcanBatteryBridge::filterData(const uavcan::ReceivedDataStructure<uavcan::equipment::power::BatteryInfo> &msg,
305-
uint8_t instance)
306-
{
307-
_battery[instance]->setConnected(true);
308-
_battery[instance]->updateVoltage(msg.voltage);
309-
_battery[instance]->updateCurrent(msg.current);
310-
_battery[instance]->updateBatteryStatus(hrt_absolute_time());
311-
312-
/* Override data that is expected to arrive from UAVCAN msg*/
313-
_battery_status[instance] = _battery[instance]->getBatteryStatus();
314-
_battery_status[instance].temperature = msg.temperature + atmosphere::kAbsoluteNullCelsius; // Kelvin to Celsius
315-
//_battery_status[instance].serial_number = msg.model_instance_id;
316-
_battery_status[instance].id = msg.getSrcNodeID().get(); // overwrite zeroed index from _battery
317-
318-
publish(msg.getSrcNodeID().get(), &_battery_status[instance]);
319-
}

‎src/drivers/uavcan/sensors/battery.hpp

-16
Original file line numberDiff line numberDiff line change
@@ -66,15 +66,13 @@ class UavcanBatteryBridge : public UavcanSensorBridgeBase, public ModuleParams
6666
Raw, // data from BatteryInfo message only
6767
RawAux, // data combination from BatteryInfo and BatteryInfoAux messages
6868
RawAuxCBAT, // data combination from BatteryInfo, BatteryInfoAux, and CBAT messages
69-
Filter, // filter data from BatteryInfo message with Battery library
7069
};
7170

7271
void battery_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::power::BatteryInfo> &msg);
7372
void battery_aux_sub_cb(const uavcan::ReceivedDataStructure<ardupilot::equipment::power::BatteryInfoAux> &msg);
7473
void cbat_sub_cb(const uavcan::ReceivedDataStructure<cuav::equipment::power::CBAT> &msg);
7574
void sumDischarged(hrt_abstime timestamp, float current_a);
7675
void determineWarning(float remaining);
77-
void filterData(const uavcan::ReceivedDataStructure<uavcan::equipment::power::BatteryInfo> &msg, uint8_t instance);
7876

7977
typedef uavcan::MethodBinder < UavcanBatteryBridge *,
8078
void (UavcanBatteryBridge::*)
@@ -109,19 +107,5 @@ class UavcanBatteryBridge : public UavcanSensorBridgeBase, public ModuleParams
109107
static constexpr int RAW_DATA = 1;
110108
static constexpr int RAW_AUX_DATA = 2;
111109
static constexpr int RAW_AUX_CBAT_DATA = 3;
112-
static constexpr int FILTER_DATA = 4;
113-
static constexpr int BATTERY_INDEX_1 = 1;
114-
static constexpr int BATTERY_INDEX_2 = 2;
115-
static constexpr int BATTERY_INDEX_3 = 3;
116-
static constexpr int BATTERY_INDEX_4 = 4;
117110
static constexpr int SAMPLE_INTERVAL_US = 20_ms; // assume higher frequency UAVCAN feedback than 50Hz
118-
119-
static_assert(battery_status_s::MAX_INSTANCES <= BATTERY_INDEX_4, "Battery array too big");
120-
121-
Battery battery1 = {BATTERY_INDEX_1, this, SAMPLE_INTERVAL_US, battery_status_s::EXTERNAL};
122-
Battery battery2 = {BATTERY_INDEX_2, this, SAMPLE_INTERVAL_US, battery_status_s::EXTERNAL};
123-
Battery battery3 = {BATTERY_INDEX_3, this, SAMPLE_INTERVAL_US, battery_status_s::EXTERNAL};
124-
Battery battery4 = {BATTERY_INDEX_4, this, SAMPLE_INTERVAL_US, battery_status_s::EXTERNAL};
125-
126-
Battery *_battery[battery_status_s::MAX_INSTANCES] = { &battery1, &battery2, &battery3, &battery4 };
127111
};

‎src/drivers/uavcan/uavcan_params.c

-1
Original file line numberDiff line numberDiff line change
@@ -302,7 +302,6 @@ PARAM_DEFINE_INT32(UAVCAN_SUB_BARO, 0);
302302
* @value 1 Raw data
303303
* @value 2 Raw and auxiliary data
304304
* @value 3 Raw, auxiliary, and CBAT-specific data
305-
* @value 4 Filter data
306305
* @reboot_required true
307306
* @group UAVCAN
308307
*/

0 commit comments

Comments
 (0)