diff --git a/mavlink/include/mavlink/v2.0 b/mavlink/include/mavlink/v2.0 index 8f481688d7..5637057af5 160000 --- a/mavlink/include/mavlink/v2.0 +++ b/mavlink/include/mavlink/v2.0 @@ -1 +1 @@ -Subproject commit 8f481688d70541e8f91ee8c4fc233ca70cd7fad9 +Subproject commit 5637057af5ab8ec5667e3f8e5f8c73d3bc60eed8 diff --git a/msg/battery_status.msg b/msg/battery_status.msg index 9b242d80fd..29925fe883 100644 --- a/msg/battery_status.msg +++ b/msg/battery_status.msg @@ -21,7 +21,7 @@ uint16 cycle_count # number of discharge cycles the battery has experienced uint16 run_time_to_empty # predicted remaining battery capacity based on the present rate of discharge in min uint16 average_time_to_empty # predicted remaining battery capacity based on the average rate of discharge in min uint16 serial_number # serial number of the battery pack -uint16 manufacture_date # manufacture date, part of serial number of the battery pack +uint16 manufacture_date # manufacture date, part of serial number of the battery pack. formated as: Day + Month×32 + (Year–1980)×512 uint16 state_of_health # state of health. FullChargeCapacity/DesignCapacity. uint16 max_error # max error, expected margin of error in % in the state-of-charge calculation with a range of 1 to 100% uint8 id # ID number of a battery. Should be unique and consistent for the lifetime of a vehicle. 1-indexed. diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 1b715462d6..ca3a364a54 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -834,7 +834,19 @@ protected: msg.capacity_full_specification = battery_status.capacity; msg.capacity_full = (int32_t)((float)(battery_status.state_of_health * battery_status.capacity) / 100.f); msg.cycle_count = battery_status.cycle_count; - msg.serial_number = battery_status.serial_number + (battery_status.manufacture_date << 16); + + if (battery_status.manufacture_date) { + uint16_t day = battery_status.manufacture_date % 32; + uint16_t month = (battery_status.manufacture_date >> 5) % 16; + uint16_t year = (80 + (battery_status.manufacture_date >> 9)) % 100; + + //Formatted as 'dd/mm/yy-123456' (maxed 15 + 1 chars) + snprintf(msg.serial_number, sizeof(msg.serial_number), "%d/%d/%d-%d", day, month, year, battery_status.serial_number); + + } else { + snprintf(msg.serial_number, sizeof(msg.serial_number), "%d", battery_status.serial_number); + } + //msg.device_name = ?? msg.weight = -1; msg.discharge_minimum_voltage = -1;