|
|
@@ -8,22 +8,35 @@ typedef struct __mavlink_shangfei_bms_t {
|
|
|
uint32_t time_boot_ms; /*< [ms] Timestamp in ms from system boot.*/
|
|
|
uint16_t bat_voltage; /*< [dV] Battery voltage.*/
|
|
|
int16_t bat_current; /*< [dA] Battery current.*/
|
|
|
+ uint16_t cell_max_volt; /*< [mV] Cell max voltage.*/
|
|
|
+ uint16_t cell_min_volt; /*< [mV] Cell min voltage.*/
|
|
|
+ uint16_t dc28_volt; /*< [dV] High volt-box voltage.*/
|
|
|
uint16_t hvb_voltage; /*< [dV] High volt-box voltage.*/
|
|
|
int8_t bat_maxtemp; /*< Battery Max_temperature.*/
|
|
|
+ int8_t bat_mintemp; /*< Battery Min_temperature.*/
|
|
|
+ uint8_t contactor_sta; /*< Contactor status.*/
|
|
|
+ uint8_t soh_value; /*< [%] Battery SOH value.*/
|
|
|
+ uint8_t max_volt_id; /*< Cell max voltage id.*/
|
|
|
+ uint8_t min_volt_id; /*< Cell min voltage id.*/
|
|
|
+ int8_t max_temp_id; /*< Cell max temperature id.*/
|
|
|
+ int8_t min_temp_id; /*< Cell min temperature id.*/
|
|
|
+ uint8_t fault_level; /*< Fault level.*/
|
|
|
+ uint8_t irt_status; /*< Insulation Resistance Tester status.*/
|
|
|
uint8_t cap_percent; /*< [%] Battery capacity.*/
|
|
|
uint8_t bat_id; /*< Battery id.*/
|
|
|
uint8_t err_code; /*< Error code.*/
|
|
|
+ uint8_t dc28_current; /*< [A] High volt-box current.*/
|
|
|
int8_t hvb_temp; /*< High volt-box temperature.*/
|
|
|
uint8_t hvb_status; /*< High volt-box status.*/
|
|
|
} mavlink_shangfei_bms_t;
|
|
|
|
|
|
-#define MAVLINK_MSG_ID_SHANGFEI_BMS_LEN 16
|
|
|
-#define MAVLINK_MSG_ID_SHANGFEI_BMS_MIN_LEN 16
|
|
|
-#define MAVLINK_MSG_ID_53310_LEN 16
|
|
|
-#define MAVLINK_MSG_ID_53310_MIN_LEN 16
|
|
|
+#define MAVLINK_MSG_ID_SHANGFEI_BMS_LEN 32
|
|
|
+#define MAVLINK_MSG_ID_SHANGFEI_BMS_MIN_LEN 32
|
|
|
+#define MAVLINK_MSG_ID_53310_LEN 32
|
|
|
+#define MAVLINK_MSG_ID_53310_MIN_LEN 32
|
|
|
|
|
|
-#define MAVLINK_MSG_ID_SHANGFEI_BMS_CRC 202
|
|
|
-#define MAVLINK_MSG_ID_53310_CRC 202
|
|
|
+#define MAVLINK_MSG_ID_SHANGFEI_BMS_CRC 203
|
|
|
+#define MAVLINK_MSG_ID_53310_CRC 203
|
|
|
|
|
|
|
|
|
|
|
|
@@ -31,33 +44,59 @@ typedef struct __mavlink_shangfei_bms_t {
|
|
|
#define MAVLINK_MESSAGE_INFO_SHANGFEI_BMS { \
|
|
|
53310, \
|
|
|
"SHANGFEI_BMS", \
|
|
|
- 10, \
|
|
|
+ 23, \
|
|
|
{ { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_shangfei_bms_t, time_boot_ms) }, \
|
|
|
{ "bat_voltage", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_shangfei_bms_t, bat_voltage) }, \
|
|
|
{ "bat_current", NULL, MAVLINK_TYPE_INT16_T, 0, 6, offsetof(mavlink_shangfei_bms_t, bat_current) }, \
|
|
|
- { "bat_maxtemp", NULL, MAVLINK_TYPE_INT8_T, 0, 10, offsetof(mavlink_shangfei_bms_t, bat_maxtemp) }, \
|
|
|
- { "cap_percent", NULL, MAVLINK_TYPE_UINT8_T, 0, 11, offsetof(mavlink_shangfei_bms_t, cap_percent) }, \
|
|
|
- { "bat_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 12, offsetof(mavlink_shangfei_bms_t, bat_id) }, \
|
|
|
- { "err_code", NULL, MAVLINK_TYPE_UINT8_T, 0, 13, offsetof(mavlink_shangfei_bms_t, err_code) }, \
|
|
|
- { "hvb_voltage", NULL, MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_shangfei_bms_t, hvb_voltage) }, \
|
|
|
- { "hvb_temp", NULL, MAVLINK_TYPE_INT8_T, 0, 14, offsetof(mavlink_shangfei_bms_t, hvb_temp) }, \
|
|
|
- { "hvb_status", NULL, MAVLINK_TYPE_UINT8_T, 0, 15, offsetof(mavlink_shangfei_bms_t, hvb_status) }, \
|
|
|
+ { "cell_max_volt", NULL, MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_shangfei_bms_t, cell_max_volt) }, \
|
|
|
+ { "cell_min_volt", NULL, MAVLINK_TYPE_UINT16_T, 0, 10, offsetof(mavlink_shangfei_bms_t, cell_min_volt) }, \
|
|
|
+ { "bat_maxtemp", NULL, MAVLINK_TYPE_INT8_T, 0, 16, offsetof(mavlink_shangfei_bms_t, bat_maxtemp) }, \
|
|
|
+ { "bat_mintemp", NULL, MAVLINK_TYPE_INT8_T, 0, 17, offsetof(mavlink_shangfei_bms_t, bat_mintemp) }, \
|
|
|
+ { "contactor_sta", NULL, MAVLINK_TYPE_UINT8_T, 0, 18, offsetof(mavlink_shangfei_bms_t, contactor_sta) }, \
|
|
|
+ { "soh_value", NULL, MAVLINK_TYPE_UINT8_T, 0, 19, offsetof(mavlink_shangfei_bms_t, soh_value) }, \
|
|
|
+ { "max_volt_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 20, offsetof(mavlink_shangfei_bms_t, max_volt_id) }, \
|
|
|
+ { "min_volt_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 21, offsetof(mavlink_shangfei_bms_t, min_volt_id) }, \
|
|
|
+ { "max_temp_id", NULL, MAVLINK_TYPE_INT8_T, 0, 22, offsetof(mavlink_shangfei_bms_t, max_temp_id) }, \
|
|
|
+ { "min_temp_id", NULL, MAVLINK_TYPE_INT8_T, 0, 23, offsetof(mavlink_shangfei_bms_t, min_temp_id) }, \
|
|
|
+ { "fault_level", NULL, MAVLINK_TYPE_UINT8_T, 0, 24, offsetof(mavlink_shangfei_bms_t, fault_level) }, \
|
|
|
+ { "irt_status", NULL, MAVLINK_TYPE_UINT8_T, 0, 25, offsetof(mavlink_shangfei_bms_t, irt_status) }, \
|
|
|
+ { "cap_percent", NULL, MAVLINK_TYPE_UINT8_T, 0, 26, offsetof(mavlink_shangfei_bms_t, cap_percent) }, \
|
|
|
+ { "bat_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 27, offsetof(mavlink_shangfei_bms_t, bat_id) }, \
|
|
|
+ { "err_code", NULL, MAVLINK_TYPE_UINT8_T, 0, 28, offsetof(mavlink_shangfei_bms_t, err_code) }, \
|
|
|
+ { "dc28_volt", NULL, MAVLINK_TYPE_UINT16_T, 0, 12, offsetof(mavlink_shangfei_bms_t, dc28_volt) }, \
|
|
|
+ { "dc28_current", NULL, MAVLINK_TYPE_UINT8_T, 0, 29, offsetof(mavlink_shangfei_bms_t, dc28_current) }, \
|
|
|
+ { "hvb_voltage", NULL, MAVLINK_TYPE_UINT16_T, 0, 14, offsetof(mavlink_shangfei_bms_t, hvb_voltage) }, \
|
|
|
+ { "hvb_temp", NULL, MAVLINK_TYPE_INT8_T, 0, 30, offsetof(mavlink_shangfei_bms_t, hvb_temp) }, \
|
|
|
+ { "hvb_status", NULL, MAVLINK_TYPE_UINT8_T, 0, 31, offsetof(mavlink_shangfei_bms_t, hvb_status) }, \
|
|
|
} \
|
|
|
}
|
|
|
#else
|
|
|
#define MAVLINK_MESSAGE_INFO_SHANGFEI_BMS { \
|
|
|
"SHANGFEI_BMS", \
|
|
|
- 10, \
|
|
|
+ 23, \
|
|
|
{ { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_shangfei_bms_t, time_boot_ms) }, \
|
|
|
{ "bat_voltage", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_shangfei_bms_t, bat_voltage) }, \
|
|
|
{ "bat_current", NULL, MAVLINK_TYPE_INT16_T, 0, 6, offsetof(mavlink_shangfei_bms_t, bat_current) }, \
|
|
|
- { "bat_maxtemp", NULL, MAVLINK_TYPE_INT8_T, 0, 10, offsetof(mavlink_shangfei_bms_t, bat_maxtemp) }, \
|
|
|
- { "cap_percent", NULL, MAVLINK_TYPE_UINT8_T, 0, 11, offsetof(mavlink_shangfei_bms_t, cap_percent) }, \
|
|
|
- { "bat_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 12, offsetof(mavlink_shangfei_bms_t, bat_id) }, \
|
|
|
- { "err_code", NULL, MAVLINK_TYPE_UINT8_T, 0, 13, offsetof(mavlink_shangfei_bms_t, err_code) }, \
|
|
|
- { "hvb_voltage", NULL, MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_shangfei_bms_t, hvb_voltage) }, \
|
|
|
- { "hvb_temp", NULL, MAVLINK_TYPE_INT8_T, 0, 14, offsetof(mavlink_shangfei_bms_t, hvb_temp) }, \
|
|
|
- { "hvb_status", NULL, MAVLINK_TYPE_UINT8_T, 0, 15, offsetof(mavlink_shangfei_bms_t, hvb_status) }, \
|
|
|
+ { "cell_max_volt", NULL, MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_shangfei_bms_t, cell_max_volt) }, \
|
|
|
+ { "cell_min_volt", NULL, MAVLINK_TYPE_UINT16_T, 0, 10, offsetof(mavlink_shangfei_bms_t, cell_min_volt) }, \
|
|
|
+ { "bat_maxtemp", NULL, MAVLINK_TYPE_INT8_T, 0, 16, offsetof(mavlink_shangfei_bms_t, bat_maxtemp) }, \
|
|
|
+ { "bat_mintemp", NULL, MAVLINK_TYPE_INT8_T, 0, 17, offsetof(mavlink_shangfei_bms_t, bat_mintemp) }, \
|
|
|
+ { "contactor_sta", NULL, MAVLINK_TYPE_UINT8_T, 0, 18, offsetof(mavlink_shangfei_bms_t, contactor_sta) }, \
|
|
|
+ { "soh_value", NULL, MAVLINK_TYPE_UINT8_T, 0, 19, offsetof(mavlink_shangfei_bms_t, soh_value) }, \
|
|
|
+ { "max_volt_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 20, offsetof(mavlink_shangfei_bms_t, max_volt_id) }, \
|
|
|
+ { "min_volt_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 21, offsetof(mavlink_shangfei_bms_t, min_volt_id) }, \
|
|
|
+ { "max_temp_id", NULL, MAVLINK_TYPE_INT8_T, 0, 22, offsetof(mavlink_shangfei_bms_t, max_temp_id) }, \
|
|
|
+ { "min_temp_id", NULL, MAVLINK_TYPE_INT8_T, 0, 23, offsetof(mavlink_shangfei_bms_t, min_temp_id) }, \
|
|
|
+ { "fault_level", NULL, MAVLINK_TYPE_UINT8_T, 0, 24, offsetof(mavlink_shangfei_bms_t, fault_level) }, \
|
|
|
+ { "irt_status", NULL, MAVLINK_TYPE_UINT8_T, 0, 25, offsetof(mavlink_shangfei_bms_t, irt_status) }, \
|
|
|
+ { "cap_percent", NULL, MAVLINK_TYPE_UINT8_T, 0, 26, offsetof(mavlink_shangfei_bms_t, cap_percent) }, \
|
|
|
+ { "bat_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 27, offsetof(mavlink_shangfei_bms_t, bat_id) }, \
|
|
|
+ { "err_code", NULL, MAVLINK_TYPE_UINT8_T, 0, 28, offsetof(mavlink_shangfei_bms_t, err_code) }, \
|
|
|
+ { "dc28_volt", NULL, MAVLINK_TYPE_UINT16_T, 0, 12, offsetof(mavlink_shangfei_bms_t, dc28_volt) }, \
|
|
|
+ { "dc28_current", NULL, MAVLINK_TYPE_UINT8_T, 0, 29, offsetof(mavlink_shangfei_bms_t, dc28_current) }, \
|
|
|
+ { "hvb_voltage", NULL, MAVLINK_TYPE_UINT16_T, 0, 14, offsetof(mavlink_shangfei_bms_t, hvb_voltage) }, \
|
|
|
+ { "hvb_temp", NULL, MAVLINK_TYPE_INT8_T, 0, 30, offsetof(mavlink_shangfei_bms_t, hvb_temp) }, \
|
|
|
+ { "hvb_status", NULL, MAVLINK_TYPE_UINT8_T, 0, 31, offsetof(mavlink_shangfei_bms_t, hvb_status) }, \
|
|
|
} \
|
|
|
}
|
|
|
#endif
|
|
|
@@ -71,30 +110,56 @@ typedef struct __mavlink_shangfei_bms_t {
|
|
|
* @param time_boot_ms [ms] Timestamp in ms from system boot.
|
|
|
* @param bat_voltage [dV] Battery voltage.
|
|
|
* @param bat_current [dA] Battery current.
|
|
|
+ * @param cell_max_volt [mV] Cell max voltage.
|
|
|
+ * @param cell_min_volt [mV] Cell min voltage.
|
|
|
* @param bat_maxtemp Battery Max_temperature.
|
|
|
+ * @param bat_mintemp Battery Min_temperature.
|
|
|
+ * @param contactor_sta Contactor status.
|
|
|
+ * @param soh_value [%] Battery SOH value.
|
|
|
+ * @param max_volt_id Cell max voltage id.
|
|
|
+ * @param min_volt_id Cell min voltage id.
|
|
|
+ * @param max_temp_id Cell max temperature id.
|
|
|
+ * @param min_temp_id Cell min temperature id.
|
|
|
+ * @param fault_level Fault level.
|
|
|
+ * @param irt_status Insulation Resistance Tester status.
|
|
|
* @param cap_percent [%] Battery capacity.
|
|
|
* @param bat_id Battery id.
|
|
|
* @param err_code Error code.
|
|
|
+ * @param dc28_volt [dV] High volt-box voltage.
|
|
|
+ * @param dc28_current [A] High volt-box current.
|
|
|
* @param hvb_voltage [dV] High volt-box voltage.
|
|
|
* @param hvb_temp High volt-box temperature.
|
|
|
* @param hvb_status High volt-box status.
|
|
|
* @return length of the message in bytes (excluding serial stream start sign)
|
|
|
*/
|
|
|
static inline uint16_t mavlink_msg_shangfei_bms_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
|
|
|
- uint32_t time_boot_ms, uint16_t bat_voltage, int16_t bat_current, int8_t bat_maxtemp, uint8_t cap_percent, uint8_t bat_id, uint8_t err_code, uint16_t hvb_voltage, int8_t hvb_temp, uint8_t hvb_status)
|
|
|
+ uint32_t time_boot_ms, uint16_t bat_voltage, int16_t bat_current, uint16_t cell_max_volt, uint16_t cell_min_volt, int8_t bat_maxtemp, int8_t bat_mintemp, uint8_t contactor_sta, uint8_t soh_value, uint8_t max_volt_id, uint8_t min_volt_id, int8_t max_temp_id, int8_t min_temp_id, uint8_t fault_level, uint8_t irt_status, uint8_t cap_percent, uint8_t bat_id, uint8_t err_code, uint16_t dc28_volt, uint8_t dc28_current, uint16_t hvb_voltage, int8_t hvb_temp, uint8_t hvb_status)
|
|
|
{
|
|
|
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
|
|
|
char buf[MAVLINK_MSG_ID_SHANGFEI_BMS_LEN];
|
|
|
_mav_put_uint32_t(buf, 0, time_boot_ms);
|
|
|
_mav_put_uint16_t(buf, 4, bat_voltage);
|
|
|
_mav_put_int16_t(buf, 6, bat_current);
|
|
|
- _mav_put_uint16_t(buf, 8, hvb_voltage);
|
|
|
- _mav_put_int8_t(buf, 10, bat_maxtemp);
|
|
|
- _mav_put_uint8_t(buf, 11, cap_percent);
|
|
|
- _mav_put_uint8_t(buf, 12, bat_id);
|
|
|
- _mav_put_uint8_t(buf, 13, err_code);
|
|
|
- _mav_put_int8_t(buf, 14, hvb_temp);
|
|
|
- _mav_put_uint8_t(buf, 15, hvb_status);
|
|
|
+ _mav_put_uint16_t(buf, 8, cell_max_volt);
|
|
|
+ _mav_put_uint16_t(buf, 10, cell_min_volt);
|
|
|
+ _mav_put_uint16_t(buf, 12, dc28_volt);
|
|
|
+ _mav_put_uint16_t(buf, 14, hvb_voltage);
|
|
|
+ _mav_put_int8_t(buf, 16, bat_maxtemp);
|
|
|
+ _mav_put_int8_t(buf, 17, bat_mintemp);
|
|
|
+ _mav_put_uint8_t(buf, 18, contactor_sta);
|
|
|
+ _mav_put_uint8_t(buf, 19, soh_value);
|
|
|
+ _mav_put_uint8_t(buf, 20, max_volt_id);
|
|
|
+ _mav_put_uint8_t(buf, 21, min_volt_id);
|
|
|
+ _mav_put_int8_t(buf, 22, max_temp_id);
|
|
|
+ _mav_put_int8_t(buf, 23, min_temp_id);
|
|
|
+ _mav_put_uint8_t(buf, 24, fault_level);
|
|
|
+ _mav_put_uint8_t(buf, 25, irt_status);
|
|
|
+ _mav_put_uint8_t(buf, 26, cap_percent);
|
|
|
+ _mav_put_uint8_t(buf, 27, bat_id);
|
|
|
+ _mav_put_uint8_t(buf, 28, err_code);
|
|
|
+ _mav_put_uint8_t(buf, 29, dc28_current);
|
|
|
+ _mav_put_int8_t(buf, 30, hvb_temp);
|
|
|
+ _mav_put_uint8_t(buf, 31, hvb_status);
|
|
|
|
|
|
memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SHANGFEI_BMS_LEN);
|
|
|
#else
|
|
|
@@ -102,11 +167,24 @@ static inline uint16_t mavlink_msg_shangfei_bms_pack(uint8_t system_id, uint8_t
|
|
|
packet.time_boot_ms = time_boot_ms;
|
|
|
packet.bat_voltage = bat_voltage;
|
|
|
packet.bat_current = bat_current;
|
|
|
+ packet.cell_max_volt = cell_max_volt;
|
|
|
+ packet.cell_min_volt = cell_min_volt;
|
|
|
+ packet.dc28_volt = dc28_volt;
|
|
|
packet.hvb_voltage = hvb_voltage;
|
|
|
packet.bat_maxtemp = bat_maxtemp;
|
|
|
+ packet.bat_mintemp = bat_mintemp;
|
|
|
+ packet.contactor_sta = contactor_sta;
|
|
|
+ packet.soh_value = soh_value;
|
|
|
+ packet.max_volt_id = max_volt_id;
|
|
|
+ packet.min_volt_id = min_volt_id;
|
|
|
+ packet.max_temp_id = max_temp_id;
|
|
|
+ packet.min_temp_id = min_temp_id;
|
|
|
+ packet.fault_level = fault_level;
|
|
|
+ packet.irt_status = irt_status;
|
|
|
packet.cap_percent = cap_percent;
|
|
|
packet.bat_id = bat_id;
|
|
|
packet.err_code = err_code;
|
|
|
+ packet.dc28_current = dc28_current;
|
|
|
packet.hvb_temp = hvb_temp;
|
|
|
packet.hvb_status = hvb_status;
|
|
|
|
|
|
@@ -127,30 +205,56 @@ static inline uint16_t mavlink_msg_shangfei_bms_pack(uint8_t system_id, uint8_t
|
|
|
* @param time_boot_ms [ms] Timestamp in ms from system boot.
|
|
|
* @param bat_voltage [dV] Battery voltage.
|
|
|
* @param bat_current [dA] Battery current.
|
|
|
+ * @param cell_max_volt [mV] Cell max voltage.
|
|
|
+ * @param cell_min_volt [mV] Cell min voltage.
|
|
|
* @param bat_maxtemp Battery Max_temperature.
|
|
|
+ * @param bat_mintemp Battery Min_temperature.
|
|
|
+ * @param contactor_sta Contactor status.
|
|
|
+ * @param soh_value [%] Battery SOH value.
|
|
|
+ * @param max_volt_id Cell max voltage id.
|
|
|
+ * @param min_volt_id Cell min voltage id.
|
|
|
+ * @param max_temp_id Cell max temperature id.
|
|
|
+ * @param min_temp_id Cell min temperature id.
|
|
|
+ * @param fault_level Fault level.
|
|
|
+ * @param irt_status Insulation Resistance Tester status.
|
|
|
* @param cap_percent [%] Battery capacity.
|
|
|
* @param bat_id Battery id.
|
|
|
* @param err_code Error code.
|
|
|
+ * @param dc28_volt [dV] High volt-box voltage.
|
|
|
+ * @param dc28_current [A] High volt-box current.
|
|
|
* @param hvb_voltage [dV] High volt-box voltage.
|
|
|
* @param hvb_temp High volt-box temperature.
|
|
|
* @param hvb_status High volt-box status.
|
|
|
* @return length of the message in bytes (excluding serial stream start sign)
|
|
|
*/
|
|
|
static inline uint16_t mavlink_msg_shangfei_bms_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg,
|
|
|
- uint32_t time_boot_ms, uint16_t bat_voltage, int16_t bat_current, int8_t bat_maxtemp, uint8_t cap_percent, uint8_t bat_id, uint8_t err_code, uint16_t hvb_voltage, int8_t hvb_temp, uint8_t hvb_status)
|
|
|
+ uint32_t time_boot_ms, uint16_t bat_voltage, int16_t bat_current, uint16_t cell_max_volt, uint16_t cell_min_volt, int8_t bat_maxtemp, int8_t bat_mintemp, uint8_t contactor_sta, uint8_t soh_value, uint8_t max_volt_id, uint8_t min_volt_id, int8_t max_temp_id, int8_t min_temp_id, uint8_t fault_level, uint8_t irt_status, uint8_t cap_percent, uint8_t bat_id, uint8_t err_code, uint16_t dc28_volt, uint8_t dc28_current, uint16_t hvb_voltage, int8_t hvb_temp, uint8_t hvb_status)
|
|
|
{
|
|
|
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
|
|
|
char buf[MAVLINK_MSG_ID_SHANGFEI_BMS_LEN];
|
|
|
_mav_put_uint32_t(buf, 0, time_boot_ms);
|
|
|
_mav_put_uint16_t(buf, 4, bat_voltage);
|
|
|
_mav_put_int16_t(buf, 6, bat_current);
|
|
|
- _mav_put_uint16_t(buf, 8, hvb_voltage);
|
|
|
- _mav_put_int8_t(buf, 10, bat_maxtemp);
|
|
|
- _mav_put_uint8_t(buf, 11, cap_percent);
|
|
|
- _mav_put_uint8_t(buf, 12, bat_id);
|
|
|
- _mav_put_uint8_t(buf, 13, err_code);
|
|
|
- _mav_put_int8_t(buf, 14, hvb_temp);
|
|
|
- _mav_put_uint8_t(buf, 15, hvb_status);
|
|
|
+ _mav_put_uint16_t(buf, 8, cell_max_volt);
|
|
|
+ _mav_put_uint16_t(buf, 10, cell_min_volt);
|
|
|
+ _mav_put_uint16_t(buf, 12, dc28_volt);
|
|
|
+ _mav_put_uint16_t(buf, 14, hvb_voltage);
|
|
|
+ _mav_put_int8_t(buf, 16, bat_maxtemp);
|
|
|
+ _mav_put_int8_t(buf, 17, bat_mintemp);
|
|
|
+ _mav_put_uint8_t(buf, 18, contactor_sta);
|
|
|
+ _mav_put_uint8_t(buf, 19, soh_value);
|
|
|
+ _mav_put_uint8_t(buf, 20, max_volt_id);
|
|
|
+ _mav_put_uint8_t(buf, 21, min_volt_id);
|
|
|
+ _mav_put_int8_t(buf, 22, max_temp_id);
|
|
|
+ _mav_put_int8_t(buf, 23, min_temp_id);
|
|
|
+ _mav_put_uint8_t(buf, 24, fault_level);
|
|
|
+ _mav_put_uint8_t(buf, 25, irt_status);
|
|
|
+ _mav_put_uint8_t(buf, 26, cap_percent);
|
|
|
+ _mav_put_uint8_t(buf, 27, bat_id);
|
|
|
+ _mav_put_uint8_t(buf, 28, err_code);
|
|
|
+ _mav_put_uint8_t(buf, 29, dc28_current);
|
|
|
+ _mav_put_int8_t(buf, 30, hvb_temp);
|
|
|
+ _mav_put_uint8_t(buf, 31, hvb_status);
|
|
|
|
|
|
memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SHANGFEI_BMS_LEN);
|
|
|
#else
|
|
|
@@ -158,11 +262,24 @@ static inline uint16_t mavlink_msg_shangfei_bms_pack_status(uint8_t system_id, u
|
|
|
packet.time_boot_ms = time_boot_ms;
|
|
|
packet.bat_voltage = bat_voltage;
|
|
|
packet.bat_current = bat_current;
|
|
|
+ packet.cell_max_volt = cell_max_volt;
|
|
|
+ packet.cell_min_volt = cell_min_volt;
|
|
|
+ packet.dc28_volt = dc28_volt;
|
|
|
packet.hvb_voltage = hvb_voltage;
|
|
|
packet.bat_maxtemp = bat_maxtemp;
|
|
|
+ packet.bat_mintemp = bat_mintemp;
|
|
|
+ packet.contactor_sta = contactor_sta;
|
|
|
+ packet.soh_value = soh_value;
|
|
|
+ packet.max_volt_id = max_volt_id;
|
|
|
+ packet.min_volt_id = min_volt_id;
|
|
|
+ packet.max_temp_id = max_temp_id;
|
|
|
+ packet.min_temp_id = min_temp_id;
|
|
|
+ packet.fault_level = fault_level;
|
|
|
+ packet.irt_status = irt_status;
|
|
|
packet.cap_percent = cap_percent;
|
|
|
packet.bat_id = bat_id;
|
|
|
packet.err_code = err_code;
|
|
|
+ packet.dc28_current = dc28_current;
|
|
|
packet.hvb_temp = hvb_temp;
|
|
|
packet.hvb_status = hvb_status;
|
|
|
|
|
|
@@ -186,10 +303,23 @@ static inline uint16_t mavlink_msg_shangfei_bms_pack_status(uint8_t system_id, u
|
|
|
* @param time_boot_ms [ms] Timestamp in ms from system boot.
|
|
|
* @param bat_voltage [dV] Battery voltage.
|
|
|
* @param bat_current [dA] Battery current.
|
|
|
+ * @param cell_max_volt [mV] Cell max voltage.
|
|
|
+ * @param cell_min_volt [mV] Cell min voltage.
|
|
|
* @param bat_maxtemp Battery Max_temperature.
|
|
|
+ * @param bat_mintemp Battery Min_temperature.
|
|
|
+ * @param contactor_sta Contactor status.
|
|
|
+ * @param soh_value [%] Battery SOH value.
|
|
|
+ * @param max_volt_id Cell max voltage id.
|
|
|
+ * @param min_volt_id Cell min voltage id.
|
|
|
+ * @param max_temp_id Cell max temperature id.
|
|
|
+ * @param min_temp_id Cell min temperature id.
|
|
|
+ * @param fault_level Fault level.
|
|
|
+ * @param irt_status Insulation Resistance Tester status.
|
|
|
* @param cap_percent [%] Battery capacity.
|
|
|
* @param bat_id Battery id.
|
|
|
* @param err_code Error code.
|
|
|
+ * @param dc28_volt [dV] High volt-box voltage.
|
|
|
+ * @param dc28_current [A] High volt-box current.
|
|
|
* @param hvb_voltage [dV] High volt-box voltage.
|
|
|
* @param hvb_temp High volt-box temperature.
|
|
|
* @param hvb_status High volt-box status.
|
|
|
@@ -197,20 +327,33 @@ static inline uint16_t mavlink_msg_shangfei_bms_pack_status(uint8_t system_id, u
|
|
|
*/
|
|
|
static inline uint16_t mavlink_msg_shangfei_bms_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
|
|
|
mavlink_message_t* msg,
|
|
|
- uint32_t time_boot_ms,uint16_t bat_voltage,int16_t bat_current,int8_t bat_maxtemp,uint8_t cap_percent,uint8_t bat_id,uint8_t err_code,uint16_t hvb_voltage,int8_t hvb_temp,uint8_t hvb_status)
|
|
|
+ uint32_t time_boot_ms,uint16_t bat_voltage,int16_t bat_current,uint16_t cell_max_volt,uint16_t cell_min_volt,int8_t bat_maxtemp,int8_t bat_mintemp,uint8_t contactor_sta,uint8_t soh_value,uint8_t max_volt_id,uint8_t min_volt_id,int8_t max_temp_id,int8_t min_temp_id,uint8_t fault_level,uint8_t irt_status,uint8_t cap_percent,uint8_t bat_id,uint8_t err_code,uint16_t dc28_volt,uint8_t dc28_current,uint16_t hvb_voltage,int8_t hvb_temp,uint8_t hvb_status)
|
|
|
{
|
|
|
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
|
|
|
char buf[MAVLINK_MSG_ID_SHANGFEI_BMS_LEN];
|
|
|
_mav_put_uint32_t(buf, 0, time_boot_ms);
|
|
|
_mav_put_uint16_t(buf, 4, bat_voltage);
|
|
|
_mav_put_int16_t(buf, 6, bat_current);
|
|
|
- _mav_put_uint16_t(buf, 8, hvb_voltage);
|
|
|
- _mav_put_int8_t(buf, 10, bat_maxtemp);
|
|
|
- _mav_put_uint8_t(buf, 11, cap_percent);
|
|
|
- _mav_put_uint8_t(buf, 12, bat_id);
|
|
|
- _mav_put_uint8_t(buf, 13, err_code);
|
|
|
- _mav_put_int8_t(buf, 14, hvb_temp);
|
|
|
- _mav_put_uint8_t(buf, 15, hvb_status);
|
|
|
+ _mav_put_uint16_t(buf, 8, cell_max_volt);
|
|
|
+ _mav_put_uint16_t(buf, 10, cell_min_volt);
|
|
|
+ _mav_put_uint16_t(buf, 12, dc28_volt);
|
|
|
+ _mav_put_uint16_t(buf, 14, hvb_voltage);
|
|
|
+ _mav_put_int8_t(buf, 16, bat_maxtemp);
|
|
|
+ _mav_put_int8_t(buf, 17, bat_mintemp);
|
|
|
+ _mav_put_uint8_t(buf, 18, contactor_sta);
|
|
|
+ _mav_put_uint8_t(buf, 19, soh_value);
|
|
|
+ _mav_put_uint8_t(buf, 20, max_volt_id);
|
|
|
+ _mav_put_uint8_t(buf, 21, min_volt_id);
|
|
|
+ _mav_put_int8_t(buf, 22, max_temp_id);
|
|
|
+ _mav_put_int8_t(buf, 23, min_temp_id);
|
|
|
+ _mav_put_uint8_t(buf, 24, fault_level);
|
|
|
+ _mav_put_uint8_t(buf, 25, irt_status);
|
|
|
+ _mav_put_uint8_t(buf, 26, cap_percent);
|
|
|
+ _mav_put_uint8_t(buf, 27, bat_id);
|
|
|
+ _mav_put_uint8_t(buf, 28, err_code);
|
|
|
+ _mav_put_uint8_t(buf, 29, dc28_current);
|
|
|
+ _mav_put_int8_t(buf, 30, hvb_temp);
|
|
|
+ _mav_put_uint8_t(buf, 31, hvb_status);
|
|
|
|
|
|
memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SHANGFEI_BMS_LEN);
|
|
|
#else
|
|
|
@@ -218,11 +361,24 @@ static inline uint16_t mavlink_msg_shangfei_bms_pack_chan(uint8_t system_id, uin
|
|
|
packet.time_boot_ms = time_boot_ms;
|
|
|
packet.bat_voltage = bat_voltage;
|
|
|
packet.bat_current = bat_current;
|
|
|
+ packet.cell_max_volt = cell_max_volt;
|
|
|
+ packet.cell_min_volt = cell_min_volt;
|
|
|
+ packet.dc28_volt = dc28_volt;
|
|
|
packet.hvb_voltage = hvb_voltage;
|
|
|
packet.bat_maxtemp = bat_maxtemp;
|
|
|
+ packet.bat_mintemp = bat_mintemp;
|
|
|
+ packet.contactor_sta = contactor_sta;
|
|
|
+ packet.soh_value = soh_value;
|
|
|
+ packet.max_volt_id = max_volt_id;
|
|
|
+ packet.min_volt_id = min_volt_id;
|
|
|
+ packet.max_temp_id = max_temp_id;
|
|
|
+ packet.min_temp_id = min_temp_id;
|
|
|
+ packet.fault_level = fault_level;
|
|
|
+ packet.irt_status = irt_status;
|
|
|
packet.cap_percent = cap_percent;
|
|
|
packet.bat_id = bat_id;
|
|
|
packet.err_code = err_code;
|
|
|
+ packet.dc28_current = dc28_current;
|
|
|
packet.hvb_temp = hvb_temp;
|
|
|
packet.hvb_status = hvb_status;
|
|
|
|
|
|
@@ -243,7 +399,7 @@ static inline uint16_t mavlink_msg_shangfei_bms_pack_chan(uint8_t system_id, uin
|
|
|
*/
|
|
|
static inline uint16_t mavlink_msg_shangfei_bms_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_shangfei_bms_t* shangfei_bms)
|
|
|
{
|
|
|
- return mavlink_msg_shangfei_bms_pack(system_id, component_id, msg, shangfei_bms->time_boot_ms, shangfei_bms->bat_voltage, shangfei_bms->bat_current, shangfei_bms->bat_maxtemp, shangfei_bms->cap_percent, shangfei_bms->bat_id, shangfei_bms->err_code, shangfei_bms->hvb_voltage, shangfei_bms->hvb_temp, shangfei_bms->hvb_status);
|
|
|
+ return mavlink_msg_shangfei_bms_pack(system_id, component_id, msg, shangfei_bms->time_boot_ms, shangfei_bms->bat_voltage, shangfei_bms->bat_current, shangfei_bms->cell_max_volt, shangfei_bms->cell_min_volt, shangfei_bms->bat_maxtemp, shangfei_bms->bat_mintemp, shangfei_bms->contactor_sta, shangfei_bms->soh_value, shangfei_bms->max_volt_id, shangfei_bms->min_volt_id, shangfei_bms->max_temp_id, shangfei_bms->min_temp_id, shangfei_bms->fault_level, shangfei_bms->irt_status, shangfei_bms->cap_percent, shangfei_bms->bat_id, shangfei_bms->err_code, shangfei_bms->dc28_volt, shangfei_bms->dc28_current, shangfei_bms->hvb_voltage, shangfei_bms->hvb_temp, shangfei_bms->hvb_status);
|
|
|
}
|
|
|
|
|
|
/**
|
|
|
@@ -257,7 +413,7 @@ static inline uint16_t mavlink_msg_shangfei_bms_encode(uint8_t system_id, uint8_
|
|
|
*/
|
|
|
static inline uint16_t mavlink_msg_shangfei_bms_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_shangfei_bms_t* shangfei_bms)
|
|
|
{
|
|
|
- return mavlink_msg_shangfei_bms_pack_chan(system_id, component_id, chan, msg, shangfei_bms->time_boot_ms, shangfei_bms->bat_voltage, shangfei_bms->bat_current, shangfei_bms->bat_maxtemp, shangfei_bms->cap_percent, shangfei_bms->bat_id, shangfei_bms->err_code, shangfei_bms->hvb_voltage, shangfei_bms->hvb_temp, shangfei_bms->hvb_status);
|
|
|
+ return mavlink_msg_shangfei_bms_pack_chan(system_id, component_id, chan, msg, shangfei_bms->time_boot_ms, shangfei_bms->bat_voltage, shangfei_bms->bat_current, shangfei_bms->cell_max_volt, shangfei_bms->cell_min_volt, shangfei_bms->bat_maxtemp, shangfei_bms->bat_mintemp, shangfei_bms->contactor_sta, shangfei_bms->soh_value, shangfei_bms->max_volt_id, shangfei_bms->min_volt_id, shangfei_bms->max_temp_id, shangfei_bms->min_temp_id, shangfei_bms->fault_level, shangfei_bms->irt_status, shangfei_bms->cap_percent, shangfei_bms->bat_id, shangfei_bms->err_code, shangfei_bms->dc28_volt, shangfei_bms->dc28_current, shangfei_bms->hvb_voltage, shangfei_bms->hvb_temp, shangfei_bms->hvb_status);
|
|
|
}
|
|
|
|
|
|
/**
|
|
|
@@ -271,7 +427,7 @@ static inline uint16_t mavlink_msg_shangfei_bms_encode_chan(uint8_t system_id, u
|
|
|
*/
|
|
|
static inline uint16_t mavlink_msg_shangfei_bms_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_shangfei_bms_t* shangfei_bms)
|
|
|
{
|
|
|
- return mavlink_msg_shangfei_bms_pack_status(system_id, component_id, _status, msg, shangfei_bms->time_boot_ms, shangfei_bms->bat_voltage, shangfei_bms->bat_current, shangfei_bms->bat_maxtemp, shangfei_bms->cap_percent, shangfei_bms->bat_id, shangfei_bms->err_code, shangfei_bms->hvb_voltage, shangfei_bms->hvb_temp, shangfei_bms->hvb_status);
|
|
|
+ return mavlink_msg_shangfei_bms_pack_status(system_id, component_id, _status, msg, shangfei_bms->time_boot_ms, shangfei_bms->bat_voltage, shangfei_bms->bat_current, shangfei_bms->cell_max_volt, shangfei_bms->cell_min_volt, shangfei_bms->bat_maxtemp, shangfei_bms->bat_mintemp, shangfei_bms->contactor_sta, shangfei_bms->soh_value, shangfei_bms->max_volt_id, shangfei_bms->min_volt_id, shangfei_bms->max_temp_id, shangfei_bms->min_temp_id, shangfei_bms->fault_level, shangfei_bms->irt_status, shangfei_bms->cap_percent, shangfei_bms->bat_id, shangfei_bms->err_code, shangfei_bms->dc28_volt, shangfei_bms->dc28_current, shangfei_bms->hvb_voltage, shangfei_bms->hvb_temp, shangfei_bms->hvb_status);
|
|
|
}
|
|
|
|
|
|
/**
|
|
|
@@ -281,30 +437,56 @@ static inline uint16_t mavlink_msg_shangfei_bms_encode_status(uint8_t system_id,
|
|
|
* @param time_boot_ms [ms] Timestamp in ms from system boot.
|
|
|
* @param bat_voltage [dV] Battery voltage.
|
|
|
* @param bat_current [dA] Battery current.
|
|
|
+ * @param cell_max_volt [mV] Cell max voltage.
|
|
|
+ * @param cell_min_volt [mV] Cell min voltage.
|
|
|
* @param bat_maxtemp Battery Max_temperature.
|
|
|
+ * @param bat_mintemp Battery Min_temperature.
|
|
|
+ * @param contactor_sta Contactor status.
|
|
|
+ * @param soh_value [%] Battery SOH value.
|
|
|
+ * @param max_volt_id Cell max voltage id.
|
|
|
+ * @param min_volt_id Cell min voltage id.
|
|
|
+ * @param max_temp_id Cell max temperature id.
|
|
|
+ * @param min_temp_id Cell min temperature id.
|
|
|
+ * @param fault_level Fault level.
|
|
|
+ * @param irt_status Insulation Resistance Tester status.
|
|
|
* @param cap_percent [%] Battery capacity.
|
|
|
* @param bat_id Battery id.
|
|
|
* @param err_code Error code.
|
|
|
+ * @param dc28_volt [dV] High volt-box voltage.
|
|
|
+ * @param dc28_current [A] High volt-box current.
|
|
|
* @param hvb_voltage [dV] High volt-box voltage.
|
|
|
* @param hvb_temp High volt-box temperature.
|
|
|
* @param hvb_status High volt-box status.
|
|
|
*/
|
|
|
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
|
|
|
|
|
|
-static inline void mavlink_msg_shangfei_bms_send(mavlink_channel_t chan, uint32_t time_boot_ms, uint16_t bat_voltage, int16_t bat_current, int8_t bat_maxtemp, uint8_t cap_percent, uint8_t bat_id, uint8_t err_code, uint16_t hvb_voltage, int8_t hvb_temp, uint8_t hvb_status)
|
|
|
+static inline void mavlink_msg_shangfei_bms_send(mavlink_channel_t chan, uint32_t time_boot_ms, uint16_t bat_voltage, int16_t bat_current, uint16_t cell_max_volt, uint16_t cell_min_volt, int8_t bat_maxtemp, int8_t bat_mintemp, uint8_t contactor_sta, uint8_t soh_value, uint8_t max_volt_id, uint8_t min_volt_id, int8_t max_temp_id, int8_t min_temp_id, uint8_t fault_level, uint8_t irt_status, uint8_t cap_percent, uint8_t bat_id, uint8_t err_code, uint16_t dc28_volt, uint8_t dc28_current, uint16_t hvb_voltage, int8_t hvb_temp, uint8_t hvb_status)
|
|
|
{
|
|
|
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
|
|
|
char buf[MAVLINK_MSG_ID_SHANGFEI_BMS_LEN];
|
|
|
_mav_put_uint32_t(buf, 0, time_boot_ms);
|
|
|
_mav_put_uint16_t(buf, 4, bat_voltage);
|
|
|
_mav_put_int16_t(buf, 6, bat_current);
|
|
|
- _mav_put_uint16_t(buf, 8, hvb_voltage);
|
|
|
- _mav_put_int8_t(buf, 10, bat_maxtemp);
|
|
|
- _mav_put_uint8_t(buf, 11, cap_percent);
|
|
|
- _mav_put_uint8_t(buf, 12, bat_id);
|
|
|
- _mav_put_uint8_t(buf, 13, err_code);
|
|
|
- _mav_put_int8_t(buf, 14, hvb_temp);
|
|
|
- _mav_put_uint8_t(buf, 15, hvb_status);
|
|
|
+ _mav_put_uint16_t(buf, 8, cell_max_volt);
|
|
|
+ _mav_put_uint16_t(buf, 10, cell_min_volt);
|
|
|
+ _mav_put_uint16_t(buf, 12, dc28_volt);
|
|
|
+ _mav_put_uint16_t(buf, 14, hvb_voltage);
|
|
|
+ _mav_put_int8_t(buf, 16, bat_maxtemp);
|
|
|
+ _mav_put_int8_t(buf, 17, bat_mintemp);
|
|
|
+ _mav_put_uint8_t(buf, 18, contactor_sta);
|
|
|
+ _mav_put_uint8_t(buf, 19, soh_value);
|
|
|
+ _mav_put_uint8_t(buf, 20, max_volt_id);
|
|
|
+ _mav_put_uint8_t(buf, 21, min_volt_id);
|
|
|
+ _mav_put_int8_t(buf, 22, max_temp_id);
|
|
|
+ _mav_put_int8_t(buf, 23, min_temp_id);
|
|
|
+ _mav_put_uint8_t(buf, 24, fault_level);
|
|
|
+ _mav_put_uint8_t(buf, 25, irt_status);
|
|
|
+ _mav_put_uint8_t(buf, 26, cap_percent);
|
|
|
+ _mav_put_uint8_t(buf, 27, bat_id);
|
|
|
+ _mav_put_uint8_t(buf, 28, err_code);
|
|
|
+ _mav_put_uint8_t(buf, 29, dc28_current);
|
|
|
+ _mav_put_int8_t(buf, 30, hvb_temp);
|
|
|
+ _mav_put_uint8_t(buf, 31, hvb_status);
|
|
|
|
|
|
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SHANGFEI_BMS, buf, MAVLINK_MSG_ID_SHANGFEI_BMS_MIN_LEN, MAVLINK_MSG_ID_SHANGFEI_BMS_LEN, MAVLINK_MSG_ID_SHANGFEI_BMS_CRC);
|
|
|
#else
|
|
|
@@ -312,11 +494,24 @@ static inline void mavlink_msg_shangfei_bms_send(mavlink_channel_t chan, uint32_
|
|
|
packet.time_boot_ms = time_boot_ms;
|
|
|
packet.bat_voltage = bat_voltage;
|
|
|
packet.bat_current = bat_current;
|
|
|
+ packet.cell_max_volt = cell_max_volt;
|
|
|
+ packet.cell_min_volt = cell_min_volt;
|
|
|
+ packet.dc28_volt = dc28_volt;
|
|
|
packet.hvb_voltage = hvb_voltage;
|
|
|
packet.bat_maxtemp = bat_maxtemp;
|
|
|
+ packet.bat_mintemp = bat_mintemp;
|
|
|
+ packet.contactor_sta = contactor_sta;
|
|
|
+ packet.soh_value = soh_value;
|
|
|
+ packet.max_volt_id = max_volt_id;
|
|
|
+ packet.min_volt_id = min_volt_id;
|
|
|
+ packet.max_temp_id = max_temp_id;
|
|
|
+ packet.min_temp_id = min_temp_id;
|
|
|
+ packet.fault_level = fault_level;
|
|
|
+ packet.irt_status = irt_status;
|
|
|
packet.cap_percent = cap_percent;
|
|
|
packet.bat_id = bat_id;
|
|
|
packet.err_code = err_code;
|
|
|
+ packet.dc28_current = dc28_current;
|
|
|
packet.hvb_temp = hvb_temp;
|
|
|
packet.hvb_status = hvb_status;
|
|
|
|
|
|
@@ -332,7 +527,7 @@ static inline void mavlink_msg_shangfei_bms_send(mavlink_channel_t chan, uint32_
|
|
|
static inline void mavlink_msg_shangfei_bms_send_struct(mavlink_channel_t chan, const mavlink_shangfei_bms_t* shangfei_bms)
|
|
|
{
|
|
|
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
|
|
|
- mavlink_msg_shangfei_bms_send(chan, shangfei_bms->time_boot_ms, shangfei_bms->bat_voltage, shangfei_bms->bat_current, shangfei_bms->bat_maxtemp, shangfei_bms->cap_percent, shangfei_bms->bat_id, shangfei_bms->err_code, shangfei_bms->hvb_voltage, shangfei_bms->hvb_temp, shangfei_bms->hvb_status);
|
|
|
+ mavlink_msg_shangfei_bms_send(chan, shangfei_bms->time_boot_ms, shangfei_bms->bat_voltage, shangfei_bms->bat_current, shangfei_bms->cell_max_volt, shangfei_bms->cell_min_volt, shangfei_bms->bat_maxtemp, shangfei_bms->bat_mintemp, shangfei_bms->contactor_sta, shangfei_bms->soh_value, shangfei_bms->max_volt_id, shangfei_bms->min_volt_id, shangfei_bms->max_temp_id, shangfei_bms->min_temp_id, shangfei_bms->fault_level, shangfei_bms->irt_status, shangfei_bms->cap_percent, shangfei_bms->bat_id, shangfei_bms->err_code, shangfei_bms->dc28_volt, shangfei_bms->dc28_current, shangfei_bms->hvb_voltage, shangfei_bms->hvb_temp, shangfei_bms->hvb_status);
|
|
|
#else
|
|
|
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SHANGFEI_BMS, (const char *)shangfei_bms, MAVLINK_MSG_ID_SHANGFEI_BMS_MIN_LEN, MAVLINK_MSG_ID_SHANGFEI_BMS_LEN, MAVLINK_MSG_ID_SHANGFEI_BMS_CRC);
|
|
|
#endif
|
|
|
@@ -346,20 +541,33 @@ static inline void mavlink_msg_shangfei_bms_send_struct(mavlink_channel_t chan,
|
|
|
is usually the receive buffer for the channel, and allows a reply to an
|
|
|
incoming message with minimum stack space usage.
|
|
|
*/
|
|
|
-static inline void mavlink_msg_shangfei_bms_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, uint16_t bat_voltage, int16_t bat_current, int8_t bat_maxtemp, uint8_t cap_percent, uint8_t bat_id, uint8_t err_code, uint16_t hvb_voltage, int8_t hvb_temp, uint8_t hvb_status)
|
|
|
+static inline void mavlink_msg_shangfei_bms_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, uint16_t bat_voltage, int16_t bat_current, uint16_t cell_max_volt, uint16_t cell_min_volt, int8_t bat_maxtemp, int8_t bat_mintemp, uint8_t contactor_sta, uint8_t soh_value, uint8_t max_volt_id, uint8_t min_volt_id, int8_t max_temp_id, int8_t min_temp_id, uint8_t fault_level, uint8_t irt_status, uint8_t cap_percent, uint8_t bat_id, uint8_t err_code, uint16_t dc28_volt, uint8_t dc28_current, uint16_t hvb_voltage, int8_t hvb_temp, uint8_t hvb_status)
|
|
|
{
|
|
|
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
|
|
|
char *buf = (char *)msgbuf;
|
|
|
_mav_put_uint32_t(buf, 0, time_boot_ms);
|
|
|
_mav_put_uint16_t(buf, 4, bat_voltage);
|
|
|
_mav_put_int16_t(buf, 6, bat_current);
|
|
|
- _mav_put_uint16_t(buf, 8, hvb_voltage);
|
|
|
- _mav_put_int8_t(buf, 10, bat_maxtemp);
|
|
|
- _mav_put_uint8_t(buf, 11, cap_percent);
|
|
|
- _mav_put_uint8_t(buf, 12, bat_id);
|
|
|
- _mav_put_uint8_t(buf, 13, err_code);
|
|
|
- _mav_put_int8_t(buf, 14, hvb_temp);
|
|
|
- _mav_put_uint8_t(buf, 15, hvb_status);
|
|
|
+ _mav_put_uint16_t(buf, 8, cell_max_volt);
|
|
|
+ _mav_put_uint16_t(buf, 10, cell_min_volt);
|
|
|
+ _mav_put_uint16_t(buf, 12, dc28_volt);
|
|
|
+ _mav_put_uint16_t(buf, 14, hvb_voltage);
|
|
|
+ _mav_put_int8_t(buf, 16, bat_maxtemp);
|
|
|
+ _mav_put_int8_t(buf, 17, bat_mintemp);
|
|
|
+ _mav_put_uint8_t(buf, 18, contactor_sta);
|
|
|
+ _mav_put_uint8_t(buf, 19, soh_value);
|
|
|
+ _mav_put_uint8_t(buf, 20, max_volt_id);
|
|
|
+ _mav_put_uint8_t(buf, 21, min_volt_id);
|
|
|
+ _mav_put_int8_t(buf, 22, max_temp_id);
|
|
|
+ _mav_put_int8_t(buf, 23, min_temp_id);
|
|
|
+ _mav_put_uint8_t(buf, 24, fault_level);
|
|
|
+ _mav_put_uint8_t(buf, 25, irt_status);
|
|
|
+ _mav_put_uint8_t(buf, 26, cap_percent);
|
|
|
+ _mav_put_uint8_t(buf, 27, bat_id);
|
|
|
+ _mav_put_uint8_t(buf, 28, err_code);
|
|
|
+ _mav_put_uint8_t(buf, 29, dc28_current);
|
|
|
+ _mav_put_int8_t(buf, 30, hvb_temp);
|
|
|
+ _mav_put_uint8_t(buf, 31, hvb_status);
|
|
|
|
|
|
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SHANGFEI_BMS, buf, MAVLINK_MSG_ID_SHANGFEI_BMS_MIN_LEN, MAVLINK_MSG_ID_SHANGFEI_BMS_LEN, MAVLINK_MSG_ID_SHANGFEI_BMS_CRC);
|
|
|
#else
|
|
|
@@ -367,11 +575,24 @@ static inline void mavlink_msg_shangfei_bms_send_buf(mavlink_message_t *msgbuf,
|
|
|
packet->time_boot_ms = time_boot_ms;
|
|
|
packet->bat_voltage = bat_voltage;
|
|
|
packet->bat_current = bat_current;
|
|
|
+ packet->cell_max_volt = cell_max_volt;
|
|
|
+ packet->cell_min_volt = cell_min_volt;
|
|
|
+ packet->dc28_volt = dc28_volt;
|
|
|
packet->hvb_voltage = hvb_voltage;
|
|
|
packet->bat_maxtemp = bat_maxtemp;
|
|
|
+ packet->bat_mintemp = bat_mintemp;
|
|
|
+ packet->contactor_sta = contactor_sta;
|
|
|
+ packet->soh_value = soh_value;
|
|
|
+ packet->max_volt_id = max_volt_id;
|
|
|
+ packet->min_volt_id = min_volt_id;
|
|
|
+ packet->max_temp_id = max_temp_id;
|
|
|
+ packet->min_temp_id = min_temp_id;
|
|
|
+ packet->fault_level = fault_level;
|
|
|
+ packet->irt_status = irt_status;
|
|
|
packet->cap_percent = cap_percent;
|
|
|
packet->bat_id = bat_id;
|
|
|
packet->err_code = err_code;
|
|
|
+ packet->dc28_current = dc28_current;
|
|
|
packet->hvb_temp = hvb_temp;
|
|
|
packet->hvb_status = hvb_status;
|
|
|
|
|
|
@@ -415,6 +636,26 @@ static inline int16_t mavlink_msg_shangfei_bms_get_bat_current(const mavlink_mes
|
|
|
return _MAV_RETURN_int16_t(msg, 6);
|
|
|
}
|
|
|
|
|
|
+/**
|
|
|
+ * @brief Get field cell_max_volt from shangfei_bms message
|
|
|
+ *
|
|
|
+ * @return [mV] Cell max voltage.
|
|
|
+ */
|
|
|
+static inline uint16_t mavlink_msg_shangfei_bms_get_cell_max_volt(const mavlink_message_t* msg)
|
|
|
+{
|
|
|
+ return _MAV_RETURN_uint16_t(msg, 8);
|
|
|
+}
|
|
|
+
|
|
|
+/**
|
|
|
+ * @brief Get field cell_min_volt from shangfei_bms message
|
|
|
+ *
|
|
|
+ * @return [mV] Cell min voltage.
|
|
|
+ */
|
|
|
+static inline uint16_t mavlink_msg_shangfei_bms_get_cell_min_volt(const mavlink_message_t* msg)
|
|
|
+{
|
|
|
+ return _MAV_RETURN_uint16_t(msg, 10);
|
|
|
+}
|
|
|
+
|
|
|
/**
|
|
|
* @brief Get field bat_maxtemp from shangfei_bms message
|
|
|
*
|
|
|
@@ -422,7 +663,97 @@ static inline int16_t mavlink_msg_shangfei_bms_get_bat_current(const mavlink_mes
|
|
|
*/
|
|
|
static inline int8_t mavlink_msg_shangfei_bms_get_bat_maxtemp(const mavlink_message_t* msg)
|
|
|
{
|
|
|
- return _MAV_RETURN_int8_t(msg, 10);
|
|
|
+ return _MAV_RETURN_int8_t(msg, 16);
|
|
|
+}
|
|
|
+
|
|
|
+/**
|
|
|
+ * @brief Get field bat_mintemp from shangfei_bms message
|
|
|
+ *
|
|
|
+ * @return Battery Min_temperature.
|
|
|
+ */
|
|
|
+static inline int8_t mavlink_msg_shangfei_bms_get_bat_mintemp(const mavlink_message_t* msg)
|
|
|
+{
|
|
|
+ return _MAV_RETURN_int8_t(msg, 17);
|
|
|
+}
|
|
|
+
|
|
|
+/**
|
|
|
+ * @brief Get field contactor_sta from shangfei_bms message
|
|
|
+ *
|
|
|
+ * @return Contactor status.
|
|
|
+ */
|
|
|
+static inline uint8_t mavlink_msg_shangfei_bms_get_contactor_sta(const mavlink_message_t* msg)
|
|
|
+{
|
|
|
+ return _MAV_RETURN_uint8_t(msg, 18);
|
|
|
+}
|
|
|
+
|
|
|
+/**
|
|
|
+ * @brief Get field soh_value from shangfei_bms message
|
|
|
+ *
|
|
|
+ * @return [%] Battery SOH value.
|
|
|
+ */
|
|
|
+static inline uint8_t mavlink_msg_shangfei_bms_get_soh_value(const mavlink_message_t* msg)
|
|
|
+{
|
|
|
+ return _MAV_RETURN_uint8_t(msg, 19);
|
|
|
+}
|
|
|
+
|
|
|
+/**
|
|
|
+ * @brief Get field max_volt_id from shangfei_bms message
|
|
|
+ *
|
|
|
+ * @return Cell max voltage id.
|
|
|
+ */
|
|
|
+static inline uint8_t mavlink_msg_shangfei_bms_get_max_volt_id(const mavlink_message_t* msg)
|
|
|
+{
|
|
|
+ return _MAV_RETURN_uint8_t(msg, 20);
|
|
|
+}
|
|
|
+
|
|
|
+/**
|
|
|
+ * @brief Get field min_volt_id from shangfei_bms message
|
|
|
+ *
|
|
|
+ * @return Cell min voltage id.
|
|
|
+ */
|
|
|
+static inline uint8_t mavlink_msg_shangfei_bms_get_min_volt_id(const mavlink_message_t* msg)
|
|
|
+{
|
|
|
+ return _MAV_RETURN_uint8_t(msg, 21);
|
|
|
+}
|
|
|
+
|
|
|
+/**
|
|
|
+ * @brief Get field max_temp_id from shangfei_bms message
|
|
|
+ *
|
|
|
+ * @return Cell max temperature id.
|
|
|
+ */
|
|
|
+static inline int8_t mavlink_msg_shangfei_bms_get_max_temp_id(const mavlink_message_t* msg)
|
|
|
+{
|
|
|
+ return _MAV_RETURN_int8_t(msg, 22);
|
|
|
+}
|
|
|
+
|
|
|
+/**
|
|
|
+ * @brief Get field min_temp_id from shangfei_bms message
|
|
|
+ *
|
|
|
+ * @return Cell min temperature id.
|
|
|
+ */
|
|
|
+static inline int8_t mavlink_msg_shangfei_bms_get_min_temp_id(const mavlink_message_t* msg)
|
|
|
+{
|
|
|
+ return _MAV_RETURN_int8_t(msg, 23);
|
|
|
+}
|
|
|
+
|
|
|
+/**
|
|
|
+ * @brief Get field fault_level from shangfei_bms message
|
|
|
+ *
|
|
|
+ * @return Fault level.
|
|
|
+ */
|
|
|
+static inline uint8_t mavlink_msg_shangfei_bms_get_fault_level(const mavlink_message_t* msg)
|
|
|
+{
|
|
|
+ return _MAV_RETURN_uint8_t(msg, 24);
|
|
|
+}
|
|
|
+
|
|
|
+/**
|
|
|
+ * @brief Get field irt_status from shangfei_bms message
|
|
|
+ *
|
|
|
+ * @return Insulation Resistance Tester status.
|
|
|
+ */
|
|
|
+static inline uint8_t mavlink_msg_shangfei_bms_get_irt_status(const mavlink_message_t* msg)
|
|
|
+{
|
|
|
+ return _MAV_RETURN_uint8_t(msg, 25);
|
|
|
}
|
|
|
|
|
|
/**
|
|
|
@@ -432,7 +763,7 @@ static inline int8_t mavlink_msg_shangfei_bms_get_bat_maxtemp(const mavlink_mess
|
|
|
*/
|
|
|
static inline uint8_t mavlink_msg_shangfei_bms_get_cap_percent(const mavlink_message_t* msg)
|
|
|
{
|
|
|
- return _MAV_RETURN_uint8_t(msg, 11);
|
|
|
+ return _MAV_RETURN_uint8_t(msg, 26);
|
|
|
}
|
|
|
|
|
|
/**
|
|
|
@@ -442,7 +773,7 @@ static inline uint8_t mavlink_msg_shangfei_bms_get_cap_percent(const mavlink_mes
|
|
|
*/
|
|
|
static inline uint8_t mavlink_msg_shangfei_bms_get_bat_id(const mavlink_message_t* msg)
|
|
|
{
|
|
|
- return _MAV_RETURN_uint8_t(msg, 12);
|
|
|
+ return _MAV_RETURN_uint8_t(msg, 27);
|
|
|
}
|
|
|
|
|
|
/**
|
|
|
@@ -452,7 +783,27 @@ static inline uint8_t mavlink_msg_shangfei_bms_get_bat_id(const mavlink_message_
|
|
|
*/
|
|
|
static inline uint8_t mavlink_msg_shangfei_bms_get_err_code(const mavlink_message_t* msg)
|
|
|
{
|
|
|
- return _MAV_RETURN_uint8_t(msg, 13);
|
|
|
+ return _MAV_RETURN_uint8_t(msg, 28);
|
|
|
+}
|
|
|
+
|
|
|
+/**
|
|
|
+ * @brief Get field dc28_volt from shangfei_bms message
|
|
|
+ *
|
|
|
+ * @return [dV] High volt-box voltage.
|
|
|
+ */
|
|
|
+static inline uint16_t mavlink_msg_shangfei_bms_get_dc28_volt(const mavlink_message_t* msg)
|
|
|
+{
|
|
|
+ return _MAV_RETURN_uint16_t(msg, 12);
|
|
|
+}
|
|
|
+
|
|
|
+/**
|
|
|
+ * @brief Get field dc28_current from shangfei_bms message
|
|
|
+ *
|
|
|
+ * @return [A] High volt-box current.
|
|
|
+ */
|
|
|
+static inline uint8_t mavlink_msg_shangfei_bms_get_dc28_current(const mavlink_message_t* msg)
|
|
|
+{
|
|
|
+ return _MAV_RETURN_uint8_t(msg, 29);
|
|
|
}
|
|
|
|
|
|
/**
|
|
|
@@ -462,7 +813,7 @@ static inline uint8_t mavlink_msg_shangfei_bms_get_err_code(const mavlink_messag
|
|
|
*/
|
|
|
static inline uint16_t mavlink_msg_shangfei_bms_get_hvb_voltage(const mavlink_message_t* msg)
|
|
|
{
|
|
|
- return _MAV_RETURN_uint16_t(msg, 8);
|
|
|
+ return _MAV_RETURN_uint16_t(msg, 14);
|
|
|
}
|
|
|
|
|
|
/**
|
|
|
@@ -472,7 +823,7 @@ static inline uint16_t mavlink_msg_shangfei_bms_get_hvb_voltage(const mavlink_me
|
|
|
*/
|
|
|
static inline int8_t mavlink_msg_shangfei_bms_get_hvb_temp(const mavlink_message_t* msg)
|
|
|
{
|
|
|
- return _MAV_RETURN_int8_t(msg, 14);
|
|
|
+ return _MAV_RETURN_int8_t(msg, 30);
|
|
|
}
|
|
|
|
|
|
/**
|
|
|
@@ -482,7 +833,7 @@ static inline int8_t mavlink_msg_shangfei_bms_get_hvb_temp(const mavlink_message
|
|
|
*/
|
|
|
static inline uint8_t mavlink_msg_shangfei_bms_get_hvb_status(const mavlink_message_t* msg)
|
|
|
{
|
|
|
- return _MAV_RETURN_uint8_t(msg, 15);
|
|
|
+ return _MAV_RETURN_uint8_t(msg, 31);
|
|
|
}
|
|
|
|
|
|
/**
|
|
|
@@ -497,11 +848,24 @@ static inline void mavlink_msg_shangfei_bms_decode(const mavlink_message_t* msg,
|
|
|
shangfei_bms->time_boot_ms = mavlink_msg_shangfei_bms_get_time_boot_ms(msg);
|
|
|
shangfei_bms->bat_voltage = mavlink_msg_shangfei_bms_get_bat_voltage(msg);
|
|
|
shangfei_bms->bat_current = mavlink_msg_shangfei_bms_get_bat_current(msg);
|
|
|
+ shangfei_bms->cell_max_volt = mavlink_msg_shangfei_bms_get_cell_max_volt(msg);
|
|
|
+ shangfei_bms->cell_min_volt = mavlink_msg_shangfei_bms_get_cell_min_volt(msg);
|
|
|
+ shangfei_bms->dc28_volt = mavlink_msg_shangfei_bms_get_dc28_volt(msg);
|
|
|
shangfei_bms->hvb_voltage = mavlink_msg_shangfei_bms_get_hvb_voltage(msg);
|
|
|
shangfei_bms->bat_maxtemp = mavlink_msg_shangfei_bms_get_bat_maxtemp(msg);
|
|
|
+ shangfei_bms->bat_mintemp = mavlink_msg_shangfei_bms_get_bat_mintemp(msg);
|
|
|
+ shangfei_bms->contactor_sta = mavlink_msg_shangfei_bms_get_contactor_sta(msg);
|
|
|
+ shangfei_bms->soh_value = mavlink_msg_shangfei_bms_get_soh_value(msg);
|
|
|
+ shangfei_bms->max_volt_id = mavlink_msg_shangfei_bms_get_max_volt_id(msg);
|
|
|
+ shangfei_bms->min_volt_id = mavlink_msg_shangfei_bms_get_min_volt_id(msg);
|
|
|
+ shangfei_bms->max_temp_id = mavlink_msg_shangfei_bms_get_max_temp_id(msg);
|
|
|
+ shangfei_bms->min_temp_id = mavlink_msg_shangfei_bms_get_min_temp_id(msg);
|
|
|
+ shangfei_bms->fault_level = mavlink_msg_shangfei_bms_get_fault_level(msg);
|
|
|
+ shangfei_bms->irt_status = mavlink_msg_shangfei_bms_get_irt_status(msg);
|
|
|
shangfei_bms->cap_percent = mavlink_msg_shangfei_bms_get_cap_percent(msg);
|
|
|
shangfei_bms->bat_id = mavlink_msg_shangfei_bms_get_bat_id(msg);
|
|
|
shangfei_bms->err_code = mavlink_msg_shangfei_bms_get_err_code(msg);
|
|
|
+ shangfei_bms->dc28_current = mavlink_msg_shangfei_bms_get_dc28_current(msg);
|
|
|
shangfei_bms->hvb_temp = mavlink_msg_shangfei_bms_get_hvb_temp(msg);
|
|
|
shangfei_bms->hvb_status = mavlink_msg_shangfei_bms_get_hvb_status(msg);
|
|
|
#else
|