From e908931286de5b921974223a8ebb3b4e7a46b56f Mon Sep 17 00:00:00 2001 From: Eugene Shilin Date: Fri, 8 May 2026 09:50:18 +0300 Subject: [PATCH 01/20] initial script --- drivers/subsystems-UVH25/NAV_UVH25.cpp | 460 +++++++++++++++++++++++++ 1 file changed, 460 insertions(+) create mode 100644 drivers/subsystems-UVH25/NAV_UVH25.cpp diff --git a/drivers/subsystems-UVH25/NAV_UVH25.cpp b/drivers/subsystems-UVH25/NAV_UVH25.cpp new file mode 100644 index 0000000..7ee4416 --- /dev/null +++ b/drivers/subsystems-UVH25/NAV_UVH25.cpp @@ -0,0 +1,460 @@ +#include + +//====================================================================================== +// CONSTANTS & CONFIGURATION +//====================================================================================== + +// Communication Ports +const uint8_t PORT_ID_CAN{1}; +const uint8_t PORT_ID_ESC{50}; + +// Protocol Package Sizes +const uint8_t PACK_SIZE_CAN{12}; +const uint8_t PACK_SIZE_ESC{10}; + +//====================================================================================== +// CAN DEFINES AND IDS +//====================================================================================== + +// VESC Tail +#define VESC_TAIL_ID 0x24 // VESC ID 36 +#define STATUS_MSG_1 0x09 +#define STATUS_MSG_2 0x0E +#define STATUS_MSG_3 0x0F +#define STATUS_MSG_4 0x10 +#define STATUS_MSG_5 0x1B + +// VESC CAN Packets +#define CAN_PACKET_SET_CURRENT 1 +#define CAN_PACKET_SET_CURRENT_BRAKE 2 +#define CAN_PACKET_SET_RPM 3 + +// UVHPU Power Management Unit +#define UVHPU_ID 0x80 +#define UVHPU_PACK1 UVHPU_ID + 1 +#define UVHPU_PACK2 UVHPU_ID + 2 +#define UVHPU_PACK3 UVHPU_ID + 3 +#define UVHPU_PACK4 UVHPU_ID + 4 +#define UVHPU_PACK5 UVHPU_ID + 5 +#define UVHPU_PACK6 UVHPU_ID + 6 +#define UVHPU_PACK7 UVHPU_ID + 7 + +// Altitude (AGL) Sensor +#define AGL_CAN_ID 0x00090002 + +//====================================================================================== +// TYPE ALIASES - MANDALA PARAMETERS +//====================================================================================== + +// VESC Tail Parameters +using m_vesc_tail_rpm = Mandala; +using m_vesc_tail_current = Mandala; +using m_vesc_tail_duty = Mandala; +using m_vesc_tail_temp_fet = Mandala; +using m_vesc_tail_temp_motor = Mandala; +using m_vesc_tail_curr_in = Mandala; + +// UVH Power Management Unit +using m_uvhpy_status = Mandala; +using m_uvhpy_ibat_filt = Mandala; + +// Engine Parameters +using m_eng_ctr = Mandala; +using m_rpm = Mandala; +using m_eng_temp = Mandala; +using m_eng_volt = Mandala; +using m_eng_current = Mandala; +using m_eng_rpm = Mandala; + +// Altitude (AGL) +using m_agl = Mandala; + +//====================================================================================== +// DATA STRUCTURES +//====================================================================================== + +// ESC VCP Data Structure +struct ESC_VCP_Data +{ + uint8_t temp; + float voltage; + float current; + uint16_t consumption; + uint16_t rpm; +}; + +// VESC CAN Data Structure +struct VESC_CAN_Data +{ + int32_t rpm; // STATUS_MSG_1 + float current; // STATUS_MSG_1 + float duty; // STATUS_MSG_1 + uint32_t apm_hours; // STATUS_MSG_2 + uint32_t apm_hours_charged; // STATUS_MSG_2 + uint32_t watt_hours; // STATUS_MSG_3 + uint32_t watt_hours_charged; // STATUS_MSG_3 + float temp_fet; // STATUS_MSG_4 + float temp_mot; // STATUS_MSG_4 + float curr_in; // STATUS_MSG_4 + float pid_pos_now; // STATUS_MSG_4 + float voltage; // STATUS_MSG_5 + uint32_t tacho; // STATUS_MSG_5 +}; + +// UVHPU Data Structure +#pragma pack(1) +struct UVHPU +{ + struct + { + float vbat; + float ibat; + float imon; + } MSG1; + struct + { + float vout; + float tbat; + float pbat; + float status; + } MSG2; + struct + { + float cbat; + float ebat; + } MSG3; + struct + { + float res_bar; + float v_res; + } MSG4; + struct + { + float ibat_filt; + float vbat_filt; + } MSG5; + struct + { + float cbat_res; + float ebat_res; + } MSG6; + struct + { + int16_t life_cycles; + float cbat_mod; + } MSG7; +}; +#pragma pack() + +//====================================================================================== +// GLOBAL VARIABLES +//====================================================================================== + +// ESC Handler Data +ESC_VCP_Data esc_data; +uint8_t esc_tbuf[PACK_SIZE_ESC]{}; + +// VESC Tail Data +VESC_CAN_Data tail_data{}; + +// UVHPU Data +UVHPU _uvhpu{}; + +//====================================================================================== +// FUNCTION DECLARATIONS +//====================================================================================== + +// CAN Control Functions +void setRPM(const uint8_t &, const int32_t &); +void setCurrent(const uint8_t &, const float &); + + +//====================================================================================== +// MAIN ENTRY POINT +//====================================================================================== + +int main() +{ + schedule_periodic(task("on_main"), 100); + + task("uvhpu"); // GCS with terminal command `vmexec("uvhpu")` + + m_eng_ctr(); + m_rpm(); + + receive(PORT_ID_ESC, "esc_handler"); + receive(PORT_ID_CAN, "on_serial"); +} + +//====================================================================================== +// UTILITY FUNCTIONS - CRC & SERIALIZATION +//====================================================================================== + +uint8_t update_crc8(uint8_t data, uint8_t crc) +{ + data ^= crc; + + for (uint8_t i = 0; i < 8; i++) { + data = uint8_t((data & 0x80) ? 0x07 ^ (data << 1) : (data << 1)); + } + return data; +} + +uint8_t get_crc8(const uint8_t *data, uint8_t len) +{ + uint8_t crc{0}; + for (uint8_t i = 0; i < len; i++) { + crc = update_crc8(data[i], crc); + } + return crc & 0xFF; +} + +void serializeInt(uint8_t *data, uint8_t index, int32_t value) +{ + for (uint8_t i = 0; i < 4; i++) { + uint8_t shift = 8 * (4 - i - 1); + data[index + i] = (value >> shift) & 0xFF; + } +} + +int16_t unpackInt16(const uint8_t *data, uint8_t index) +{ + return (int16_t) (data[index] | (data[index + 1] << 8)); +} + +//====================================================================================== +// ESC HANDLER - VCP DATA PROCESSING +//====================================================================================== + +EXPORT void esc_handler(const uint8_t *data, size_t size) +{ + if (size != PACK_SIZE_ESC) { + return; + } + + memcpy(esc_tbuf, data, size); + + if (get_crc8(esc_tbuf, PACK_SIZE_ESC - 1) != esc_tbuf[PACK_SIZE_ESC - 1]) { + return; + } + + esc_data.temp = data[0]; + esc_data.voltage = float((esc_tbuf[1] << 8) | (esc_tbuf[2])) / 100.f; + esc_data.current = float((esc_tbuf[3] << 8) | (esc_tbuf[4])) / 100.f; + esc_data.consumption = uint16_t((esc_tbuf[5] << 8) | (esc_tbuf[6])); + esc_data.rpm = uint16_t((esc_tbuf[7] << 8) | (esc_tbuf[8])) * 100 / 7u; +} + +//====================================================================================== +// VESC CAN PROCESSING - TAIL MOTOR CONTROLLER +//====================================================================================== + +void processVESCPackage(const uint32_t &msg_id, const uint8_t *data, VESC_CAN_Data *vesc_data) +{ + switch (msg_id) { + case STATUS_MSG_1: { + vesc_data->rpm = int32_t((data[0] << 24) | (data[1] << 16) | (data[2] << 8) | data[3]); + vesc_data->current = float(int16_t((data[4] << 8) | data[5])) / 10.f; + vesc_data->duty = float(int16_t((data[6] << 8) | data[7])) / 10.f; + break; + } + case STATUS_MSG_2: { + vesc_data->apm_hours = uint32_t((data[0] << 24) | (data[1] << 16) | (data[2] << 8) + | data[3]); + vesc_data->apm_hours_charged = uint32_t((data[4] << 24) | (data[5] << 16) | (data[6] << 8) + | data[7]); + break; + } + case STATUS_MSG_3: { + vesc_data->watt_hours = uint32_t((data[0] << 24) | (data[1] << 16) | (data[2] << 8) + | data[3]); + vesc_data->watt_hours_charged = uint32_t((data[4] << 24) | (data[5] << 16) | (data[6] << 8) + | data[7]); + break; + } + case STATUS_MSG_4: { + vesc_data->temp_fet = float(int16_t((data[0] << 8) | data[1])) / 10.f; + vesc_data->temp_mot = float(int16_t((data[2] << 8) | data[3])) / 10.f; + vesc_data->curr_in = float(int16_t((data[4] << 8) | data[5])) / 10.f; + vesc_data->pid_pos_now = float(int16_t((data[6] << 8) | data[7])) / 50.f; + break; + } + case STATUS_MSG_5: { + vesc_data->voltage = float(int16_t((data[4] << 8) | data[5])) / 10.f; + vesc_data->tacho = uint32_t((data[0] << 24) | (data[1] << 16) | (data[2] << 8) | data[3]); + break; + } + } +} + +void setRPM(const uint8_t &VECS_CAN_ID, const int32_t &val) +{ + uint8_t msg[4 + 4] = {}; // ext id + DATA + + msg[0] = VECS_CAN_ID; + msg[1] = CAN_PACKET_SET_RPM; + msg[3] |= 0x80; // IDE (bit 7) 1=ext,0=std + serializeInt(msg, 4, val); + send(PORT_ID_CAN, msg, 8, false); +} + +void setCurrent(const uint8_t &VECS_CAN_ID, const float &val) +{ + uint8_t msg[4 + 4] = {}; // ext id + DATA + int32_t current = int32_t(val * 1000); + + msg[0] = VECS_CAN_ID; + msg[1] = CAN_PACKET_SET_CURRENT; + msg[3] |= 0x80; // IDE (bit 7) 1=ext,0=std; + serializeInt(msg, 4, current); + send(PORT_ID_CAN, msg, 8, false); +} + +//====================================================================================== +// UVHPU PROCESSING - POWER MANAGEMENT UNIT +//====================================================================================== + +void processUVHPUackage(const uint32_t &can_id, const uint8_t *data) +{ + switch (can_id) { + case UVHPU_PACK1: { + _uvhpu.MSG1.vbat = (float) unpackInt16(data, 0) / 100.f; + memcpy(&_uvhpu.MSG1.ibat, data + 2, 4); + _uvhpu.MSG1.imon = (float) unpackInt16(data, 6) / 100.f; + break; + } + case UVHPU_PACK2: { + _uvhpu.MSG2.vout = (float) unpackInt16(data, 0) / 100.f; + _uvhpu.MSG2.tbat = (float) unpackInt16(data, 2) / 100.f; + _uvhpu.MSG2.pbat = (float) unpackInt16(data, 4); + _uvhpu.MSG2.status = data[7]; + + m_uvhpy_status::publish(_uvhpu.MSG2.status); + break; + } + case UVHPU_PACK3: { + memcpy(&_uvhpu.MSG3.cbat, data, 8); + break; + } + case UVHPU_PACK4: { + memcpy(&_uvhpu.MSG4.res_bar, data, 8); + break; + } + case UVHPU_PACK5: { + memcpy(&_uvhpu.MSG5.ibat_filt, data, 8); + m_uvhpy_ibat_filt::publish(_uvhpu.MSG5.ibat_filt); + break; + } + case UVHPU_PACK6: { + memcpy(&_uvhpu.MSG6.cbat_res, data, 8); + break; + } + case UVHPU_PACK7: { + _uvhpu.MSG7.life_cycles = unpackInt16(data, 0); + memcpy(&_uvhpu.MSG7.cbat_mod, data + 2, 4); + break; + } + } +} + +//====================================================================================== +// MAIN PERIODIC TASK - ESC DATA PUBLISHING +//====================================================================================== + +EXPORT void on_main() +{ + // Save ESC data to mandala + m_eng_temp::publish((uint32_t) esc_data.temp); + m_eng_volt::publish((float) esc_data.voltage); + m_eng_current::publish((float) esc_data.current); + m_eng_rpm::publish((uint32_t) esc_data.rpm); +} + +//====================================================================================== +// UVHPU TERMINAL COMMAND - DATA DISPLAY +//====================================================================================== + +EXPORT void uvhpu() +{ + printf("vbat: %.2f", _uvhpu.MSG1.vbat); + printf("ibat: %.2f", _uvhpu.MSG1.ibat); + printf("imon: %.2f", _uvhpu.MSG1.imon); + + printf("vout: %.2f", _uvhpu.MSG2.vout); + printf("tbat: %.2f", _uvhpu.MSG2.tbat); + printf("pbat: %.2f", _uvhpu.MSG2.pbat); + printf("status: %u", _uvhpu.MSG2.status); + + printf("cbat: %.2f", _uvhpu.MSG3.cbat); + printf("ebat: %.2f", _uvhpu.MSG3.ebat); + + printf("res_bar: %.2f", _uvhpu.MSG4.res_bar); + printf("v_res: %.2f", _uvhpu.MSG4.v_res); + + printf("ibat_filt: %.2f", _uvhpu.MSG5.ibat_filt); + printf("vbat_filt: %.2f", _uvhpu.MSG5.vbat_filt); + + printf("cbat_res: %.2f", _uvhpu.MSG6.cbat_res); + printf("ebat_res: %.2f", _uvhpu.MSG6.ebat_res); + + printf("life_cycles: %u", _uvhpu.MSG7.life_cycles); + printf("cbat_mod: %.2f", _uvhpu.MSG7.cbat_mod); +} + +//====================================================================================== +// CAN MESSAGE ROUTER - MAIN SERIAL HANDLER +//====================================================================================== + +EXPORT void on_serial(const uint8_t *data, size_t size) +{ + if (size != PACK_SIZE_CAN) { + //return; + } + + uint32_t can_id = (uint32_t) (data[0] + (data[1] << 8) + (data[2] << 16) + (data[3] << 24)); + can_id &= 0x7FFFFFFF; // 32nd bit is ext/std flag + + uint8_t can_data[8] = {}; + for (uint8_t i = 0; i < 8; i++) { + can_data[i] = data[4 + i]; // 4 is data position + } + + // Process AGL (Altitude) message + if (can_id == AGL_CAN_ID) { + uint16_t raw = (uint16_t) ((data[4] << 8) + data[5]); + float altitude = float(raw) / 100.0f; + if (altitude > 0.1f && altitude < 40.0f) + m_agl::publish(altitude); + return; + } + + // Process VESC Tail Motor (CAN ID 0xFF) + switch (can_id & 0xFF) { + case VESC_TAIL_ID: { + uint16_t msg_id = (can_id >> 8) & 0xFF; + processVESCPackage(msg_id, can_data, &tail_data); + + m_vesc_tail_rpm::publish((float) tail_data.rpm / 11); + m_vesc_tail_current::publish(tail_data.current); + m_vesc_tail_duty::publish(tail_data.duty); + m_vesc_tail_temp_fet::publish(tail_data.temp_fet); + m_vesc_tail_temp_motor::publish(tail_data.temp_mot); + m_vesc_tail_curr_in::publish(tail_data.curr_in); + break; + } + } + + // Process UVHPU Power Management (CAN ID 0xFFFF) + switch (can_id & 0xFFFF) { + case UVHPU_PACK1: + case UVHPU_PACK2: + case UVHPU_PACK3: + case UVHPU_PACK4: + case UVHPU_PACK5: + case UVHPU_PACK6: + case UVHPU_PACK7: { + processUVHPUackage(can_id, can_data); + break; + } + } +} From 4d3ceb675e0473c82207b218d6832ff34ff74b06 Mon Sep 17 00:00:00 2001 From: Eugene Shilin Date: Mon, 11 May 2026 09:40:24 +0300 Subject: [PATCH 02/20] radio agl checked --- drivers/subsystems-UVH25/NAV_UVH25.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/drivers/subsystems-UVH25/NAV_UVH25.cpp b/drivers/subsystems-UVH25/NAV_UVH25.cpp index 7ee4416..49c1025 100644 --- a/drivers/subsystems-UVH25/NAV_UVH25.cpp +++ b/drivers/subsystems-UVH25/NAV_UVH25.cpp @@ -67,7 +67,7 @@ using m_eng_current = Mandala; using m_eng_rpm = Mandala; // Altitude (AGL) -using m_agl = Mandala; +using m_agl = Mandala; //====================================================================================== // DATA STRUCTURES From adee0ce6895930a0a93e476ca15f30d1ad9d8d9d Mon Sep 17 00:00:00 2001 From: Eugene Shilin Date: Mon, 11 May 2026 17:58:24 +0300 Subject: [PATCH 03/20] ERS initial algorithm --- drivers/subsystems-UVH25/NAV_UVH25.cpp | 149 ++++++++++++++++++++++++- 1 file changed, 147 insertions(+), 2 deletions(-) diff --git a/drivers/subsystems-UVH25/NAV_UVH25.cpp b/drivers/subsystems-UVH25/NAV_UVH25.cpp index 49c1025..320a74b 100644 --- a/drivers/subsystems-UVH25/NAV_UVH25.cpp +++ b/drivers/subsystems-UVH25/NAV_UVH25.cpp @@ -12,6 +12,19 @@ const uint8_t PORT_ID_ESC{50}; const uint8_t PACK_SIZE_CAN{12}; const uint8_t PACK_SIZE_ESC{10}; +//====================================================================================== +// ERS CONSTANTS +//====================================================================================== + +#define R_SQUIB_MIN 1.0f //Ohm +#define R_SQUIB_MAX 8.5f //Ohm +#define R_WIRES 0.68f //Ohm, measured by shorting pyro with diag on + +#define MULT_PIRO_U_DIAG 1.128f +#define MULT_SQUIB_U_DIAG 1.1f +#define MULT_PIRO_U_ARM 1.128f +#define MULT_SQUIB_U_ARM 1.1f + //====================================================================================== // CAN DEFINES AND IDS //====================================================================================== @@ -42,6 +55,12 @@ const uint8_t PACK_SIZE_ESC{10}; // Altitude (AGL) Sensor #define AGL_CAN_ID 0x00090002 +//====================================================================================== +// ERS STATE ENUM +//====================================================================================== + +enum class ERS_State { ERROR = 0, DISARMED = 1, ARMED = 2, FIRED = 3 }; + //====================================================================================== // TYPE ALIASES - MANDALA PARAMETERS //====================================================================================== @@ -69,6 +88,16 @@ using m_eng_rpm = Mandala; // Altitude (AGL) using m_agl = Mandala; +//ERS +using m_ERS_block = Mandala; +using m_ERS_launch = Mandala; +using m_pyro_U = Mandala; +using m_squib_U = Mandala; +using m_ERS_fire = Mandala; +using m_ERS_charge = Mandala; +using m_ERS_diag = Mandala; +using m_ERS_status = Mandala; + //====================================================================================== // DATA STRUCTURES //====================================================================================== @@ -160,6 +189,10 @@ VESC_CAN_Data tail_data{}; // UVHPU Data UVHPU _uvhpu{}; +// ERS State +ERS_State ers_state = ERS_State::DISARMED; +uint8_t stab_counter = 10; // 1sec time + //====================================================================================== // FUNCTION DECLARATIONS //====================================================================================== @@ -168,7 +201,6 @@ UVHPU _uvhpu{}; void setRPM(const uint8_t &, const int32_t &); void setCurrent(const uint8_t &, const float &); - //====================================================================================== // MAIN ENTRY POINT //====================================================================================== @@ -176,12 +208,20 @@ void setCurrent(const uint8_t &, const float &); int main() { schedule_periodic(task("on_main"), 100); + schedule_periodic(task("on_ers"), 100); task("uvhpu"); // GCS with terminal command `vmexec("uvhpu")` - + m_eng_ctr(); m_rpm(); + m_squib_U(); + m_pyro_U(); + m_ERS_block(); + m_ERS_launch(); + + m_ERS_block::publish(true); + receive(PORT_ID_ESC, "esc_handler"); receive(PORT_ID_CAN, "on_serial"); } @@ -357,6 +397,111 @@ void processUVHPUackage(const uint32_t &can_id, const uint8_t *data) } } +//====================================================================================== +// ERS PERIODIC TASK +//====================================================================================== + +EXPORT void on_ers() +{ + /*float squib_R = (m_squib_U::value() * MULT_SQUIB_U_DIAG) / (200.0f) + / (m_pyro_U::value() * MULT_PIRO_U_DIAG / 5600.0f) + - R_WIRES; + printf("resist %.2f", squib_R);*/ + + switch (ers_state) { + case ERS_State::DISARMED: { + if (stab_counter == 10) { //start of check + m_ERS_status::publish(false); + m_ERS_diag::publish(false); + m_ERS_charge::publish(false); + m_ERS_fire::publish(false); + m_ERS_diag::publish(true); + } + + float squib_R = (m_squib_U::value() * MULT_SQUIB_U_DIAG) / (200.0f) + / (m_pyro_U::value() * MULT_PIRO_U_DIAG / 5600.0f) + - R_WIRES; + printf("resist %.2f", squib_R); + if (squib_R < R_SQUIB_MIN || squib_R > R_SQUIB_MAX) { + stab_counter--; + if (stab_counter == 0) { + stab_counter = 10; + m_ERS_diag::publish(false); + //ers_state = ERS_State::ERROR; + printf("squib resistance out of range: %.2f", squib_R); + } + return; + } + + if (m_pyro_U::value() > 3.3f) { + m_ERS_diag::publish(false); + printf("pyro voltage too high: %.2f", m_pyro_U::value()); + //ers_state = ERS_State::ERROR; + return; + } + + m_ERS_fire::publish(true); + sleep(1000); + + if (m_pyro_U::value() > 0.3f) { + m_ERS_diag::publish(false); + printf("pyro voltage too high: %.2f", m_pyro_U::value()); + //ers_state = ERS_State::ERROR; + return; + } + + m_ERS_fire::publish(false); + //sleep(1000); + m_ERS_diag::publish(false); + //sleep(1000); + + if (m_ERS_block::value() == false) { + ers_state = ERS_State::ARMED; + m_ + } + break; + } + case ERS_State::ARMED: { + if (m_ERS_block::value() == true) { + ers_state = ERS_State::DISARMED; + return; + } + m_ERS_status::publish(true); //turn LED on + m_ERS_charge::publish(true); //charge capacitor + //delaY? + + if (m_pyro_U::value() < 8.0f) { + printf("pyro voltage too low: %.2f", m_pyro_U::value()); + return; + } + + float squib_R = (m_squib_U::value() * MULT_SQUIB_U_ARM) / (200.0f) + / (m_pyro_U::value() * MULT_PIRO_U_ARM / 5600.0f) + - R_WIRES; + + if (squib_R < R_SQUIB_MIN || squib_R > R_SQUIB_MAX) { + printf("squib resistance out of range: %.2f", squib_R); + return; + } + + if (m_ERS_launch::value() == true) { + m_ERS_fire::publish(true); + ers_state = ERS_State::FIRED; + } + + break; + } + case ERS_State::FIRED: { + printf("ERS FIRED"); + break; + } + case ERS_State::ERROR: { + printf("ERS ERROR"); + break; + } + } +} + //====================================================================================== // MAIN PERIODIC TASK - ESC DATA PUBLISHING //====================================================================================== From 028dfac8f0dc9e32af84dc88138104df6746eec1 Mon Sep 17 00:00:00 2001 From: Eugene Shilin Date: Tue, 12 May 2026 15:45:08 +0300 Subject: [PATCH 04/20] recalculate volt and resist after charge --- drivers/subsystems-UVH25/NAV_UVH25.cpp | 29 +++++++++++++------------- 1 file changed, 15 insertions(+), 14 deletions(-) diff --git a/drivers/subsystems-UVH25/NAV_UVH25.cpp b/drivers/subsystems-UVH25/NAV_UVH25.cpp index 320a74b..ff129db 100644 --- a/drivers/subsystems-UVH25/NAV_UVH25.cpp +++ b/drivers/subsystems-UVH25/NAV_UVH25.cpp @@ -418,9 +418,10 @@ EXPORT void on_ers() m_ERS_diag::publish(true); } - float squib_R = (m_squib_U::value() * MULT_SQUIB_U_DIAG) / (200.0f) - / (m_pyro_U::value() * MULT_PIRO_U_DIAG / 5600.0f) - - R_WIRES; + float squib_U = m_squib_U::value() * MULT_SQUIB_U_DIAG; + float pyro_U = m_pyro_U::value() * MULT_PIRO_U_DIAG; + float squib_R = (squib_U / 200.0f) / (pyro_U / 5600.0f) - R_WIRES; + printf("resist %.2f", squib_R); if (squib_R < R_SQUIB_MIN || squib_R > R_SQUIB_MAX) { stab_counter--; @@ -433,19 +434,19 @@ EXPORT void on_ers() return; } - if (m_pyro_U::value() > 3.3f) { + if (pyro_U > 3.3f) { m_ERS_diag::publish(false); - printf("pyro voltage too high: %.2f", m_pyro_U::value()); + printf("pyro voltage too high: %.2f", pyro_U); //ers_state = ERS_State::ERROR; return; } m_ERS_fire::publish(true); - sleep(1000); + //sleep(1000); - if (m_pyro_U::value() > 0.3f) { + if (pyro_U > 0.3f) { m_ERS_diag::publish(false); - printf("pyro voltage too high: %.2f", m_pyro_U::value()); + printf("pyro voltage too high: %.2f", pyro_U); //ers_state = ERS_State::ERROR; return; } @@ -470,15 +471,15 @@ EXPORT void on_ers() m_ERS_charge::publish(true); //charge capacitor //delaY? - if (m_pyro_U::value() < 8.0f) { - printf("pyro voltage too low: %.2f", m_pyro_U::value()); + float squib_U = m_squib_U::value() * MULT_SQUIB_U_ARM; //recalibrate after charging + float pyro_U = m_pyro_U::value() * MULT_PIRO_U_ARM; + float squib_R = (squib_U / 200.0f) / (pyro_U / 5600.0f) - R_WIRES; + + if (pyro_U < 8.0f) { + printf("pyro voltage too low: %.2f", pyro_U); return; } - float squib_R = (m_squib_U::value() * MULT_SQUIB_U_ARM) / (200.0f) - / (m_pyro_U::value() * MULT_PIRO_U_ARM / 5600.0f) - - R_WIRES; - if (squib_R < R_SQUIB_MIN || squib_R > R_SQUIB_MAX) { printf("squib resistance out of range: %.2f", squib_R); return; From e163aaa372c7a264fba330733e5cfa94e7e31d89 Mon Sep 17 00:00:00 2001 From: Eugene Shilin Date: Tue, 12 May 2026 17:47:44 +0300 Subject: [PATCH 05/20] logic for ERS algorithm improved --- drivers/subsystems-UVH25/NAV_UVH25.cpp | 122 ++++++++++++++++--------- 1 file changed, 77 insertions(+), 45 deletions(-) diff --git a/drivers/subsystems-UVH25/NAV_UVH25.cpp b/drivers/subsystems-UVH25/NAV_UVH25.cpp index ff129db..eb5b81a 100644 --- a/drivers/subsystems-UVH25/NAV_UVH25.cpp +++ b/drivers/subsystems-UVH25/NAV_UVH25.cpp @@ -24,6 +24,7 @@ const uint8_t PACK_SIZE_ESC{10}; #define MULT_SQUIB_U_DIAG 1.1f #define MULT_PIRO_U_ARM 1.128f #define MULT_SQUIB_U_ARM 1.1f +#define COUNTER_THRESHOLD 10 //====================================================================================== // CAN DEFINES AND IDS @@ -59,7 +60,14 @@ const uint8_t PACK_SIZE_ESC{10}; // ERS STATE ENUM //====================================================================================== -enum class ERS_State { ERROR = 0, DISARMED = 1, ARMED = 2, FIRED = 3 }; +enum class ERS_State { + ERROR = 0, + DISARMED_INIT = 1, + DISARMED_LOOP = 2, + ARM_INIT = 3, + ARM_LOOP = 4, + FIRED = 5 +}; //====================================================================================== // TYPE ALIASES - MANDALA PARAMETERS @@ -190,8 +198,11 @@ VESC_CAN_Data tail_data{}; UVHPU _uvhpu{}; // ERS State -ERS_State ers_state = ERS_State::DISARMED; -uint8_t stab_counter = 10; // 1sec time +ERS_State ers_state = ERS_State::DISARMED_INIT; +bool fire_check_done = false; +uint8_t diag_counter = COUNTER_THRESHOLD; +uint8_t fire_check_counter = COUNTER_THRESHOLD; +uint8_t charge_counter = COUNTER_THRESHOLD; //====================================================================================== // FUNCTION DECLARATIONS @@ -409,80 +420,92 @@ EXPORT void on_ers() printf("resist %.2f", squib_R);*/ switch (ers_state) { - case ERS_State::DISARMED: { - if (stab_counter == 10) { //start of check - m_ERS_status::publish(false); - m_ERS_diag::publish(false); - m_ERS_charge::publish(false); - m_ERS_fire::publish(false); - m_ERS_diag::publish(true); - } + case ERS_State::DISARMED_INIT: { + m_ERS_status::publish(false); //turn LED off + m_ERS_charge::publish(false); //discharge capacitor + m_ERS_fire::publish(false); //ensure fire is off + m_ERS_diag::publish(true); //turn on diag voltage + ers_state = ERS_State::DISARMED_LOOP; + break; + } + case ERS_State::DISARMED_LOOP: { float squib_U = m_squib_U::value() * MULT_SQUIB_U_DIAG; float pyro_U = m_pyro_U::value() * MULT_PIRO_U_DIAG; float squib_R = (squib_U / 200.0f) / (pyro_U / 5600.0f) - R_WIRES; - printf("resist %.2f", squib_R); if (squib_R < R_SQUIB_MIN || squib_R > R_SQUIB_MAX) { - stab_counter--; - if (stab_counter == 0) { - stab_counter = 10; - m_ERS_diag::publish(false); - //ers_state = ERS_State::ERROR; + diag_counter--; + if (diag_counter == 0) { + ers_state = ERS_State::ERROR; printf("squib resistance out of range: %.2f", squib_R); } return; + } else { + diag_counter = COUNTER_THRESHOLD; } if (pyro_U > 3.3f) { - m_ERS_diag::publish(false); printf("pyro voltage too high: %.2f", pyro_U); - //ers_state = ERS_State::ERROR; + ers_state = ERS_State::ERROR; return; } - m_ERS_fire::publish(true); - //sleep(1000); - - if (pyro_U > 0.3f) { - m_ERS_diag::publish(false); - printf("pyro voltage too high: %.2f", pyro_U); - //ers_state = ERS_State::ERROR; - return; + if (fire_check_done == false) { + m_ERS_fire::publish(true); + //delay here; + + if (pyro_U > 0.3f) { + fire_check_counter--; + if (fire_check_counter == 0) { + ers_state = ERS_State::ERROR; + printf("pyro voltage did not drop after fire: %.2f", pyro_U); + } + return; + } else { + fire_check_counter = COUNTER_THRESHOLD; + } + //everything ok, disarm fire and move on + m_ERS_fire::publish(false); + fire_check_done = true; } - m_ERS_fire::publish(false); - //sleep(1000); - m_ERS_diag::publish(false); - //sleep(1000); - + //all checks passed, wait for arm command if (m_ERS_block::value() == false) { - ers_state = ERS_State::ARMED; - m_ + ers_state = ERS_State::ARM_INIT; } break; } - case ERS_State::ARMED: { + case ERS_State::ARM_INIT: { + m_ERS_diag::publish(false); //ensure diag voltage is off + m_ERS_status::publish(true); //turn LED on + m_ERS_charge::publish(true); //charge capacitor + + ers_state = ERS_State::ARM_LOOP; + break; + } + + case ERS_State::ARM_LOOP: { if (m_ERS_block::value() == true) { - ers_state = ERS_State::DISARMED; + ers_state = ERS_State::DISARMED_INIT; return; } - m_ERS_status::publish(true); //turn LED on - m_ERS_charge::publish(true); //charge capacitor - //delaY? float squib_U = m_squib_U::value() * MULT_SQUIB_U_ARM; //recalibrate after charging float pyro_U = m_pyro_U::value() * MULT_PIRO_U_ARM; float squib_R = (squib_U / 200.0f) / (pyro_U / 5600.0f) - R_WIRES; - if (pyro_U < 8.0f) { - printf("pyro voltage too low: %.2f", pyro_U); - return; + if (squib_R < R_SQUIB_MIN || squib_R > R_SQUIB_MAX) { + charge_counter--; + if (charge_counter == 0) { + printf("squib resistance out of range: %.2f", squib_R); + } + } else { + charge_counter = COUNTER_THRESHOLD; } - if (squib_R < R_SQUIB_MIN || squib_R > R_SQUIB_MAX) { - printf("squib resistance out of range: %.2f", squib_R); - return; + if (pyro_U < 8.0f) { //already stabilized at this point + printf("pyro voltage too low: %.2f", pyro_U); } if (m_ERS_launch::value() == true) { @@ -492,12 +515,21 @@ EXPORT void on_ers() break; } + case ERS_State::FIRED: { printf("ERS FIRED"); + m_ERS_status::publish(false); //turn arm LED off + m_ERS_charge::publish(false); //discharge capacitor + m_ERS_fire::publish(false); //ensure fire is off break; } case ERS_State::ERROR: { printf("ERS ERROR"); + m_ERS_fire::publish(false); //ensure fire is off + m_ERS_diag::publish(false); //turn off diag voltage + m_ERS_status::publish(false); //turn arm LED off + m_ERS_charge::publish(false); //discharge capacitor + break; } } From 27af68456946e6a6b13bfc0fd9475a47f59a5c82 Mon Sep 17 00:00:00 2001 From: Eugene Shilin Date: Wed, 13 May 2026 11:14:18 +0300 Subject: [PATCH 06/20] ERS algorithm implemented --- drivers/subsystems-UVH25/NAV_UVH25.cpp | 97 +++++++++++++++----------- 1 file changed, 58 insertions(+), 39 deletions(-) diff --git a/drivers/subsystems-UVH25/NAV_UVH25.cpp b/drivers/subsystems-UVH25/NAV_UVH25.cpp index eb5b81a..9436370 100644 --- a/drivers/subsystems-UVH25/NAV_UVH25.cpp +++ b/drivers/subsystems-UVH25/NAV_UVH25.cpp @@ -24,7 +24,12 @@ const uint8_t PACK_SIZE_ESC{10}; #define MULT_SQUIB_U_DIAG 1.1f #define MULT_PIRO_U_ARM 1.128f #define MULT_SQUIB_U_ARM 1.1f -#define COUNTER_THRESHOLD 10 +#define DIAG_CNT_THRESHOLD 10 //1s for 100ms period, ADC should be set for 10Hz! +#define CHRG_CNT_THRESHOLD 30 //3s for 100ms period, ADC should be set for 10Hz! +#define TASK_ERS_PERIOD 100 //ms +#define MIN_VOLT_CHARGED 8.0f +#define MAX_VOLT_DIAG 3.3f +#define MAX_VOLT_DIAG_FIRE 0.3f //====================================================================================== // CAN DEFINES AND IDS @@ -200,9 +205,9 @@ UVHPU _uvhpu{}; // ERS State ERS_State ers_state = ERS_State::DISARMED_INIT; bool fire_check_done = false; -uint8_t diag_counter = COUNTER_THRESHOLD; -uint8_t fire_check_counter = COUNTER_THRESHOLD; -uint8_t charge_counter = COUNTER_THRESHOLD; +uint8_t diag_counter = DIAG_CNT_THRESHOLD; +uint8_t fire_check_counter = DIAG_CNT_THRESHOLD; +uint8_t charge_counter = CHRG_CNT_THRESHOLD; //need more time to charge //====================================================================================== // FUNCTION DECLARATIONS @@ -219,7 +224,7 @@ void setCurrent(const uint8_t &, const float &); int main() { schedule_periodic(task("on_main"), 100); - schedule_periodic(task("on_ers"), 100); + schedule_periodic(task("on_ers"), TASK_ERS_PERIOD); task("uvhpu"); // GCS with terminal command `vmexec("uvhpu")` @@ -228,6 +233,7 @@ int main() m_squib_U(); m_pyro_U(); + m_ERS_fire(); m_ERS_block(); m_ERS_launch(); @@ -430,49 +436,56 @@ EXPORT void on_ers() break; } case ERS_State::DISARMED_LOOP: { - float squib_U = m_squib_U::value() * MULT_SQUIB_U_DIAG; - float pyro_U = m_pyro_U::value() * MULT_PIRO_U_DIAG; - float squib_R = (squib_U / 200.0f) / (pyro_U / 5600.0f) - R_WIRES; - - if (squib_R < R_SQUIB_MIN || squib_R > R_SQUIB_MAX) { - diag_counter--; - if (diag_counter == 0) { - ers_state = ERS_State::ERROR; - printf("squib resistance out of range: %.2f", squib_R); + if (fire_check_done == false) { //fire check done once + if (m_ERS_fire::value() == false) { + m_ERS_fire::publish(true); + return; + //time to settle } - return; - } else { - diag_counter = COUNTER_THRESHOLD; - } - - if (pyro_U > 3.3f) { - printf("pyro voltage too high: %.2f", pyro_U); - ers_state = ERS_State::ERROR; - return; - } - if (fire_check_done == false) { - m_ERS_fire::publish(true); - //delay here; + float pyro_U = m_pyro_U::value() * MULT_PIRO_U_DIAG; - if (pyro_U > 0.3f) { + if (pyro_U > MAX_VOLT_DIAG_FIRE) { fire_check_counter--; if (fire_check_counter == 0) { + printf("pyro voltage did not drop after fire check: %.2f", pyro_U); + printf("ERS ERROR"); ers_state = ERS_State::ERROR; - printf("pyro voltage did not drop after fire: %.2f", pyro_U); } return; } else { - fire_check_counter = COUNTER_THRESHOLD; + fire_check_counter = DIAG_CNT_THRESHOLD; } //everything ok, disarm fire and move on m_ERS_fire::publish(false); + //time to settle fire_check_done = true; + return; + } + + float squib_U = m_squib_U::value() * MULT_SQUIB_U_DIAG; + float pyro_U = m_pyro_U::value() * MULT_PIRO_U_DIAG; + float squib_R = (squib_U / 200.0f) / (pyro_U / 5600.0f) - R_WIRES; + + if (squib_R < R_SQUIB_MIN || squib_R > R_SQUIB_MAX || pyro_U > MAX_VOLT_DIAG) { + diag_counter--; + if (diag_counter == 0) { + printf("ERS ERROR"); + if (pyro_U > MAX_VOLT_DIAG) + printf("pyro voltage too high: %.2f", pyro_U); + else + printf("squib resistance out of range: %.2f", squib_R); + ers_state = ERS_State::ERROR; + } + return; + } else { + diag_counter = DIAG_CNT_THRESHOLD; } //all checks passed, wait for arm command if (m_ERS_block::value() == false) { ers_state = ERS_State::ARM_INIT; + printf("ARMED"); } break; } @@ -488,6 +501,8 @@ EXPORT void on_ers() case ERS_State::ARM_LOOP: { if (m_ERS_block::value() == true) { ers_state = ERS_State::DISARMED_INIT; + printf("DISARMED"); + m_ERS_charge::publish(false); //discharge capacitor return; } @@ -495,36 +510,40 @@ EXPORT void on_ers() float pyro_U = m_pyro_U::value() * MULT_PIRO_U_ARM; float squib_R = (squib_U / 200.0f) / (pyro_U / 5600.0f) - R_WIRES; - if (squib_R < R_SQUIB_MIN || squib_R > R_SQUIB_MAX) { + if (pyro_U < MIN_VOLT_CHARGED) { charge_counter--; if (charge_counter == 0) { - printf("squib resistance out of range: %.2f", squib_R); + printf("pyro voltage too low: %.2f", pyro_U); + printf("ERS ERROR"); } } else { - charge_counter = COUNTER_THRESHOLD; + charge_counter = CHRG_CNT_THRESHOLD; } - if (pyro_U < 8.0f) { //already stabilized at this point - printf("pyro voltage too low: %.2f", pyro_U); + if (squib_R < R_SQUIB_MIN || squib_R > R_SQUIB_MAX) { //already stabilized at this point + printf("squib resistance out of range: %.2f", squib_R); } if (m_ERS_launch::value() == true) { m_ERS_fire::publish(true); ers_state = ERS_State::FIRED; + printf("ERS FIRED"); } break; } case ERS_State::FIRED: { - printf("ERS FIRED"); m_ERS_status::publish(false); //turn arm LED off - m_ERS_charge::publish(false); //discharge capacitor - m_ERS_fire::publish(false); //ensure fire is off + + if (m_ERS_block::value() == true) { //reset ERS state if needed by blocking ers + ers_state = ERS_State::DISARMED_INIT; + printf("DISARMED"); + m_ERS_charge::publish(false); //discharge capacitor + } break; } case ERS_State::ERROR: { - printf("ERS ERROR"); m_ERS_fire::publish(false); //ensure fire is off m_ERS_diag::publish(false); //turn off diag voltage m_ERS_status::publish(false); //turn arm LED off From f0a9f2e6b3ab5d0414ab538c8969c8daa328e9e4 Mon Sep 17 00:00:00 2001 From: Eugene Shilin Date: Wed, 13 May 2026 11:29:56 +0300 Subject: [PATCH 07/20] anti stuck logic for RPM --- drivers/subsystems-UVH25/NAV_UVH25.cpp | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) diff --git a/drivers/subsystems-UVH25/NAV_UVH25.cpp b/drivers/subsystems-UVH25/NAV_UVH25.cpp index 9436370..c5cda52 100644 --- a/drivers/subsystems-UVH25/NAV_UVH25.cpp +++ b/drivers/subsystems-UVH25/NAV_UVH25.cpp @@ -195,6 +195,11 @@ struct UVHPU // ESC Handler Data ESC_VCP_Data esc_data; uint8_t esc_tbuf[PACK_SIZE_ESC]{}; +// anti-stuck rpm logic variables +float rpm_prev = 0.0f; +uint8_t same_counter = 0; +#define SAME_LIMIT 30 //3 sec at 100ms interval +#define MIN_RPM_CHECK 500.f //reset stuck rpm only below this value // VESC Tail Data VESC_CAN_Data tail_data{}; @@ -565,6 +570,19 @@ EXPORT void on_main() m_eng_volt::publish((float) esc_data.voltage); m_eng_current::publish((float) esc_data.current); m_eng_rpm::publish((uint32_t) esc_data.rpm); + + //RPM anti-stuck logic: if RPM is the same for a long time and less than 500, set it to 0 + float rpm_main = m_rpm::value(); + if (rpm_main == rpm_prev) { + same_counter++; + if (same_counter >= SAME_LIMIT && rpm_main < MIN_RPM_CHECK) { + m_rpm::publish(0.0f); + same_counter = 0; + } + } else { + rpm_prev = rpm_main; + same_counter = 0; + } } //====================================================================================== From 86a23f0c22701fd6b4dac044d658a6c3072cd56f Mon Sep 17 00:00:00 2001 From: Eugene Shilin Date: Wed, 13 May 2026 12:35:49 +0300 Subject: [PATCH 08/20] added ERS status variable --- drivers/subsystems-UVH25/NAV_UVH25.cpp | 31 +++++++++++++++----------- 1 file changed, 18 insertions(+), 13 deletions(-) diff --git a/drivers/subsystems-UVH25/NAV_UVH25.cpp b/drivers/subsystems-UVH25/NAV_UVH25.cpp index c5cda52..c01cb9d 100644 --- a/drivers/subsystems-UVH25/NAV_UVH25.cpp +++ b/drivers/subsystems-UVH25/NAV_UVH25.cpp @@ -109,7 +109,8 @@ using m_squib_U = Mandala; using m_ERS_fire = Mandala; using m_ERS_charge = Mandala; using m_ERS_diag = Mandala; -using m_ERS_status = Mandala; +using m_ERS_LED = Mandala; +using m_ERS_status = Mandala; //====================================================================================== // DATA STRUCTURES @@ -242,7 +243,8 @@ int main() m_ERS_block(); m_ERS_launch(); - m_ERS_block::publish(true); + m_ERS_block::publish(true); //disarm on start + m_ERS_status::publish((uint32_t) 2); //set status disarmed receive(PORT_ID_ESC, "esc_handler"); receive(PORT_ID_CAN, "on_serial"); @@ -432,9 +434,10 @@ EXPORT void on_ers() switch (ers_state) { case ERS_State::DISARMED_INIT: { - m_ERS_status::publish(false); //turn LED off - m_ERS_charge::publish(false); //discharge capacitor - m_ERS_fire::publish(false); //ensure fire is off + m_ERS_LED::publish(false); //turn LED off + m_ERS_charge::publish(false); //discharge capacitor + m_ERS_fire::publish(false); //ensure fire is off + m_ERS_status::publish((uint32_t) 2); //set status disarmed m_ERS_diag::publish(true); //turn on diag voltage ers_state = ERS_State::DISARMED_LOOP; @@ -495,9 +498,10 @@ EXPORT void on_ers() break; } case ERS_State::ARM_INIT: { - m_ERS_diag::publish(false); //ensure diag voltage is off - m_ERS_status::publish(true); //turn LED on - m_ERS_charge::publish(true); //charge capacitor + m_ERS_diag::publish(false); //ensure diag voltage is off + m_ERS_LED::publish(true); //turn LED on + m_ERS_charge::publish(true); //charge capacitor + m_ERS_status::publish((uint32_t) 1); //set status ok ers_state = ERS_State::ARM_LOOP; break; @@ -539,7 +543,7 @@ EXPORT void on_ers() } case ERS_State::FIRED: { - m_ERS_status::publish(false); //turn arm LED off + m_ERS_LED::publish(false); //turn arm LED off if (m_ERS_block::value() == true) { //reset ERS state if needed by blocking ers ers_state = ERS_State::DISARMED_INIT; @@ -549,10 +553,11 @@ EXPORT void on_ers() break; } case ERS_State::ERROR: { - m_ERS_fire::publish(false); //ensure fire is off - m_ERS_diag::publish(false); //turn off diag voltage - m_ERS_status::publish(false); //turn arm LED off - m_ERS_charge::publish(false); //discharge capacitor + m_ERS_fire::publish(false); //ensure fire is off + m_ERS_diag::publish(false); //turn off diag voltage + m_ERS_LED::publish(false); //turn arm LED off + m_ERS_charge::publish(false); //discharge capacitor + m_ERS_status::publish((uint32_t) 4); //set status failure break; } From 891c44b076a433600088bca502ce9389f77099af Mon Sep 17 00:00:00 2001 From: Eugene Shilin Date: Mon, 25 May 2026 18:20:37 +0300 Subject: [PATCH 09/20] MultiCell support --- drivers/subsystems-UVH25/NAV_UVH25.cpp | 100 ++++++++++++++++++++++++- 1 file changed, 99 insertions(+), 1 deletion(-) diff --git a/drivers/subsystems-UVH25/NAV_UVH25.cpp b/drivers/subsystems-UVH25/NAV_UVH25.cpp index c01cb9d..3512545 100644 --- a/drivers/subsystems-UVH25/NAV_UVH25.cpp +++ b/drivers/subsystems-UVH25/NAV_UVH25.cpp @@ -58,6 +58,13 @@ const uint8_t PACK_SIZE_ESC{10}; #define UVHPU_PACK6 UVHPU_ID + 6 #define UVHPU_PACK7 UVHPU_ID + 7 +//MCELL +#define MCELL_ID 0x0100 +#define MCELL_PACK1 MCELL_ID + 1 +#define MCELL_PACK2 MCELL_ID + 2 +#define MCELL_PACK3 MCELL_ID + 3 +#define MCELL_PACK4 MCELL_ID + 4 + // Altitude (AGL) Sensor #define AGL_CAN_ID 0x00090002 @@ -98,6 +105,14 @@ using m_eng_volt = Mandala; using m_eng_current = Mandala; using m_eng_rpm = Mandala; +// Mcell +using m_mcel_vbat = Mandala; +using m_mcel_tbat = Mandala; +using m_mcel_status = Mandala; +//using m_cell_tpcb = Mandala< +//vcl_min +//vcl_max + // Altitude (AGL) using m_agl = Mandala; @@ -144,8 +159,19 @@ struct VESC_CAN_Data uint32_t tacho; // STATUS_MSG_5 }; -// UVHPU Data Structure +// MCELL #pragma pack(1) +struct MCELL +{ + float v_bat; + float t_bat; + float t_pcb; + uint8_t status; + int16_t cell[12] = {}; + float cell_volt(uint8_t cell_idx) { return cell[cell_idx] / 1000.f; }; +}; + +// UVHPU Data Structure struct UVHPU { struct @@ -208,6 +234,9 @@ VESC_CAN_Data tail_data{}; // UVHPU Data UVHPU _uvhpu{}; +// MCELL data +MCELL _mcel{}; + // ERS State ERS_State ers_state = ERS_State::DISARMED_INIT; bool fire_check_done = false; @@ -232,6 +261,7 @@ int main() schedule_periodic(task("on_main"), 100); schedule_periodic(task("on_ers"), TASK_ERS_PERIOD); + task("mcell"); //GCS with terminal command `vmexec("mcell")` task("uvhpu"); // GCS with terminal command `vmexec("uvhpu")` m_eng_ctr(); @@ -309,6 +339,39 @@ EXPORT void esc_handler(const uint8_t *data, size_t size) esc_data.rpm = uint16_t((esc_tbuf[7] << 8) | (esc_tbuf[8])) * 100 / 7u; } +//====================================================================================== +// MultiCell PROCESSING - Battery monitor +//====================================================================================== + +void processMCELLPackage(const uint32_t &can_id, const uint8_t *data) +{ + switch (can_id) { + case MCELL_PACK1: { + _mcel.v_bat = (float) unpackInt16(data, 0) / 100.f; + _mcel.t_bat = (float) unpackInt16(data, 2) / 100.f; + _mcel.t_pcb = (float) unpackInt16(data, 4) / 100.f; + _mcel.status = data[7]; + + m_mcel_vbat::publish(_mcel.v_bat); + m_mcel_tbat::publish(_mcel.t_bat); + m_mcel_status::publish((uint32_t) _mcel.status); + break; + } + case MCELL_PACK2: { + memcpy(_mcel.cell, data, 8); + break; + } + case MCELL_PACK3: { + memcpy(_mcel.cell + 4, data, 8); + break; + } + case MCELL_PACK4: { + memcpy(_mcel.cell + 8, data, 8); + break; + } + } +} + //====================================================================================== // VESC CAN PROCESSING - TAIL MOTOR CONTROLLER //====================================================================================== @@ -621,6 +684,33 @@ EXPORT void uvhpu() printf("cbat_mod: %.2f", _uvhpu.MSG7.cbat_mod); } +//====================================================================================== +// MultiCell TERMINAL COMMAND - DATA DISPLAY +//====================================================================================== + +EXPORT void mcell() +{ + printf("v_bat: %.2f", _mcel.v_bat); + printf("t_bat: %.2f", _mcel.t_bat); + printf("t_pcb: %.2f", _mcel.t_pcb); + printf("state: %u", _mcel.status); + + printf("C[1]: %.2f", _mcel.cell_volt(0)); + printf("C[2]: %.2f", _mcel.cell_volt(1)); + printf("C[3]: %.2f", _mcel.cell_volt(2)); + printf("C[4]: %.2f", _mcel.cell_volt(3)); + + printf("C[5]: %.2f", _mcel.cell_volt(4)); + printf("C[6]: %.2f", _mcel.cell_volt(5)); + printf("C[7]: %.2f", _mcel.cell_volt(6)); + //printf("C[8]: %.2f", _mcel.cell_volt(7)); + + //printf("C[9] %.2f", _mcel.cell_volt(8)); + //printf("C[10] %.2f", _mcel.cell_volt(9)); + //printf("C[11] %.2f", _mcel.cell_volt(10)); + //printf("C[12] %.2f", _mcel.cell_volt(11)); +} + //====================================================================================== // CAN MESSAGE ROUTER - MAIN SERIAL HANDLER //====================================================================================== @@ -666,6 +756,14 @@ EXPORT void on_serial(const uint8_t *data, size_t size) // Process UVHPU Power Management (CAN ID 0xFFFF) switch (can_id & 0xFFFF) { + case MCELL_PACK1: + case MCELL_PACK2: + case MCELL_PACK3: + case MCELL_PACK4: { + //printf("mcell %x", can_id); + processMCELLPackage(can_id, can_data); + break; + } case UVHPU_PACK1: case UVHPU_PACK2: case UVHPU_PACK3: From e438eae24b711ff2da85be21b7cd8035c7b8f1a0 Mon Sep 17 00:00:00 2001 From: Eugene Shilin Date: Tue, 26 May 2026 17:20:10 +0300 Subject: [PATCH 10/20] mcell expanded --- drivers/subsystems-UVH25/NAV_UVH25.cpp | 49 ++++++++++++++++++-------- 1 file changed, 34 insertions(+), 15 deletions(-) diff --git a/drivers/subsystems-UVH25/NAV_UVH25.cpp b/drivers/subsystems-UVH25/NAV_UVH25.cpp index 3512545..97ba049 100644 --- a/drivers/subsystems-UVH25/NAV_UVH25.cpp +++ b/drivers/subsystems-UVH25/NAV_UVH25.cpp @@ -64,6 +64,9 @@ const uint8_t PACK_SIZE_ESC{10}; #define MCELL_PACK2 MCELL_ID + 2 #define MCELL_PACK3 MCELL_ID + 3 #define MCELL_PACK4 MCELL_ID + 4 +#define MCELL_PACK5 MCELL_ID + 5 +#define MCELL_PACK6 MCELL_ID + 6 +#define MCELL_PACK7 MCELL_ID + 7 // Altitude (AGL) Sensor #define AGL_CAN_ID 0x00090002 @@ -106,12 +109,12 @@ using m_eng_current = Mandala; using m_eng_rpm = Mandala; // Mcell -using m_mcel_vbat = Mandala; -using m_mcel_tbat = Mandala; -using m_mcel_status = Mandala; -//using m_cell_tpcb = Mandala< -//vcl_min -//vcl_max +using m_mcell_vbat = Mandala; +using m_mcell_tbat = Mandala; +using m_mcell_status = Mandala; +using m_mcell_tpcb = Mandala; +//vcl_min = Mandala; +//vcl_max = Mandala; // Altitude (AGL) using m_agl = Mandala; @@ -352,9 +355,10 @@ void processMCELLPackage(const uint32_t &can_id, const uint8_t *data) _mcel.t_pcb = (float) unpackInt16(data, 4) / 100.f; _mcel.status = data[7]; - m_mcel_vbat::publish(_mcel.v_bat); - m_mcel_tbat::publish(_mcel.t_bat); - m_mcel_status::publish((uint32_t) _mcel.status); + m_mcell_vbat::publish(_mcel.v_bat); + m_mcell_tbat::publish(_mcel.t_bat); + m_mcell_status::publish((uint32_t) _mcel.status); + m_mcell_tpcb::publish(_mcel.t_pcb); break; } case MCELL_PACK2: { @@ -369,6 +373,18 @@ void processMCELLPackage(const uint32_t &can_id, const uint8_t *data) memcpy(_mcel.cell + 8, data, 8); break; } + case MCELL_PACK5: { + memcpy(_mcel.cell + 12, data, 8); + break; + } + case MCELL_PACK6: { + memcpy(_mcel.cell + 16, data, 8); + break; + } + case MCELL_PACK7: { + memcpy(_mcel.cell + 20, data, 8); + break; + } } } @@ -703,12 +719,12 @@ EXPORT void mcell() printf("C[5]: %.2f", _mcel.cell_volt(4)); printf("C[6]: %.2f", _mcel.cell_volt(5)); printf("C[7]: %.2f", _mcel.cell_volt(6)); - //printf("C[8]: %.2f", _mcel.cell_volt(7)); + printf("C[8]: %.2f", _mcel.cell_volt(7)); - //printf("C[9] %.2f", _mcel.cell_volt(8)); - //printf("C[10] %.2f", _mcel.cell_volt(9)); - //printf("C[11] %.2f", _mcel.cell_volt(10)); - //printf("C[12] %.2f", _mcel.cell_volt(11)); + printf("C[9] %.2f", _mcel.cell_volt(8)); + printf("C[10] %.2f", _mcel.cell_volt(9)); + printf("C[11] %.2f", _mcel.cell_volt(10)); + printf("C[12] %.2f", _mcel.cell_volt(11)); } //====================================================================================== @@ -759,7 +775,10 @@ EXPORT void on_serial(const uint8_t *data, size_t size) case MCELL_PACK1: case MCELL_PACK2: case MCELL_PACK3: - case MCELL_PACK4: { + case MCELL_PACK4: + case MCELL_PACK5: + case MCELL_PACK6: + case MCELL_PACK7: { //printf("mcell %x", can_id); processMCELLPackage(can_id, can_data); break; From 0cb405a4f508c50ca2f8f6be9fa70adb9ba8e51b Mon Sep 17 00:00:00 2001 From: Eugene Shilin Date: Tue, 26 May 2026 17:29:45 +0300 Subject: [PATCH 11/20] user variables changed --- drivers/subsystems-UVH25/NAV_UVH25.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/drivers/subsystems-UVH25/NAV_UVH25.cpp b/drivers/subsystems-UVH25/NAV_UVH25.cpp index 97ba049..62e317c 100644 --- a/drivers/subsystems-UVH25/NAV_UVH25.cpp +++ b/drivers/subsystems-UVH25/NAV_UVH25.cpp @@ -94,11 +94,11 @@ using m_vesc_tail_current = Mandala; using m_vesc_tail_duty = Mandala; using m_vesc_tail_temp_fet = Mandala; using m_vesc_tail_temp_motor = Mandala; -using m_vesc_tail_curr_in = Mandala; +using m_vesc_tail_curr_in = Mandala; // UVH Power Management Unit -using m_uvhpy_status = Mandala; -using m_uvhpy_ibat_filt = Mandala; +using m_uvhpy_status = Mandala; +//using m_uvhpy_ibat_filt = Mandala; // Engine Parameters using m_eng_ctr = Mandala; @@ -110,11 +110,11 @@ using m_eng_rpm = Mandala; // Mcell using m_mcell_vbat = Mandala; -using m_mcell_tbat = Mandala; -using m_mcell_status = Mandala; using m_mcell_tpcb = Mandala; -//vcl_min = Mandala; +using m_mcell_tbat = Mandala; //vcl_max = Mandala; +//vcl_min = Mandala; +using m_mcell_status = Mandala; // Altitude (AGL) using m_agl = Mandala; @@ -122,7 +122,7 @@ using m_agl = Mandala; //ERS using m_ERS_block = Mandala; using m_ERS_launch = Mandala; -using m_pyro_U = Mandala; +using m_pyro_U = Mandala; using m_squib_U = Mandala; using m_ERS_fire = Mandala; using m_ERS_charge = Mandala; @@ -485,7 +485,7 @@ void processUVHPUackage(const uint32_t &can_id, const uint8_t *data) } case UVHPU_PACK5: { memcpy(&_uvhpu.MSG5.ibat_filt, data, 8); - m_uvhpy_ibat_filt::publish(_uvhpu.MSG5.ibat_filt); + //m_uvhpy_ibat_filt::publish(_uvhpu.MSG5.ibat_filt); break; } case UVHPU_PACK6: { From fbfeb72225af25fb5b1d908c8b751fe8e06ca004 Mon Sep 17 00:00:00 2001 From: Eugene Shilin Date: Tue, 26 May 2026 17:33:49 +0300 Subject: [PATCH 12/20] power unit data process --- drivers/subsystems-UVH25/NAV_UVH25.cpp | 20 +++++++++++++++----- 1 file changed, 15 insertions(+), 5 deletions(-) diff --git a/drivers/subsystems-UVH25/NAV_UVH25.cpp b/drivers/subsystems-UVH25/NAV_UVH25.cpp index 62e317c..b2d1461 100644 --- a/drivers/subsystems-UVH25/NAV_UVH25.cpp +++ b/drivers/subsystems-UVH25/NAV_UVH25.cpp @@ -96,9 +96,14 @@ using m_vesc_tail_temp_fet = Mandala; using m_vesc_tail_temp_motor = Mandala; using m_vesc_tail_curr_in = Mandala; -// UVH Power Management Unit -using m_uvhpy_status = Mandala; -//using m_uvhpy_ibat_filt = Mandala; +//Uvhpu +using m_uvhpu_vbat = Mandala; +using m_uvhpu_pbat = Mandala; +using m_uvhpu_status = Mandala; +using m_uvhpu_cbat = Mandala; +using m_uvhpu_ebat = Mandala; +using m_uvhpu_ibat = Mandala; +using m_uvhpu_tbat = Mandala; // Engine Parameters using m_eng_ctr = Mandala; @@ -464,6 +469,8 @@ void processUVHPUackage(const uint32_t &can_id, const uint8_t *data) _uvhpu.MSG1.vbat = (float) unpackInt16(data, 0) / 100.f; memcpy(&_uvhpu.MSG1.ibat, data + 2, 4); _uvhpu.MSG1.imon = (float) unpackInt16(data, 6) / 100.f; + m_uvhpu_vbat::publish(_uvhpu.MSG1.vbat); + m_uvhpu_ibat::publish(_uvhpu.MSG1.ibat); break; } case UVHPU_PACK2: { @@ -472,11 +479,15 @@ void processUVHPUackage(const uint32_t &can_id, const uint8_t *data) _uvhpu.MSG2.pbat = (float) unpackInt16(data, 4); _uvhpu.MSG2.status = data[7]; - m_uvhpy_status::publish(_uvhpu.MSG2.status); + m_uvhpu_status::publish(_uvhpu.MSG2.status); + m_uvhpu_pbat::publish(_uvhpu.MSG2.pbat); + m_uvhpu_tbat::publish(_uvhpu.MSG2.tbat); break; } case UVHPU_PACK3: { memcpy(&_uvhpu.MSG3.cbat, data, 8); + m_uvhpu_cbat::publish(_uvhpu.MSG3.cbat); + m_uvhpu_ebat::publish(_uvhpu.MSG3.ebat); break; } case UVHPU_PACK4: { @@ -485,7 +496,6 @@ void processUVHPUackage(const uint32_t &can_id, const uint8_t *data) } case UVHPU_PACK5: { memcpy(&_uvhpu.MSG5.ibat_filt, data, 8); - //m_uvhpy_ibat_filt::publish(_uvhpu.MSG5.ibat_filt); break; } case UVHPU_PACK6: { From e9a37490fa412b4a75ad58adb011ade7dd90b945 Mon Sep 17 00:00:00 2001 From: Eugene Shilin Date: Tue, 26 May 2026 17:49:15 +0300 Subject: [PATCH 13/20] v cell min and v cell max calculation --- drivers/subsystems-UVH25/NAV_UVH25.cpp | 26 ++++++++++++++++++++++++-- 1 file changed, 24 insertions(+), 2 deletions(-) diff --git a/drivers/subsystems-UVH25/NAV_UVH25.cpp b/drivers/subsystems-UVH25/NAV_UVH25.cpp index b2d1461..e8397dc 100644 --- a/drivers/subsystems-UVH25/NAV_UVH25.cpp +++ b/drivers/subsystems-UVH25/NAV_UVH25.cpp @@ -117,8 +117,8 @@ using m_eng_rpm = Mandala; using m_mcell_vbat = Mandala; using m_mcell_tpcb = Mandala; using m_mcell_tbat = Mandala; -//vcl_max = Mandala; -//vcl_min = Mandala; +using m_mcell_vcl_max = Mandala; +using m_mcell_vcl_min = Mandala; using m_mcell_status = Mandala; // Altitude (AGL) @@ -665,6 +665,28 @@ EXPORT void on_main() m_eng_current::publish((float) esc_data.current); m_eng_rpm::publish((uint32_t) esc_data.rpm); + // Calculate min and max cell voltages from MCELL data + float vcl_max = -1.0f; + float vcl_min = 1000.0f; + + for (uint8_t i = 0; i < 12; i++) { + float cell_voltage = _mcel.cell_volt(i); + if (cell_voltage > 0) { // Only consider valid readings + if (cell_voltage > vcl_max) { + vcl_max = cell_voltage; + } + if (cell_voltage < vcl_min) { + vcl_min = cell_voltage; + } + } + } + + // Publish min/max values if valid readings exist + if (vcl_max > 0 && vcl_min < 1000.0f) { + m_mcell_vcl_max::publish(vcl_max); + m_mcell_vcl_min::publish(vcl_min); + } + //RPM anti-stuck logic: if RPM is the same for a long time and less than 500, set it to 0 float rpm_main = m_rpm::value(); if (rpm_main == rpm_prev) { From 5684e29cbd2edfc1772c62f865d2e9ff403b7847 Mon Sep 17 00:00:00 2001 From: Eugene Shilin Date: Wed, 27 May 2026 11:48:38 +0300 Subject: [PATCH 14/20] rotor rpm variable changed --- drivers/subsystems-UVH25/NAV_UVH25.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/drivers/subsystems-UVH25/NAV_UVH25.cpp b/drivers/subsystems-UVH25/NAV_UVH25.cpp index e8397dc..7c9280a 100644 --- a/drivers/subsystems-UVH25/NAV_UVH25.cpp +++ b/drivers/subsystems-UVH25/NAV_UVH25.cpp @@ -107,7 +107,7 @@ using m_uvhpu_tbat = Mandala; // Engine Parameters using m_eng_ctr = Mandala; -using m_rpm = Mandala; +using m_rotor_rpm = Mandala; using m_eng_temp = Mandala; using m_eng_volt = Mandala; using m_eng_current = Mandala; @@ -273,7 +273,7 @@ int main() task("uvhpu"); // GCS with terminal command `vmexec("uvhpu")` m_eng_ctr(); - m_rpm(); + m_rotor_rpm(); m_squib_U(); m_pyro_U(); @@ -688,11 +688,11 @@ EXPORT void on_main() } //RPM anti-stuck logic: if RPM is the same for a long time and less than 500, set it to 0 - float rpm_main = m_rpm::value(); + float rpm_main = m_rotor_rpm::value(); if (rpm_main == rpm_prev) { same_counter++; if (same_counter >= SAME_LIMIT && rpm_main < MIN_RPM_CHECK) { - m_rpm::publish(0.0f); + m_rotor_rpm::publish(0.0f); same_counter = 0; } } else { From 499e1dc1d4da317fd06b6eece27002d2bd7377b9 Mon Sep 17 00:00:00 2001 From: Eugene Shilin Date: Thu, 28 May 2026 15:50:57 +0300 Subject: [PATCH 15/20] pu hold mechanism --- drivers/subsystems-UVH25/NAV_UVH25.cpp | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/drivers/subsystems-UVH25/NAV_UVH25.cpp b/drivers/subsystems-UVH25/NAV_UVH25.cpp index 7c9280a..fbb8c00 100644 --- a/drivers/subsystems-UVH25/NAV_UVH25.cpp +++ b/drivers/subsystems-UVH25/NAV_UVH25.cpp @@ -104,6 +104,8 @@ using m_uvhpu_cbat = Mandala; using m_uvhpu_ebat = Mandala; using m_uvhpu_ibat = Mandala; using m_uvhpu_tbat = Mandala; +using m_uvhpu_hold = Mandala; +using m_procedure = Mandala; // Engine Parameters using m_eng_ctr = Mandala; @@ -274,6 +276,7 @@ int main() m_eng_ctr(); m_rotor_rpm(); + m_procedure(); m_squib_U(); m_pyro_U(); @@ -699,6 +702,13 @@ EXPORT void on_main() rpm_prev = rpm_main; same_counter = 0; } + + //pu hold + if (m_procedure::value() == (uint32_t) mandala::proc_mode_TAXI) { //only in taxi mode + m_uvhpu_hold::publish(false); + } else { + m_uvhpu_hold::publish(true); + } } //====================================================================================== From 709655777a51feacd2b9f7f8a886b579db1c8fad Mon Sep 17 00:00:00 2001 From: Eugene Shilin Date: Fri, 29 May 2026 14:25:24 +0300 Subject: [PATCH 16/20] motor divider moved to define --- drivers/subsystems-UVH25/NAV_UVH25.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/drivers/subsystems-UVH25/NAV_UVH25.cpp b/drivers/subsystems-UVH25/NAV_UVH25.cpp index fbb8c00..8c2f974 100644 --- a/drivers/subsystems-UVH25/NAV_UVH25.cpp +++ b/drivers/subsystems-UVH25/NAV_UVH25.cpp @@ -42,6 +42,7 @@ const uint8_t PACK_SIZE_ESC{10}; #define STATUS_MSG_3 0x0F #define STATUS_MSG_4 0x10 #define STATUS_MSG_5 0x1B +#define ERPM_DIVIDER 14.f //amount of magnets divided by 2 // VESC CAN Packets #define CAN_PACKET_SET_CURRENT 1 @@ -802,7 +803,7 @@ EXPORT void on_serial(const uint8_t *data, size_t size) uint16_t msg_id = (can_id >> 8) & 0xFF; processVESCPackage(msg_id, can_data, &tail_data); - m_vesc_tail_rpm::publish((float) tail_data.rpm / 11); + m_vesc_tail_rpm::publish((float) tail_data.rpm / ERPM_DIVIDER); m_vesc_tail_current::publish(tail_data.current); m_vesc_tail_duty::publish(tail_data.duty); m_vesc_tail_temp_fet::publish(tail_data.temp_fet); From be2f051d665986c147fcd222cc8b86332df44314 Mon Sep 17 00:00:00 2001 From: Eugene Shilin Date: Mon, 1 Jun 2026 17:27:30 +0300 Subject: [PATCH 17/20] mcell max, min, delta added --- drivers/subsystems-UVH25/NAV_UVH25.cpp | 35 ++++++++++---------------- 1 file changed, 13 insertions(+), 22 deletions(-) diff --git a/drivers/subsystems-UVH25/NAV_UVH25.cpp b/drivers/subsystems-UVH25/NAV_UVH25.cpp index 8c2f974..c4cfaf3 100644 --- a/drivers/subsystems-UVH25/NAV_UVH25.cpp +++ b/drivers/subsystems-UVH25/NAV_UVH25.cpp @@ -68,6 +68,7 @@ const uint8_t PACK_SIZE_ESC{10}; #define MCELL_PACK5 MCELL_ID + 5 #define MCELL_PACK6 MCELL_ID + 6 #define MCELL_PACK7 MCELL_ID + 7 +#define MCELL_PACK15 MCELL_ID + 15 // Altitude (AGL) Sensor #define AGL_CAN_ID 0x00090002 @@ -122,6 +123,7 @@ using m_mcell_tpcb = Mandala; using m_mcell_tbat = Mandala; using m_mcell_vcl_max = Mandala; using m_mcell_vcl_min = Mandala; +using m_mcell_delta = Mandala; using m_mcell_status = Mandala; // Altitude (AGL) @@ -394,6 +396,17 @@ void processMCELLPackage(const uint32_t &can_id, const uint8_t *data) memcpy(_mcel.cell + 20, data, 8); break; } + case MCELL_PACK15: { + float vmin; + float vmax; + memcpy(&vmin, data, 4); //0-3 bytes min + memcpy(&vmax, data + 4, 4); //4-7 bytes max + m_mcell_vcl_min::publish(vmin); + m_mcell_vcl_max::publish(vmax); + float delta = vmax - vmin; + m_mcell_delta::publish(delta); + break; + } } } @@ -669,28 +682,6 @@ EXPORT void on_main() m_eng_current::publish((float) esc_data.current); m_eng_rpm::publish((uint32_t) esc_data.rpm); - // Calculate min and max cell voltages from MCELL data - float vcl_max = -1.0f; - float vcl_min = 1000.0f; - - for (uint8_t i = 0; i < 12; i++) { - float cell_voltage = _mcel.cell_volt(i); - if (cell_voltage > 0) { // Only consider valid readings - if (cell_voltage > vcl_max) { - vcl_max = cell_voltage; - } - if (cell_voltage < vcl_min) { - vcl_min = cell_voltage; - } - } - } - - // Publish min/max values if valid readings exist - if (vcl_max > 0 && vcl_min < 1000.0f) { - m_mcell_vcl_max::publish(vcl_max); - m_mcell_vcl_min::publish(vcl_min); - } - //RPM anti-stuck logic: if RPM is the same for a long time and less than 500, set it to 0 float rpm_main = m_rotor_rpm::value(); if (rpm_main == rpm_prev) { From 910e4a9089dcdc7eadee58fae274ddfdfb49d2dc Mon Sep 17 00:00:00 2001 From: Eugene Shilin Date: Tue, 2 Jun 2026 12:34:15 +0300 Subject: [PATCH 18/20] vmax, vmin, delta restored --- drivers/subsystems-UVH25/NAV_UVH25.cpp | 36 +++++++++++++++++--------- 1 file changed, 24 insertions(+), 12 deletions(-) diff --git a/drivers/subsystems-UVH25/NAV_UVH25.cpp b/drivers/subsystems-UVH25/NAV_UVH25.cpp index c4cfaf3..1445f89 100644 --- a/drivers/subsystems-UVH25/NAV_UVH25.cpp +++ b/drivers/subsystems-UVH25/NAV_UVH25.cpp @@ -68,7 +68,6 @@ const uint8_t PACK_SIZE_ESC{10}; #define MCELL_PACK5 MCELL_ID + 5 #define MCELL_PACK6 MCELL_ID + 6 #define MCELL_PACK7 MCELL_ID + 7 -#define MCELL_PACK15 MCELL_ID + 15 // Altitude (AGL) Sensor #define AGL_CAN_ID 0x00090002 @@ -396,17 +395,6 @@ void processMCELLPackage(const uint32_t &can_id, const uint8_t *data) memcpy(_mcel.cell + 20, data, 8); break; } - case MCELL_PACK15: { - float vmin; - float vmax; - memcpy(&vmin, data, 4); //0-3 bytes min - memcpy(&vmax, data + 4, 4); //4-7 bytes max - m_mcell_vcl_min::publish(vmin); - m_mcell_vcl_max::publish(vmax); - float delta = vmax - vmin; - m_mcell_delta::publish(delta); - break; - } } } @@ -682,6 +670,30 @@ EXPORT void on_main() m_eng_current::publish((float) esc_data.current); m_eng_rpm::publish((uint32_t) esc_data.rpm); + // Calculate min and max cell voltages from MCELL data + float vcl_max = -1.0f; + float vcl_min = 1000.0f; + + for (uint8_t i = 0; i < 12; i++) { + float cell_voltage = _mcel.cell_volt(i); + if (cell_voltage > 0) { // Only consider valid readings + if (cell_voltage > vcl_max) { + vcl_max = cell_voltage; + } + if (cell_voltage < vcl_min) { + vcl_min = cell_voltage; + } + } + } + + // Publish min/max values if valid readings exist + if (vcl_max > 0 && vcl_min < 1000.0f) { + m_mcell_vcl_max::publish(vcl_max); + m_mcell_vcl_min::publish(vcl_min); + } + float delta = vcl_max - vcl_min; + m_mcell_delta::publish(delta); + //RPM anti-stuck logic: if RPM is the same for a long time and less than 500, set it to 0 float rpm_main = m_rotor_rpm::value(); if (rpm_main == rpm_prev) { From 4515cca6319306ddc53ae6786b9c9d7fc84f2652 Mon Sep 17 00:00:00 2001 From: Eugene Shilin Date: Tue, 25 Aug 2026 10:36:24 +0100 Subject: [PATCH 19/20] moved to heli folder and renamed --- .../subsystems-UVH25/NAV_UVH25.cpp => vehicles/heli/uvh25/nav.cpp | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename drivers/subsystems-UVH25/NAV_UVH25.cpp => vehicles/heli/uvh25/nav.cpp (100%) diff --git a/drivers/subsystems-UVH25/NAV_UVH25.cpp b/vehicles/heli/uvh25/nav.cpp similarity index 100% rename from drivers/subsystems-UVH25/NAV_UVH25.cpp rename to vehicles/heli/uvh25/nav.cpp From 1f413ff668bfd06392da32e57af4a693fd89cd95 Mon Sep 17 00:00:00 2001 From: Eugene Shilin Date: Tue, 25 Aug 2026 10:40:48 +0100 Subject: [PATCH 20/20] script updated --- vehicles/heli/uvh25/nav.cpp | 570 ++++++++++++++++-------------------- 1 file changed, 258 insertions(+), 312 deletions(-) diff --git a/vehicles/heli/uvh25/nav.cpp b/vehicles/heli/uvh25/nav.cpp index 1445f89..c4f0adc 100644 --- a/vehicles/heli/uvh25/nav.cpp +++ b/vehicles/heli/uvh25/nav.cpp @@ -5,13 +5,15 @@ //====================================================================================== // Communication Ports -const uint8_t PORT_ID_CAN{1}; -const uint8_t PORT_ID_ESC{50}; +const uint8_t PORT_ID_CAN{11}; +const uint8_t PORT_ID_ESC{12}; // Protocol Package Sizes -const uint8_t PACK_SIZE_CAN{12}; const uint8_t PACK_SIZE_ESC{10}; +constexpr const uint16_t TASK_MAIN_PERIOD{100}; //ms +constexpr const uint16_t TASK_ERS_PERIOD{100}; //ms + //====================================================================================== // ERS CONSTANTS //====================================================================================== @@ -26,7 +28,6 @@ const uint8_t PACK_SIZE_ESC{10}; #define MULT_SQUIB_U_ARM 1.1f #define DIAG_CNT_THRESHOLD 10 //1s for 100ms period, ADC should be set for 10Hz! #define CHRG_CNT_THRESHOLD 30 //3s for 100ms period, ADC should be set for 10Hz! -#define TASK_ERS_PERIOD 100 //ms #define MIN_VOLT_CHARGED 8.0f #define MAX_VOLT_DIAG 3.3f #define MAX_VOLT_DIAG_FIRE 0.3f @@ -44,11 +45,6 @@ const uint8_t PACK_SIZE_ESC{10}; #define STATUS_MSG_5 0x1B #define ERPM_DIVIDER 14.f //amount of magnets divided by 2 -// VESC CAN Packets -#define CAN_PACKET_SET_CURRENT 1 -#define CAN_PACKET_SET_CURRENT_BRAKE 2 -#define CAN_PACKET_SET_RPM 3 - // UVHPU Power Management Unit #define UVHPU_ID 0x80 #define UVHPU_PACK1 UVHPU_ID + 1 @@ -85,63 +81,61 @@ enum class ERS_State { FIRED = 5 }; -//====================================================================================== -// TYPE ALIASES - MANDALA PARAMETERS -//====================================================================================== - -// VESC Tail Parameters -using m_vesc_tail_rpm = Mandala; -using m_vesc_tail_current = Mandala; -using m_vesc_tail_duty = Mandala; -using m_vesc_tail_temp_fet = Mandala; -using m_vesc_tail_temp_motor = Mandala; -using m_vesc_tail_curr_in = Mandala; - -//Uvhpu -using m_uvhpu_vbat = Mandala; -using m_uvhpu_pbat = Mandala; -using m_uvhpu_status = Mandala; -using m_uvhpu_cbat = Mandala; -using m_uvhpu_ebat = Mandala; -using m_uvhpu_ibat = Mandala; -using m_uvhpu_tbat = Mandala; -using m_uvhpu_hold = Mandala; -using m_procedure = Mandala; - -// Engine Parameters -using m_eng_ctr = Mandala; -using m_rotor_rpm = Mandala; -using m_eng_temp = Mandala; -using m_eng_volt = Mandala; -using m_eng_current = Mandala; -using m_eng_rpm = Mandala; - -// Mcell -using m_mcell_vbat = Mandala; -using m_mcell_tpcb = Mandala; -using m_mcell_tbat = Mandala; -using m_mcell_vcl_max = Mandala; -using m_mcell_vcl_min = Mandala; -using m_mcell_delta = Mandala; -using m_mcell_status = Mandala; - -// Altitude (AGL) +// vesc +using m_vesc_tail_rpm = Mandala; //+ +using m_vesc_tail_current = Mandala; //+ +using m_vesc_tail_duty = Mandala; //+ +using m_vesc_tail_temp_fet = Mandala; //+ +using m_vesc_tail_temp_motor = Mandala; //+ +using m_vesc_tail_curr_in = Mandala; //+ + +// uvhpu +using m_pu_vbat = Mandala; //+ +using m_pu_vsys = Mandala; //+ +using m_pu_ibat = Mandala; //+ +using m_pu_tbat = Mandala; //+ +using m_pu_pbat = Mandala; //+ +using m_pu_cbat = Mandala; //+ +using m_pu_ebat = Mandala; //+ +using m_pu_status = Mandala; //+ +using m_pu_hold = Mandala; //+ + +// mcell +using m_mcell_vbat = Mandala; //+ +using m_mcell_tpcb = Mandala; //+ +using m_mcell_tbat = Mandala; //+ +using m_mcell_max = Mandala; //+ +using m_mcell_min = Mandala; //+ +using m_mcell_status = Mandala; //+ + +// esc +using m_eng_temp = Mandala; //+ +using m_eng_volt = Mandala; //+ +using m_eng_current = Mandala; //+ +using m_eng_rpm = Mandala; //+ + +// agl using m_agl = Mandala; //ERS -using m_ERS_block = Mandala; -using m_ERS_launch = Mandala; -using m_pyro_U = Mandala; -using m_squib_U = Mandala; -using m_ERS_fire = Mandala; -using m_ERS_charge = Mandala; -using m_ERS_diag = Mandala; -using m_ERS_LED = Mandala; -using m_ERS_status = Mandala; +using m_ers_status = Mandala; //+ +using m_ers_block = Mandala; //+ +using m_ers_launch = Mandala; //+ -//====================================================================================== -// DATA STRUCTURES -//====================================================================================== +using m_ers_fire = Mandala; //+ +using m_ers_charge = Mandala; //+ +using m_ers_diag = Mandala; //+ +using m_ers_led = Mandala; //+ + +// using mandala variables +using m_mode = Mandala; +using m_rotor_rpm = Mandala; +using m_pyro_volt = Mandala; +using m_squib_volt = Mandala; + +// trim rudder +using m_reg_yaw = Mandala; +using m_trim_rudder_l = Mandala; // ESC VCP Data Structure struct ESC_VCP_Data @@ -183,7 +177,7 @@ struct MCELL float cell_volt(uint8_t cell_idx) { return cell[cell_idx] / 1000.f; }; }; -// UVHPU Data Structure +// UVHPU struct UVHPU { struct @@ -256,46 +250,54 @@ uint8_t diag_counter = DIAG_CNT_THRESHOLD; uint8_t fire_check_counter = DIAG_CNT_THRESHOLD; uint8_t charge_counter = CHRG_CNT_THRESHOLD; //need more time to charge -//====================================================================================== -// FUNCTION DECLARATIONS -//====================================================================================== +constexpr const uint16_t SERVO_TASK_MS{20}; +constexpr const float F = 3.f; +constexpr const float Ampl = 1.f; +constexpr const float w = 2.f * PI * F; +constexpr const float T = 1.f / F; +float test_servo = 0.f; -// CAN Control Functions -void setRPM(const uint8_t &, const int32_t &); -void setCurrent(const uint8_t &, const float &); - -//====================================================================================== -// MAIN ENTRY POINT -//====================================================================================== +using m_sin_test = Mandala; int main() { - schedule_periodic(task("on_main"), 100); + schedule_periodic(task("on_main"), TASK_MAIN_PERIOD); schedule_periodic(task("on_ers"), TASK_ERS_PERIOD); + //schedule_periodic(task("on_test_servo"), SERVO_TASK_MS); + task("mcell"); //GCS with terminal command `vmexec("mcell")` task("uvhpu"); // GCS with terminal command `vmexec("uvhpu")` - m_eng_ctr(); m_rotor_rpm(); - m_procedure(); + m_mode(); - m_squib_U(); - m_pyro_U(); - m_ERS_fire(); - m_ERS_block(); - m_ERS_launch(); + m_squib_volt(); + m_pyro_volt(); + m_ers_fire(); + m_ers_block(); + m_ers_launch(); - m_ERS_block::publish(true); //disarm on start - m_ERS_status::publish((uint32_t) 2); //set status disarmed + m_reg_yaw(); + + m_ers_block::publish(true); //disarm on start + m_ers_status::publish((uint32_t) mandala::ers_status_disarmed); //set status disarmed receive(PORT_ID_ESC, "esc_handler"); - receive(PORT_ID_CAN, "on_serial"); + receive(PORT_ID_CAN, "can_handler"); } -//====================================================================================== -// UTILITY FUNCTIONS - CRC & SERIALIZATION -//====================================================================================== +EXPORT void on_test_servo() +{ + float ctrl = Ampl * sin(w * test_servo); + + test_servo += SERVO_TASK_MS / 1000.f; + if (test_servo >= T) { + test_servo = 0.f; + } + + m_sin_test::publish(ctrl); +} uint8_t update_crc8(uint8_t data, uint8_t crc) { @@ -329,33 +331,6 @@ int16_t unpackInt16(const uint8_t *data, uint8_t index) return (int16_t) (data[index] | (data[index + 1] << 8)); } -//====================================================================================== -// ESC HANDLER - VCP DATA PROCESSING -//====================================================================================== - -EXPORT void esc_handler(const uint8_t *data, size_t size) -{ - if (size != PACK_SIZE_ESC) { - return; - } - - memcpy(esc_tbuf, data, size); - - if (get_crc8(esc_tbuf, PACK_SIZE_ESC - 1) != esc_tbuf[PACK_SIZE_ESC - 1]) { - return; - } - - esc_data.temp = data[0]; - esc_data.voltage = float((esc_tbuf[1] << 8) | (esc_tbuf[2])) / 100.f; - esc_data.current = float((esc_tbuf[3] << 8) | (esc_tbuf[4])) / 100.f; - esc_data.consumption = uint16_t((esc_tbuf[5] << 8) | (esc_tbuf[6])); - esc_data.rpm = uint16_t((esc_tbuf[7] << 8) | (esc_tbuf[8])) * 100 / 7u; -} - -//====================================================================================== -// MultiCell PROCESSING - Battery monitor -//====================================================================================== - void processMCELLPackage(const uint32_t &can_id, const uint8_t *data) { switch (can_id) { @@ -398,10 +373,6 @@ void processMCELLPackage(const uint32_t &can_id, const uint8_t *data) } } -//====================================================================================== -// VESC CAN PROCESSING - TAIL MOTOR CONTROLLER -//====================================================================================== - void processVESCPackage(const uint32_t &msg_id, const uint8_t *data, VESC_CAN_Data *vesc_data) { switch (msg_id) { @@ -440,33 +411,6 @@ void processVESCPackage(const uint32_t &msg_id, const uint8_t *data, VESC_CAN_Da } } -void setRPM(const uint8_t &VECS_CAN_ID, const int32_t &val) -{ - uint8_t msg[4 + 4] = {}; // ext id + DATA - - msg[0] = VECS_CAN_ID; - msg[1] = CAN_PACKET_SET_RPM; - msg[3] |= 0x80; // IDE (bit 7) 1=ext,0=std - serializeInt(msg, 4, val); - send(PORT_ID_CAN, msg, 8, false); -} - -void setCurrent(const uint8_t &VECS_CAN_ID, const float &val) -{ - uint8_t msg[4 + 4] = {}; // ext id + DATA - int32_t current = int32_t(val * 1000); - - msg[0] = VECS_CAN_ID; - msg[1] = CAN_PACKET_SET_CURRENT; - msg[3] |= 0x80; // IDE (bit 7) 1=ext,0=std; - serializeInt(msg, 4, current); - send(PORT_ID_CAN, msg, 8, false); -} - -//====================================================================================== -// UVHPU PROCESSING - POWER MANAGEMENT UNIT -//====================================================================================== - void processUVHPUackage(const uint32_t &can_id, const uint8_t *data) { switch (can_id) { @@ -474,8 +418,9 @@ void processUVHPUackage(const uint32_t &can_id, const uint8_t *data) _uvhpu.MSG1.vbat = (float) unpackInt16(data, 0) / 100.f; memcpy(&_uvhpu.MSG1.ibat, data + 2, 4); _uvhpu.MSG1.imon = (float) unpackInt16(data, 6) / 100.f; - m_uvhpu_vbat::publish(_uvhpu.MSG1.vbat); - m_uvhpu_ibat::publish(_uvhpu.MSG1.ibat); + m_pu_vbat::publish(_uvhpu.MSG1.vbat); + m_pu_vsys::publish(_uvhpu.MSG1.vbat); + m_pu_ibat::publish(_uvhpu.MSG1.ibat); break; } case UVHPU_PACK2: { @@ -484,15 +429,15 @@ void processUVHPUackage(const uint32_t &can_id, const uint8_t *data) _uvhpu.MSG2.pbat = (float) unpackInt16(data, 4); _uvhpu.MSG2.status = data[7]; - m_uvhpu_status::publish(_uvhpu.MSG2.status); - m_uvhpu_pbat::publish(_uvhpu.MSG2.pbat); - m_uvhpu_tbat::publish(_uvhpu.MSG2.tbat); + m_pu_status::publish(_uvhpu.MSG2.status); + m_pu_pbat::publish(_uvhpu.MSG2.pbat); + m_pu_tbat::publish(_uvhpu.MSG2.tbat); break; } case UVHPU_PACK3: { memcpy(&_uvhpu.MSG3.cbat, data, 8); - m_uvhpu_cbat::publish(_uvhpu.MSG3.cbat); - m_uvhpu_ebat::publish(_uvhpu.MSG3.ebat); + m_pu_cbat::publish(_uvhpu.MSG3.cbat); + m_pu_ebat::publish(_uvhpu.MSG3.ebat); break; } case UVHPU_PACK4: { @@ -515,9 +460,62 @@ void processUVHPUackage(const uint32_t &can_id, const uint8_t *data) } } -//====================================================================================== -// ERS PERIODIC TASK -//====================================================================================== +EXPORT void on_main() +{ + if (m_reg_yaw::value() >= (uint32_t) mandala::reg_yaw_fixed) { + m_trim_rudder_l::publish(0.3f); + } else { + m_trim_rudder_l::publish(0.f); + } + + // Save ESC data to mandala + m_eng_temp::publish((uint32_t) esc_data.temp); + m_eng_volt::publish((float) esc_data.voltage); + m_eng_current::publish((float) esc_data.current); + m_eng_rpm::publish((uint32_t) esc_data.rpm); + + // Calculate min and max cell voltages from MCELL data + float vcl_max = -1.f; + float vcl_min = 1000.f; + + for (uint8_t i = 0; i < 12; i++) { + float cell_voltage = _mcel.cell_volt(i); + if (cell_voltage > 0) { // Only consider valid readings + if (cell_voltage > vcl_max) { + vcl_max = cell_voltage; + } + if (cell_voltage < vcl_min) { + vcl_min = cell_voltage; + } + } + } + + // Publish min/max values if valid readings exist + if (vcl_max > 0 && vcl_min < 1000.f) { + m_mcell_max::publish(vcl_max); + m_mcell_min::publish(vcl_min); + } + + //RPM anti-stuck logic: if RPM is the same for a long time and less than 500, set it to 0 + float rpm_main = m_rotor_rpm::value(); + if (rpm_main == rpm_prev) { + same_counter++; + if (same_counter >= SAME_LIMIT && rpm_main < MIN_RPM_CHECK) { + m_rotor_rpm::publish(0.f); + same_counter = 0; + } + } else { + rpm_prev = rpm_main; + same_counter = 0; + } + + //pu hold + if (m_mode::value() == (uint32_t) mandala::proc_mode_TAXI) { //only in taxi mode + m_pu_hold::publish(false); + } else { + m_pu_hold::publish(true); + } +} EXPORT void on_ers() { @@ -528,30 +526,30 @@ EXPORT void on_ers() switch (ers_state) { case ERS_State::DISARMED_INIT: { - m_ERS_LED::publish(false); //turn LED off - m_ERS_charge::publish(false); //discharge capacitor - m_ERS_fire::publish(false); //ensure fire is off - m_ERS_status::publish((uint32_t) 2); //set status disarmed + m_ers_led::publish(false); //turn LED off + m_ers_charge::publish(false); //discharge capacitor + m_ers_fire::publish(false); //ensure fire is off + m_ers_status::publish((uint32_t) mandala::ers_status_disarmed); //set status disarmed - m_ERS_diag::publish(true); //turn on diag voltage + m_ers_diag::publish(true); //turn on diag voltage ers_state = ERS_State::DISARMED_LOOP; break; } case ERS_State::DISARMED_LOOP: { if (fire_check_done == false) { //fire check done once - if (m_ERS_fire::value() == false) { - m_ERS_fire::publish(true); + if (m_ers_fire::value() == false) { + m_ers_fire::publish(true); return; //time to settle } - float pyro_U = m_pyro_U::value() * MULT_PIRO_U_DIAG; + float pyro_U = m_pyro_volt::value() * MULT_PIRO_U_DIAG; if (pyro_U > MAX_VOLT_DIAG_FIRE) { fire_check_counter--; if (fire_check_counter == 0) { - printf("pyro voltage did not drop after fire check: %.2f", pyro_U); - printf("ERS ERROR"); + printf("VM:pyro voltage did not drop after fire check: %.2f", pyro_U); + printf("VM:ERS ERROR"); ers_state = ERS_State::ERROR; } return; @@ -559,24 +557,24 @@ EXPORT void on_ers() fire_check_counter = DIAG_CNT_THRESHOLD; } //everything ok, disarm fire and move on - m_ERS_fire::publish(false); + m_ers_fire::publish(false); //time to settle fire_check_done = true; return; } - float squib_U = m_squib_U::value() * MULT_SQUIB_U_DIAG; - float pyro_U = m_pyro_U::value() * MULT_PIRO_U_DIAG; + float squib_U = m_squib_volt::value() * MULT_SQUIB_U_DIAG; + float pyro_U = m_pyro_volt::value() * MULT_PIRO_U_DIAG; float squib_R = (squib_U / 200.0f) / (pyro_U / 5600.0f) - R_WIRES; if (squib_R < R_SQUIB_MIN || squib_R > R_SQUIB_MAX || pyro_U > MAX_VOLT_DIAG) { diag_counter--; if (diag_counter == 0) { - printf("ERS ERROR"); + printf("VM:ERS ERROR"); if (pyro_U > MAX_VOLT_DIAG) - printf("pyro voltage too high: %.2f", pyro_U); + printf("VM:pyro voltage too high: %.2f", pyro_U); else - printf("squib resistance out of range: %.2f", squib_R); + printf("VM:squib resistance out of range: %.2f", squib_R); ers_state = ERS_State::ERROR; } return; @@ -585,202 +583,101 @@ EXPORT void on_ers() } //all checks passed, wait for arm command - if (m_ERS_block::value() == false) { + if (m_ers_block::value() == false) { ers_state = ERS_State::ARM_INIT; - printf("ARMED"); + printf("VM:ARMED"); } break; } case ERS_State::ARM_INIT: { - m_ERS_diag::publish(false); //ensure diag voltage is off - m_ERS_LED::publish(true); //turn LED on - m_ERS_charge::publish(true); //charge capacitor - m_ERS_status::publish((uint32_t) 1); //set status ok + m_ers_diag::publish(false); //ensure diag voltage is off + m_ers_led::publish(true); //turn LED on + m_ers_charge::publish(true); //charge capacitor + m_ers_status::publish((uint32_t) mandala::ers_status_ok); //set status ok ers_state = ERS_State::ARM_LOOP; break; } case ERS_State::ARM_LOOP: { - if (m_ERS_block::value() == true) { + if (m_ers_block::value() == true) { ers_state = ERS_State::DISARMED_INIT; - printf("DISARMED"); - m_ERS_charge::publish(false); //discharge capacitor + printf("VM:DISARMED"); + m_ers_charge::publish(false); //discharge capacitor return; } - float squib_U = m_squib_U::value() * MULT_SQUIB_U_ARM; //recalibrate after charging - float pyro_U = m_pyro_U::value() * MULT_PIRO_U_ARM; + float squib_U = m_squib_volt::value() * MULT_SQUIB_U_ARM; //recalibrate after charging + float pyro_U = m_pyro_volt::value() * MULT_PIRO_U_ARM; float squib_R = (squib_U / 200.0f) / (pyro_U / 5600.0f) - R_WIRES; if (pyro_U < MIN_VOLT_CHARGED) { charge_counter--; if (charge_counter == 0) { - printf("pyro voltage too low: %.2f", pyro_U); - printf("ERS ERROR"); + printf("VM:pyro voltage too low: %.2f", pyro_U); + printf("VM:ERS ERROR"); } } else { charge_counter = CHRG_CNT_THRESHOLD; } if (squib_R < R_SQUIB_MIN || squib_R > R_SQUIB_MAX) { //already stabilized at this point - printf("squib resistance out of range: %.2f", squib_R); + printf("VM:squib resistance out of range: %.2f", squib_R); } - if (m_ERS_launch::value() == true) { - m_ERS_fire::publish(true); + if (m_ers_launch::value() == true) { + m_ers_fire::publish(true); ers_state = ERS_State::FIRED; - printf("ERS FIRED"); + printf("VM:ERS FIRED"); } break; } case ERS_State::FIRED: { - m_ERS_LED::publish(false); //turn arm LED off + m_ers_led::publish(false); //turn arm LED off - if (m_ERS_block::value() == true) { //reset ERS state if needed by blocking ers + if (m_ers_block::value() == true) { //reset ERS state if needed by blocking ers ers_state = ERS_State::DISARMED_INIT; printf("DISARMED"); - m_ERS_charge::publish(false); //discharge capacitor + m_ers_charge::publish(false); //discharge capacitor } break; } case ERS_State::ERROR: { - m_ERS_fire::publish(false); //ensure fire is off - m_ERS_diag::publish(false); //turn off diag voltage - m_ERS_LED::publish(false); //turn arm LED off - m_ERS_charge::publish(false); //discharge capacitor - m_ERS_status::publish((uint32_t) 4); //set status failure - + m_ers_fire::publish(false); //ensure fire is off + m_ers_diag::publish(false); //turn off diag voltage + m_ers_led::publish(false); //turn arm LED off + m_ers_charge::publish(false); //discharge capacitor + m_ers_status::publish((uint32_t) mandala::ers_status_failure); //set status failure break; } } } -//====================================================================================== -// MAIN PERIODIC TASK - ESC DATA PUBLISHING -//====================================================================================== - -EXPORT void on_main() +EXPORT void esc_handler(const uint8_t *data, size_t size) { - // Save ESC data to mandala - m_eng_temp::publish((uint32_t) esc_data.temp); - m_eng_volt::publish((float) esc_data.voltage); - m_eng_current::publish((float) esc_data.current); - m_eng_rpm::publish((uint32_t) esc_data.rpm); - - // Calculate min and max cell voltages from MCELL data - float vcl_max = -1.0f; - float vcl_min = 1000.0f; - - for (uint8_t i = 0; i < 12; i++) { - float cell_voltage = _mcel.cell_volt(i); - if (cell_voltage > 0) { // Only consider valid readings - if (cell_voltage > vcl_max) { - vcl_max = cell_voltage; - } - if (cell_voltage < vcl_min) { - vcl_min = cell_voltage; - } - } - } - - // Publish min/max values if valid readings exist - if (vcl_max > 0 && vcl_min < 1000.0f) { - m_mcell_vcl_max::publish(vcl_max); - m_mcell_vcl_min::publish(vcl_min); + if (size != PACK_SIZE_ESC) { + return; } - float delta = vcl_max - vcl_min; - m_mcell_delta::publish(delta); - //RPM anti-stuck logic: if RPM is the same for a long time and less than 500, set it to 0 - float rpm_main = m_rotor_rpm::value(); - if (rpm_main == rpm_prev) { - same_counter++; - if (same_counter >= SAME_LIMIT && rpm_main < MIN_RPM_CHECK) { - m_rotor_rpm::publish(0.0f); - same_counter = 0; - } - } else { - rpm_prev = rpm_main; - same_counter = 0; - } + memcpy(esc_tbuf, data, size); - //pu hold - if (m_procedure::value() == (uint32_t) mandala::proc_mode_TAXI) { //only in taxi mode - m_uvhpu_hold::publish(false); - } else { - m_uvhpu_hold::publish(true); + if (get_crc8(esc_tbuf, PACK_SIZE_ESC - 1) != esc_tbuf[PACK_SIZE_ESC - 1]) { + return; } -} - -//====================================================================================== -// UVHPU TERMINAL COMMAND - DATA DISPLAY -//====================================================================================== - -EXPORT void uvhpu() -{ - printf("vbat: %.2f", _uvhpu.MSG1.vbat); - printf("ibat: %.2f", _uvhpu.MSG1.ibat); - printf("imon: %.2f", _uvhpu.MSG1.imon); - - printf("vout: %.2f", _uvhpu.MSG2.vout); - printf("tbat: %.2f", _uvhpu.MSG2.tbat); - printf("pbat: %.2f", _uvhpu.MSG2.pbat); - printf("status: %u", _uvhpu.MSG2.status); - printf("cbat: %.2f", _uvhpu.MSG3.cbat); - printf("ebat: %.2f", _uvhpu.MSG3.ebat); - - printf("res_bar: %.2f", _uvhpu.MSG4.res_bar); - printf("v_res: %.2f", _uvhpu.MSG4.v_res); - - printf("ibat_filt: %.2f", _uvhpu.MSG5.ibat_filt); - printf("vbat_filt: %.2f", _uvhpu.MSG5.vbat_filt); - - printf("cbat_res: %.2f", _uvhpu.MSG6.cbat_res); - printf("ebat_res: %.2f", _uvhpu.MSG6.ebat_res); - - printf("life_cycles: %u", _uvhpu.MSG7.life_cycles); - printf("cbat_mod: %.2f", _uvhpu.MSG7.cbat_mod); -} - -//====================================================================================== -// MultiCell TERMINAL COMMAND - DATA DISPLAY -//====================================================================================== - -EXPORT void mcell() -{ - printf("v_bat: %.2f", _mcel.v_bat); - printf("t_bat: %.2f", _mcel.t_bat); - printf("t_pcb: %.2f", _mcel.t_pcb); - printf("state: %u", _mcel.status); - - printf("C[1]: %.2f", _mcel.cell_volt(0)); - printf("C[2]: %.2f", _mcel.cell_volt(1)); - printf("C[3]: %.2f", _mcel.cell_volt(2)); - printf("C[4]: %.2f", _mcel.cell_volt(3)); - - printf("C[5]: %.2f", _mcel.cell_volt(4)); - printf("C[6]: %.2f", _mcel.cell_volt(5)); - printf("C[7]: %.2f", _mcel.cell_volt(6)); - printf("C[8]: %.2f", _mcel.cell_volt(7)); - - printf("C[9] %.2f", _mcel.cell_volt(8)); - printf("C[10] %.2f", _mcel.cell_volt(9)); - printf("C[11] %.2f", _mcel.cell_volt(10)); - printf("C[12] %.2f", _mcel.cell_volt(11)); + esc_data.temp = data[0]; + esc_data.voltage = float((esc_tbuf[1] << 8) | (esc_tbuf[2])) / 100.f; + esc_data.current = float((esc_tbuf[3] << 8) | (esc_tbuf[4])) / 100.f; + esc_data.consumption = uint16_t((esc_tbuf[5] << 8) | (esc_tbuf[6])); + esc_data.rpm = uint16_t((esc_tbuf[7] << 8) | (esc_tbuf[8])) * 100 / 7u; } -//====================================================================================== -// CAN MESSAGE ROUTER - MAIN SERIAL HANDLER -//====================================================================================== - -EXPORT void on_serial(const uint8_t *data, size_t size) +EXPORT void can_handler(const uint8_t *data, size_t size) { - if (size != PACK_SIZE_CAN) { - //return; + if (size < 4) { + return; } uint32_t can_id = (uint32_t) (data[0] + (data[1] << 8) + (data[2] << 16) + (data[3] << 24)); @@ -825,7 +722,6 @@ EXPORT void on_serial(const uint8_t *data, size_t size) case MCELL_PACK5: case MCELL_PACK6: case MCELL_PACK7: { - //printf("mcell %x", can_id); processMCELLPackage(can_id, can_data); break; } @@ -841,3 +737,53 @@ EXPORT void on_serial(const uint8_t *data, size_t size) } } } + +EXPORT void uvhpu() +{ + printf("vbat: %.2f", _uvhpu.MSG1.vbat); + printf("ibat: %.2f", _uvhpu.MSG1.ibat); + printf("imon: %.2f", _uvhpu.MSG1.imon); + + printf("vout: %.2f", _uvhpu.MSG2.vout); + printf("tbat: %.2f", _uvhpu.MSG2.tbat); + printf("pbat: %.2f", _uvhpu.MSG2.pbat); + printf("status: %u", _uvhpu.MSG2.status); + + printf("cbat: %.2f", _uvhpu.MSG3.cbat); + printf("ebat: %.2f", _uvhpu.MSG3.ebat); + + printf("res_bar: %.2f", _uvhpu.MSG4.res_bar); + printf("v_res: %.2f", _uvhpu.MSG4.v_res); + + printf("ibat_filt: %.2f", _uvhpu.MSG5.ibat_filt); + printf("vbat_filt: %.2f", _uvhpu.MSG5.vbat_filt); + + printf("cbat_res: %.2f", _uvhpu.MSG6.cbat_res); + printf("ebat_res: %.2f", _uvhpu.MSG6.ebat_res); + + printf("life_cycles: %u", _uvhpu.MSG7.life_cycles); + printf("cbat_mod: %.2f", _uvhpu.MSG7.cbat_mod); +} + +EXPORT void mcell() +{ + printf("v_bat: %.2f", _mcel.v_bat); + printf("t_bat: %.2f", _mcel.t_bat); + printf("t_pcb: %.2f", _mcel.t_pcb); + printf("state: %u", _mcel.status); + + printf("C[1]: %.2f", _mcel.cell_volt(0)); + printf("C[2]: %.2f", _mcel.cell_volt(1)); + printf("C[3]: %.2f", _mcel.cell_volt(2)); + printf("C[4]: %.2f", _mcel.cell_volt(3)); + + printf("C[5]: %.2f", _mcel.cell_volt(4)); + printf("C[6]: %.2f", _mcel.cell_volt(5)); + printf("C[7]: %.2f", _mcel.cell_volt(6)); + printf("C[8]: %.2f", _mcel.cell_volt(7)); + + printf("C[9] %.2f", _mcel.cell_volt(8)); + printf("C[10] %.2f", _mcel.cell_volt(9)); + printf("C[11] %.2f", _mcel.cell_volt(10)); + printf("C[12] %.2f", _mcel.cell_volt(11)); +}