Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Reorder AP_ESC_Telem fields #26369

Closed
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 3 additions & 1 deletion libraries/AP_BLHeli/AP_BLHeli.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -1472,10 +1472,12 @@ void AP_BLHeli::read_telemetry_packet(void)
update_rpm(normalized_motor_idx, new_rpm);

TelemetryData t {
.temperature_cdeg = int16_t(buf[0] * 100),
.voltage = float(uint16_t((buf[1]<<8) | buf[2])) * 0.01,
.current = float(uint16_t((buf[3]<<8) | buf[4])) * 0.01,
.consumption_mah = float(uint16_t((buf[5]<<8) | buf[6])),
.usage_s = 0,
.last_update_ms = 0,
.temperature_cdeg = int16_t(buf[0] * 100),
};

update_telem_data(normalized_motor_idx, t,
Expand Down
10 changes: 8 additions & 2 deletions libraries/AP_DroneCAN/AP_DroneCAN.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -629,9 +629,12 @@ void AP_DroneCAN::handle_hobbywing_StatusMsg2(const CanardRxTransfer& transfer,
uint8_t esc_index;
if (hobbywing_find_esc_index(transfer.source_node_id, esc_index)) {
TelemetryData t {
.temperature_cdeg = int16_t(msg.temperature*100),
.voltage = msg.input_voltage*0.1f,
.current = msg.current*0.1f,
.consumption_mah = 0.0f,
.usage_s = 0,
.last_update_ms = 0,
.temperature_cdeg = int16_t(msg.temperature*100),
};
update_telem_data(esc_index, t,
AP_ESC_Telem_Backend::TelemetryType::CURRENT|
Expand Down Expand Up @@ -1430,9 +1433,12 @@ void AP_DroneCAN::handle_ESC_status(const CanardRxTransfer& transfer, const uavc
}

TelemetryData t {
.temperature_cdeg = int16_t((KELVIN_TO_C(msg.temperature)) * 100),
.voltage = msg.voltage,
.current = msg.current,
.consumption_mah = 0.0f,
.usage_s = 0,
.last_update_ms = 0,
.temperature_cdeg = int16_t((KELVIN_TO_C(msg.temperature)) * 100),
};

update_rpm(esc_index, msg.rpm, msg.error_count);
Expand Down
4 changes: 2 additions & 2 deletions libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.h
Original file line number Diff line number Diff line change
Expand Up @@ -10,13 +10,13 @@ class AP_ESC_Telem_Backend {
public:

struct TelemetryData {
int16_t temperature_cdeg; // centi-degrees C, negative values allowed
float voltage; // Volt
float current; // Ampere
float consumption_mah; // milli-Ampere.hours
uint32_t usage_s; // usage seconds
int16_t motor_temp_cdeg; // centi-degrees C, negative values allowed
uint32_t last_update_ms; // last update time in milliseconds, determines whether active
int16_t temperature_cdeg; // centi-degrees C, negative values allowed
int16_t motor_temp_cdeg; // centi-degrees C, negative values allowed
uint16_t types; // telemetry types present
uint16_t count; // number of times updated

Expand Down
4 changes: 3 additions & 1 deletion libraries/AP_ESC_Telem/AP_ESC_Telem_SITL.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -67,10 +67,12 @@ void AP_ESC_Telem_SITL::update()

// some fake values so that is_telemetry_active() returns true
TelemetryData t {
.temperature_cdeg = 3200,
.voltage = 16.8f,
.current = 0.8f,
.consumption_mah = 1.0f,
.usage_s = 0,
.last_update_ms = 0,
.temperature_cdeg = 3200,
};

update_telem_data(motor, t,
Expand Down
5 changes: 4 additions & 1 deletion libraries/AP_KDECAN/AP_KDECAN.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -134,9 +134,12 @@ void AP_KDECAN_Driver::handle_frame(AP_HAL::CANFrame &frame)
update_rpm(idx, uint16_t(uint16_t(frame.data[4] << 8 | frame.data[5]) * 60UL * 2 / num_poles));

const TelemetryData t {
.temperature_cdeg = int16_t(frame.data[6] * 100),
.voltage = float(uint16_t(frame.data[0] << 8 | frame.data[1])) * 0.01f,
.current = float(uint16_t(frame.data[2] << 8 | frame.data[3])) * 0.01f,
.consumption_mah = 0.0f,
.usage_s = 0,
.last_update_ms = 0,
.temperature_cdeg = int16_t(frame.data[6] * 100),
};
update_telem_data(idx, t,
AP_ESC_Telem_Backend::TelemetryType::CURRENT |
Expand Down
Loading