From da82c1a987e8fe966f866354f0a5432d58c3c5fa Mon Sep 17 00:00:00 2001 From: Eugene Shilin Date: Fri, 20 Mar 2026 18:31:22 +0300 Subject: [PATCH 01/39] ecu protocol init --- vehicles/S1C/apx11/ECU.cpp | 100 +++++++++++++++++++++++++++++++++++++ 1 file changed, 100 insertions(+) create mode 100644 vehicles/S1C/apx11/ECU.cpp diff --git a/vehicles/S1C/apx11/ECU.cpp b/vehicles/S1C/apx11/ECU.cpp new file mode 100644 index 0000000..01b25f0 --- /dev/null +++ b/vehicles/S1C/apx11/ECU.cpp @@ -0,0 +1,100 @@ +#include + +const uint8_t PORT_ID_ECU{60}; +const uint8_t CAN_PACK_SIZE{13}; +const uint8_t TASK_ECU_MS{200}; + +const uint16_t CAN_BASE_ADDR{1520}; +const uint8_t OFFSET_CAN_ID_CURRENT_THROTTLE{71}; +const uint16_t CAN_ID_CURRENT_THROTTLE = CAN_BASE_ADDR + OFFSET_CAN_ID_CURRENT_THROTTLE; + +using m_oil_temp = Mandala; +using m_cool_temp = Mandala; +using m_oil_press = Mandala; +using m_egt_1 = Mandala; +using m_egt_2 = Mandala; +using m_egt_3 = Mandala; +using m_egt_4 = Mandala; + +int main() +{ + schedule_periodic(task("on_ecu"), TASK_ECU_MS); + receive(PORT_ID_ECU, "on_ecu_serial"); +} + +void setThrottleMS() +{ + uint8_t data[13]; + data[0] = CAN_ID_CURRENT_THROTTLE & 0xFF; + data[1] = (CAN_ID_CURRENT_THROTTLE >> 8) & 0xFF; + data[2] = (CAN_ID_CURRENT_THROTTLE >> 16) & 0xFF; + data[3] = (CAN_ID_CURRENT_THROTTLE >> 24) & 0xFF; + data[4] = 5; + + float ch_throttle = 0.0; + //printf("thr:%f/n", ch_throttle); + memcpy(&data[5], &ch_throttle, 4); + data[9] = 1; + send(PORT_ID_ECU, data, 10, false); +} + +EXPORT void on_ecu() +{ + setThrottleMS(); + + //Calc LAMBDA +} + +EXPORT void on_ecu_serial(const uint8_t *data, size_t size) +{ + printf("%d", size); + + if ((size > CAN_PACK_SIZE) || (size != (data[4] & 0x7F) + 5)) { + return; + } + + uint32_t can_id = (uint32_t) (data[0] + (data[1] << 8) + (data[2] << 16) + (data[3] << 24)); + //printf("can_id:%x\n", can_id); + + switch (can_id) { + case CAN_BASE_ADDR + 2: { + float param; + int16_t raw; + + raw = (int16_t) (data[9] << 8) + data[10]; + param = float(raw); + float oil_temp = (param / 10.0f - 32.0f) * 5.0f / 9.0f; + m_oil_temp::publish(oil_temp); // Oil temperature [deg] + + raw = (int16_t) (data[11] << 8) + data[12]; + param = float(raw); + float coolant_temp = (param / 10.0f - 32.0f) * 5.0f / 9.0f; + m_cool_temp::publish(coolant_temp); // Coolant temperature [deg] + } + + case CAN_BASE_ADDR + 15: { + int16_t raw = (int16_t) (data[5] << 8) | data[6]; // raw param + float param = float(raw); + + //float oil_pressure = (float(param)*0.02822581)-3.27822581); //Oil pressure + float oil_pressure = (param * 0.02083333f) - 7.02083333f; + m_oil_press::publish(oil_pressure); + } + + case CAN_BASE_ADDR + 22: { + float egt[4]; + for (uint8_t i = 0; i < 4; i++) { + uint8_t idx = i * 2 + 5; + int16_t raw_egt = (int16_t) (data[idx] << 8) + data[idx + 1]; // EGT1 deg F s + egt[i] = float(raw_egt); + if (data[idx] & 0x80) + egt[i] += (0xffff << 16); + } + + m_egt_1::publish(egt[0] / 10.f); //EGT1 + m_egt_2::publish(egt[1] / 10.f); //EGT2 + m_egt_3::publish(egt[2] / 10.f); //EGT3 + m_egt_4::publish(egt[3] / 10.f); //EGT4 + } + } +} From f439e1cf8ea76e97dd5401c6a0a5b5f3069ed81b Mon Sep 17 00:00:00 2001 From: Eugene Shilin Date: Mon, 23 Mar 2026 14:48:51 +0300 Subject: [PATCH 02/39] rename script to uvh170 --- .../subsistems-UVH170/UVH170.cpp | 19 +++++++++---------- 1 file changed, 9 insertions(+), 10 deletions(-) rename vehicles/S1C/apx11/ECU.cpp => drivers/subsistems-UVH170/UVH170.cpp (89%) diff --git a/vehicles/S1C/apx11/ECU.cpp b/drivers/subsistems-UVH170/UVH170.cpp similarity index 89% rename from vehicles/S1C/apx11/ECU.cpp rename to drivers/subsistems-UVH170/UVH170.cpp index 01b25f0..df9850f 100644 --- a/vehicles/S1C/apx11/ECU.cpp +++ b/drivers/subsistems-UVH170/UVH170.cpp @@ -1,6 +1,6 @@ #include -const uint8_t PORT_ID_ECU{60}; +const uint8_t PORT_ID{60}; const uint8_t CAN_PACK_SIZE{13}; const uint8_t TASK_ECU_MS{200}; @@ -19,7 +19,7 @@ using m_egt_4 = Mandala; int main() { schedule_periodic(task("on_ecu"), TASK_ECU_MS); - receive(PORT_ID_ECU, "on_ecu_serial"); + receive(PORT_ID, "on_serial"); } void setThrottleMS() @@ -35,7 +35,7 @@ void setThrottleMS() //printf("thr:%f/n", ch_throttle); memcpy(&data[5], &ch_throttle, 4); data[9] = 1; - send(PORT_ID_ECU, data, 10, false); + send(PORT_ID, data, 10, false); } EXPORT void on_ecu() @@ -45,19 +45,16 @@ EXPORT void on_ecu() //Calc LAMBDA } -EXPORT void on_ecu_serial(const uint8_t *data, size_t size) +EXPORT void on_serial(const uint8_t *data, size_t size) { - printf("%d", size); - - if ((size > CAN_PACK_SIZE) || (size != (data[4] & 0x7F) + 5)) { - return; - } + //printf("%d", size); uint32_t can_id = (uint32_t) (data[0] + (data[1] << 8) + (data[2] << 16) + (data[3] << 24)); - //printf("can_id:%x\n", can_id); + //printf("can_id:%d\n", can_id); switch (can_id) { case CAN_BASE_ADDR + 2: { + //printf("temps"); float param; int16_t raw; @@ -73,6 +70,7 @@ EXPORT void on_ecu_serial(const uint8_t *data, size_t size) } case CAN_BASE_ADDR + 15: { + //printf("oil pressure"); int16_t raw = (int16_t) (data[5] << 8) | data[6]; // raw param float param = float(raw); @@ -82,6 +80,7 @@ EXPORT void on_ecu_serial(const uint8_t *data, size_t size) } case CAN_BASE_ADDR + 22: { + //printf("egt"); float egt[4]; for (uint8_t i = 0; i < 4; i++) { uint8_t idx = i * 2 + 5; From 0df0ba87260d5f85f5e034739737ef6ee11f11a9 Mon Sep 17 00:00:00 2001 From: Eugene Shilin Date: Mon, 23 Mar 2026 16:05:11 +0300 Subject: [PATCH 03/39] subsystems implemented --- drivers/subsistems-UVH170/UVH170.cpp | 292 ++++++++++++++++++++++++++- 1 file changed, 290 insertions(+), 2 deletions(-) diff --git a/drivers/subsistems-UVH170/UVH170.cpp b/drivers/subsistems-UVH170/UVH170.cpp index df9850f..67c0c6c 100644 --- a/drivers/subsistems-UVH170/UVH170.cpp +++ b/drivers/subsistems-UVH170/UVH170.cpp @@ -1,7 +1,7 @@ #include const uint8_t PORT_ID{60}; -const uint8_t CAN_PACK_SIZE{13}; +const uint8_t PACK_SIZE_CAN{12}; const uint8_t TASK_ECU_MS{200}; const uint16_t CAN_BASE_ADDR{1520}; @@ -16,6 +16,122 @@ using m_egt_2 = Mandala; using m_egt_3 = Mandala; using m_egt_4 = Mandala; +//Vesc tail +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; + +//Vesc gen +using m_vesc_gen_rpm = Mandala; +using m_vesc_gen_curr_in = Mandala; +using m_vesc_gen_motor_temp = Mandala; +using m_vesc_gen_fet_temp = Mandala; + +//Uvhpy +using m_uvhpy_status = Mandala; +using m_uvhpy_ibat_filt = Mandala; + +// AGL +using m_agl = Mandala; + +//VESC +//------------------------------------------------------------------------------------- +#define VESC_TAIL_ID 0x24 // VESC ID 36 +#define VESC_GEN_ID 0x25 // VESC ID 37 +#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 +#define CAN_PACKET_SET_CURRENT 1 +#define CAN_PACKET_SET_CURRENT_BRAKE 2 +#define CAN_PACKET_SET_RPM 3 + +struct VESC_CAN_Data +{ + int32_t rpm; //MSG1 + float current; //MSG1 + float duty; //MSG1 + uint32_t apm_hours; //MSG2 + uint32_t apm_hours_charged; //MSG2 + uint32_t watt_hours; //MSG3 + uint32_t watt_hours_charged; //MSG3 + float temp_fet; //MSG4 + float temp_mot; //MSG4 + float curr_in; //MSG4 + float pid_pos_now; //MSG4 + float voltage; //MSG5 + uint32_t tacho; //MSG5 +}; + +VESC_CAN_Data tail_data{}; +VESC_CAN_Data gen_data{}; +//------------------------------------------------------------------------------------- + +//UVHPU +//------------------------------------------------------------------------------------- +#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 + +#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() + +UVHPU _uvhpu{}; + +// AGL +//------------------------------------------------------------------------------------- +#define AGL_CAN_ID 0x00090002 + int main() { schedule_periodic(task("on_ecu"), TASK_ECU_MS); @@ -45,14 +161,167 @@ EXPORT void on_ecu() //Calc LAMBDA } +int16_t unpackInt16(const uint8_t *data, uint8_t index) +{ + return (int16_t) (data[index] | (data[index + 1] << 8)); +} + +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; + } + } +} + +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); +} + +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; + } + } +} + EXPORT void on_serial(const uint8_t *data, size_t size) { + if (size != PACK_SIZE_CAN) { + return; + } //printf("%d", size); 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 //printf("can_id:%d\n", can_id); - switch (can_id) { + uint8_t can_data[8] = {}; + for (uint8_t i = 0; i < 8; i++) { + can_data[i] = data[4 + i]; // 4 is data position + } + + //CAN ID 0xFF + switch (can_id & 0xFF) { + case VESC_TAIL_ID: { + //printf("vesc tail %u", can_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; + } + case VESC_GEN_ID: { + //printf("vesc gen %u", can_id); + uint16_t msg_id = (can_id >> 8) & 0xFF; + processVESCPackage(msg_id, can_data, &gen_data); + + m_vesc_gen_rpm::publish((float) abs(gen_data.rpm) / 21); + m_vesc_gen_fet_temp::publish(gen_data.temp_fet); + m_vesc_gen_motor_temp::publish(gen_data.temp_mot); + m_vesc_gen_curr_in::publish(fabs(gen_data.curr_in)); + + break; + } + } + + //CAN ID 0XFFFF + switch (can_id & 0xFFFF) { case CAN_BASE_ADDR + 2: { //printf("temps"); float param; @@ -95,5 +364,24 @@ EXPORT void on_serial(const uint8_t *data, size_t size) m_egt_3::publish(egt[2] / 10.f); //EGT3 m_egt_4::publish(egt[3] / 10.f); //EGT4 } + case AGL_CAN_ID: { + int16_t raw = (int16_t) (data[5] << 8) + data[6]; + float altitude = float(raw) / 100.0f; + if (altitude > 0.1f && altitude < 40.0f) + m_agl::publish(altitude); + + break; + } + case UVHPU_PACK1: + case UVHPU_PACK2: + case UVHPU_PACK3: + case UVHPU_PACK4: + case UVHPU_PACK5: + case UVHPU_PACK6: + case UVHPU_PACK7: { + //printf("uvhpu %x", can_id); + processUVHPUackage(can_id, can_data); + break; + } } } From c5edb1c700b160022cf69f0e941cc6c4304aaede Mon Sep 17 00:00:00 2001 From: Eugene Shilin Date: Mon, 23 Mar 2026 17:11:20 +0300 Subject: [PATCH 04/39] CAN agl removed --- drivers/subsistems-UVH170/UVH170.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/drivers/subsistems-UVH170/UVH170.cpp b/drivers/subsistems-UVH170/UVH170.cpp index 67c0c6c..3549e20 100644 --- a/drivers/subsistems-UVH170/UVH170.cpp +++ b/drivers/subsistems-UVH170/UVH170.cpp @@ -35,7 +35,7 @@ using m_uvhpy_status = Mandala; using m_uvhpy_ibat_filt = Mandala; // AGL -using m_agl = Mandala; +//using m_agl = Mandala; //VESC //------------------------------------------------------------------------------------- @@ -130,7 +130,7 @@ UVHPU _uvhpu{}; // AGL //------------------------------------------------------------------------------------- -#define AGL_CAN_ID 0x00090002 +//#define AGL_CAN_ID 0x00090002 int main() { @@ -364,14 +364,14 @@ EXPORT void on_serial(const uint8_t *data, size_t size) m_egt_3::publish(egt[2] / 10.f); //EGT3 m_egt_4::publish(egt[3] / 10.f); //EGT4 } - case AGL_CAN_ID: { + /*case AGL_CAN_ID: { int16_t raw = (int16_t) (data[5] << 8) + data[6]; float altitude = float(raw) / 100.0f; if (altitude > 0.1f && altitude < 40.0f) m_agl::publish(altitude); break; - } + }*/ case UVHPU_PACK1: case UVHPU_PACK2: case UVHPU_PACK3: From a4caa76e7fbe89a6ab29e9e87d02b5af55621d99 Mon Sep 17 00:00:00 2001 From: Eugene Shilin Date: Tue, 24 Mar 2026 13:12:52 +0300 Subject: [PATCH 05/39] agl fixed --- drivers/subsistems-UVH170/UVH170.cpp | 20 +++++++++++--------- 1 file changed, 11 insertions(+), 9 deletions(-) diff --git a/drivers/subsistems-UVH170/UVH170.cpp b/drivers/subsistems-UVH170/UVH170.cpp index 3549e20..118de7b 100644 --- a/drivers/subsistems-UVH170/UVH170.cpp +++ b/drivers/subsistems-UVH170/UVH170.cpp @@ -35,7 +35,7 @@ using m_uvhpy_status = Mandala; using m_uvhpy_ibat_filt = Mandala; // AGL -//using m_agl = Mandala; +using m_agl = Mandala; //VESC //------------------------------------------------------------------------------------- @@ -130,7 +130,7 @@ UVHPU _uvhpu{}; // AGL //------------------------------------------------------------------------------------- -//#define AGL_CAN_ID 0x00090002 +#define AGL_CAN_ID 0x00090002 int main() { @@ -290,6 +290,15 @@ EXPORT void on_serial(const uint8_t *data, size_t size) can_data[i] = data[4 + i]; // 4 is data position } + if (can_id == AGL_CAN_ID) { + //printf("agl"); + 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; + } + //CAN ID 0xFF switch (can_id & 0xFF) { case VESC_TAIL_ID: { @@ -364,14 +373,7 @@ EXPORT void on_serial(const uint8_t *data, size_t size) m_egt_3::publish(egt[2] / 10.f); //EGT3 m_egt_4::publish(egt[3] / 10.f); //EGT4 } - /*case AGL_CAN_ID: { - int16_t raw = (int16_t) (data[5] << 8) + data[6]; - float altitude = float(raw) / 100.0f; - if (altitude > 0.1f && altitude < 40.0f) - m_agl::publish(altitude); - break; - }*/ case UVHPU_PACK1: case UVHPU_PACK2: case UVHPU_PACK3: From ac5e0c152643a30a3069843826c28480807da855 Mon Sep 17 00:00:00 2001 From: Eugene Shilin Date: Tue, 24 Mar 2026 13:29:09 +0300 Subject: [PATCH 06/39] pu tested --- drivers/subsistems-UVH170/UVH170.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/drivers/subsistems-UVH170/UVH170.cpp b/drivers/subsistems-UVH170/UVH170.cpp index 118de7b..5d43b21 100644 --- a/drivers/subsistems-UVH170/UVH170.cpp +++ b/drivers/subsistems-UVH170/UVH170.cpp @@ -135,6 +135,9 @@ UVHPU _uvhpu{}; int main() { schedule_periodic(task("on_ecu"), TASK_ECU_MS); + + task("uvhpu"); //GCS with terminal command `vmexec("uvhpu")` + receive(PORT_ID, "on_serial"); } From 0ece5ce6176364fab80a69c08714848fedd8cfe7 Mon Sep 17 00:00:00 2001 From: Eugene Shilin Date: Tue, 24 Mar 2026 14:06:06 +0300 Subject: [PATCH 07/39] ecu fixed --- drivers/subsistems-UVH170/UVH170.cpp | 12 +++++------- 1 file changed, 5 insertions(+), 7 deletions(-) diff --git a/drivers/subsistems-UVH170/UVH170.cpp b/drivers/subsistems-UVH170/UVH170.cpp index 5d43b21..6d3e4cb 100644 --- a/drivers/subsistems-UVH170/UVH170.cpp +++ b/drivers/subsistems-UVH170/UVH170.cpp @@ -339,12 +339,12 @@ EXPORT void on_serial(const uint8_t *data, size_t size) float param; int16_t raw; - raw = (int16_t) (data[9] << 8) + data[10]; + raw = (int16_t) (data[8] << 8) + data[9]; param = float(raw); float oil_temp = (param / 10.0f - 32.0f) * 5.0f / 9.0f; m_oil_temp::publish(oil_temp); // Oil temperature [deg] - raw = (int16_t) (data[11] << 8) + data[12]; + raw = (int16_t) (data[10] << 8) + data[11]; param = float(raw); float coolant_temp = (param / 10.0f - 32.0f) * 5.0f / 9.0f; m_cool_temp::publish(coolant_temp); // Coolant temperature [deg] @@ -352,7 +352,7 @@ EXPORT void on_serial(const uint8_t *data, size_t size) case CAN_BASE_ADDR + 15: { //printf("oil pressure"); - int16_t raw = (int16_t) (data[5] << 8) | data[6]; // raw param + uint16_t raw = (uint16_t) ((data[4] << 8) | data[5]); // raw param float param = float(raw); //float oil_pressure = (float(param)*0.02822581)-3.27822581); //Oil pressure @@ -364,11 +364,9 @@ EXPORT void on_serial(const uint8_t *data, size_t size) //printf("egt"); float egt[4]; for (uint8_t i = 0; i < 4; i++) { - uint8_t idx = i * 2 + 5; - int16_t raw_egt = (int16_t) (data[idx] << 8) + data[idx + 1]; // EGT1 deg F s + uint8_t idx = i * 2 + 4; + uint16_t raw_egt = (uint16_t) (data[idx] << 8) + data[idx + 1]; // EGT1 deg F s egt[i] = float(raw_egt); - if (data[idx] & 0x80) - egt[i] += (0xffff << 16); } m_egt_1::publish(egt[0] / 10.f); //EGT1 From 806e7585face01c9f50bb624644a705c2a871e8e Mon Sep 17 00:00:00 2001 From: Eugene Shilin Date: Tue, 24 Mar 2026 14:30:50 +0300 Subject: [PATCH 08/39] fix ecu processor --- drivers/subsistems-UVH170/UVH170.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/drivers/subsistems-UVH170/UVH170.cpp b/drivers/subsistems-UVH170/UVH170.cpp index 6d3e4cb..68be2e0 100644 --- a/drivers/subsistems-UVH170/UVH170.cpp +++ b/drivers/subsistems-UVH170/UVH170.cpp @@ -348,6 +348,7 @@ EXPORT void on_serial(const uint8_t *data, size_t size) param = float(raw); float coolant_temp = (param / 10.0f - 32.0f) * 5.0f / 9.0f; m_cool_temp::publish(coolant_temp); // Coolant temperature [deg] + break; } case CAN_BASE_ADDR + 15: { @@ -358,6 +359,7 @@ EXPORT void on_serial(const uint8_t *data, size_t size) //float oil_pressure = (float(param)*0.02822581)-3.27822581); //Oil pressure float oil_pressure = (param * 0.02083333f) - 7.02083333f; m_oil_press::publish(oil_pressure); + break; } case CAN_BASE_ADDR + 22: { @@ -373,6 +375,7 @@ EXPORT void on_serial(const uint8_t *data, size_t size) m_egt_2::publish(egt[1] / 10.f); //EGT2 m_egt_3::publish(egt[2] / 10.f); //EGT3 m_egt_4::publish(egt[3] / 10.f); //EGT4 + break; } case UVHPU_PACK1: From 9eb9ded1e478db18f3526596adb2e587117f9fcb Mon Sep 17 00:00:00 2001 From: Eugene Shilin Date: Tue, 24 Mar 2026 15:56:15 +0300 Subject: [PATCH 09/39] refactor --- drivers/subsistems-UVH170/UVH170.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/drivers/subsistems-UVH170/UVH170.cpp b/drivers/subsistems-UVH170/UVH170.cpp index 68be2e0..3a65264 100644 --- a/drivers/subsistems-UVH170/UVH170.cpp +++ b/drivers/subsistems-UVH170/UVH170.cpp @@ -1,6 +1,6 @@ #include -const uint8_t PORT_ID{60}; +const uint8_t PORT_ID{1}; const uint8_t PACK_SIZE_CAN{12}; const uint8_t TASK_ECU_MS{200}; @@ -9,16 +9,16 @@ const uint8_t OFFSET_CAN_ID_CURRENT_THROTTLE{71}; const uint16_t CAN_ID_CURRENT_THROTTLE = CAN_BASE_ADDR + OFFSET_CAN_ID_CURRENT_THROTTLE; using m_oil_temp = Mandala; -using m_cool_temp = Mandala; -using m_oil_press = Mandala; -using m_egt_1 = Mandala; +using m_cool_temp = Mandala; //??? +using m_oil_press = Mandala; +using m_egt_1 = Mandala; //??? using m_egt_2 = Mandala; using m_egt_3 = Mandala; using m_egt_4 = Mandala; //Vesc tail using m_vesc_tail_rpm = Mandala; -using m_vesc_tail_current = 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; @@ -279,14 +279,14 @@ void processVESCPackage(const uint32_t &msg_id, const uint8_t *data, VESC_CAN_Da EXPORT void on_serial(const uint8_t *data, size_t size) { + //printf("%d", size); if (size != PACK_SIZE_CAN) { - return; + //return; } - //printf("%d", size); 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 - //printf("can_id:%d\n", can_id); + //printf("can_id:%x\n", can_id); uint8_t can_data[8] = {}; for (uint8_t i = 0; i < 8; i++) { From e39d443d8951c4dc255a9a71728c71ae45f78b26 Mon Sep 17 00:00:00 2001 From: Eugene Shilin Date: Tue, 24 Mar 2026 16:21:20 +0300 Subject: [PATCH 10/39] vesc functions --- drivers/subsistems-UVH170/UVH170.cpp | 39 ++++++++++++++++++++++++++++ 1 file changed, 39 insertions(+) diff --git a/drivers/subsistems-UVH170/UVH170.cpp b/drivers/subsistems-UVH170/UVH170.cpp index 3a65264..8ab2b87 100644 --- a/drivers/subsistems-UVH170/UVH170.cpp +++ b/drivers/subsistems-UVH170/UVH170.cpp @@ -132,9 +132,14 @@ UVHPU _uvhpu{}; //------------------------------------------------------------------------------------- #define AGL_CAN_ID 0x00090002 +//------------------------------------------------------------------------------------- +void setRPM(const uint8_t &, const int32_t &); +void setCurrent(const uint8_t &, const float &); + int main() { schedule_periodic(task("on_ecu"), TASK_ECU_MS); + schedule_periodic(task("on_start_eng", 100); task("uvhpu"); //GCS with terminal command `vmexec("uvhpu")` @@ -157,6 +162,17 @@ void setThrottleMS() send(PORT_ID, data, 10, false); } +EXPORT void on_start_eng() +{ + ECUDemandControl(uint16_t((m_eng_ctr::value() * 0.9f + 0.1f) * 1000.f)); + + bool on_power_ignition = (bool) m_pwr_ign::value(); + + if (on_power_ignition && (uint32_t) m_sw_starter::value()) { + setRPM(VESC_GEN_ID, -40000); + } +} + EXPORT void on_ecu() { setThrottleMS(); @@ -239,6 +255,29 @@ EXPORT void uvhpu() printf("cbat_mod: %.2f", _uvhpu.MSG7.cbat_mod); } +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_aux_id, msg, 8, false); +} + +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_aux_id, msg, 8, false); +} + void processVESCPackage(const uint32_t &msg_id, const uint8_t *data, VESC_CAN_Data *vesc_data) { switch (msg_id) { From e7adf08d3f7c1303538045a62cbc0bd114f683e3 Mon Sep 17 00:00:00 2001 From: Eugene Shilin Date: Wed, 25 Mar 2026 10:39:58 +0300 Subject: [PATCH 11/39] various fixes --- drivers/subsistems-UVH170/UVH170.cpp | 21 +++++++++++++++++---- 1 file changed, 17 insertions(+), 4 deletions(-) diff --git a/drivers/subsistems-UVH170/UVH170.cpp b/drivers/subsistems-UVH170/UVH170.cpp index 8ab2b87..f4d2a3c 100644 --- a/drivers/subsistems-UVH170/UVH170.cpp +++ b/drivers/subsistems-UVH170/UVH170.cpp @@ -34,6 +34,11 @@ using m_vesc_gen_fet_temp = Mandala; using m_uvhpy_status = Mandala; using m_uvhpy_ibat_filt = Mandala; +//Engine +using m_pwr_ign = Mandala; +using m_sw_starter = Mandala; +using m_eng_ctr = Mandala; + // AGL using m_agl = Mandala; @@ -139,7 +144,7 @@ void setCurrent(const uint8_t &, const float &); int main() { schedule_periodic(task("on_ecu"), TASK_ECU_MS); - schedule_periodic(task("on_start_eng", 100); + schedule_periodic(task("on_start_eng"), 100); task("uvhpu"); //GCS with terminal command `vmexec("uvhpu")` @@ -164,7 +169,7 @@ void setThrottleMS() EXPORT void on_start_eng() { - ECUDemandControl(uint16_t((m_eng_ctr::value() * 0.9f + 0.1f) * 1000.f)); + //ECUDemandControl(uint16_t((m_eng_ctr::value() * 0.9f + 0.1f) * 1000.f)); bool on_power_ignition = (bool) m_pwr_ign::value(); @@ -180,6 +185,14 @@ EXPORT void on_ecu() //Calc LAMBDA } +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)); @@ -264,7 +277,7 @@ void setCurrent(const uint8_t &VECS_CAN_ID, const float &val) msg[1] = CAN_PACKET_SET_CURRENT; msg[3] |= 0x80; // IDE (bit 7) 1=ext,0=std; serializeInt(msg, 4, current); - send(port_aux_id, msg, 8, false); + send(PORT_ID, msg, 8, false); } void setRPM(const uint8_t &VECS_CAN_ID, const int32_t &val) @@ -275,7 +288,7 @@ void setRPM(const uint8_t &VECS_CAN_ID, const int32_t &val) msg[1] = CAN_PACKET_SET_RPM; msg[3] |= 0x80; // IDE (bit 7) 1=ext,0=std serializeInt(msg, 4, val); - send(port_aux_id, msg, 8, false); + send(PORT_ID, msg, 8, false); } void processVESCPackage(const uint32_t &msg_id, const uint8_t *data, VESC_CAN_Data *vesc_data) From 479982cd38c4d959bb374dc74d08112873548626 Mon Sep 17 00:00:00 2001 From: Eugene Shilin Date: Wed, 25 Mar 2026 13:10:32 +0300 Subject: [PATCH 12/39] user var changed --- drivers/subsistems-UVH170/UVH170.cpp | 35 ++++++++++++++++------------ 1 file changed, 20 insertions(+), 15 deletions(-) diff --git a/drivers/subsistems-UVH170/UVH170.cpp b/drivers/subsistems-UVH170/UVH170.cpp index f4d2a3c..8a44661 100644 --- a/drivers/subsistems-UVH170/UVH170.cpp +++ b/drivers/subsistems-UVH170/UVH170.cpp @@ -8,30 +8,31 @@ const uint16_t CAN_BASE_ADDR{1520}; const uint8_t OFFSET_CAN_ID_CURRENT_THROTTLE{71}; const uint16_t CAN_ID_CURRENT_THROTTLE = CAN_BASE_ADDR + OFFSET_CAN_ID_CURRENT_THROTTLE; -using m_oil_temp = Mandala; -using m_cool_temp = Mandala; //??? +//u2 u3 u4 u10 are already used in adc & gpio +using m_oil_temp = Mandala; +using m_cool_temp = Mandala; using m_oil_press = Mandala; -using m_egt_1 = Mandala; //??? -using m_egt_2 = Mandala; -using m_egt_3 = Mandala; -using m_egt_4 = Mandala; +using m_egt_1 = Mandala; +using m_egt_2 = Mandala; +using m_egt_3 = Mandala; +using m_egt_4 = Mandala; //Vesc tail -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; +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; //Vesc gen using m_vesc_gen_rpm = Mandala; using m_vesc_gen_curr_in = Mandala; using m_vesc_gen_motor_temp = Mandala; -using m_vesc_gen_fet_temp = Mandala; +using m_vesc_gen_fet_temp = Mandala; //Uvhpy -using m_uvhpy_status = Mandala; +using m_uvhpy_status = Mandala; using m_uvhpy_ibat_filt = Mandala; //Engine @@ -40,7 +41,7 @@ using m_sw_starter = Mandala; using m_eng_ctr = Mandala; // AGL -using m_agl = Mandala; +using m_agl = Mandala; //VESC //------------------------------------------------------------------------------------- @@ -148,6 +149,10 @@ int main() task("uvhpu"); //GCS with terminal command `vmexec("uvhpu")` + m_pwr_ign(); //subscribe + m_sw_starter(); + m_eng_ctr(); + receive(PORT_ID, "on_serial"); } From a3acb361f3c099d569f0aa2862c7d82f0531017f Mon Sep 17 00:00:00 2001 From: Eugene Shilin Date: Wed, 25 Mar 2026 14:03:51 +0300 Subject: [PATCH 13/39] fix user variables --- drivers/subsistems-UVH170/UVH170.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/drivers/subsistems-UVH170/UVH170.cpp b/drivers/subsistems-UVH170/UVH170.cpp index 8a44661..a4a3160 100644 --- a/drivers/subsistems-UVH170/UVH170.cpp +++ b/drivers/subsistems-UVH170/UVH170.cpp @@ -18,7 +18,7 @@ using m_egt_3 = Mandala; using m_egt_4 = Mandala; //Vesc tail -using m_vesc_tail_rpm = Mandala; +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; @@ -29,7 +29,7 @@ using m_vesc_tail_curr_in = Mandala; using m_vesc_gen_rpm = Mandala; using m_vesc_gen_curr_in = Mandala; using m_vesc_gen_motor_temp = Mandala; -using m_vesc_gen_fet_temp = Mandala; +using m_vesc_gen_fet_temp = Mandala; //Uvhpy using m_uvhpy_status = Mandala; From d94e013aed045fdb0577c59ba221d8248302aa4c Mon Sep 17 00:00:00 2001 From: Eugene Shilin Date: Wed, 22 Apr 2026 17:24:31 +0300 Subject: [PATCH 14/39] ECU script separated --- drivers/subsistems-UVH170/ECU.cpp | 122 +++++++++++++++++++++++++++ drivers/subsistems-UVH170/UVH170.cpp | 85 +------------------ 2 files changed, 123 insertions(+), 84 deletions(-) create mode 100644 drivers/subsistems-UVH170/ECU.cpp diff --git a/drivers/subsistems-UVH170/ECU.cpp b/drivers/subsistems-UVH170/ECU.cpp new file mode 100644 index 0000000..6bfedd8 --- /dev/null +++ b/drivers/subsistems-UVH170/ECU.cpp @@ -0,0 +1,122 @@ +#include + +const uint8_t PORT_ID{1}; +const uint8_t PACK_SIZE_CAN{12}; +const uint8_t TASK_ECU_MS{200}; + +const uint16_t CAN_BASE_ADDR{1520}; +const uint8_t OFFSET_CAN_ID_CURRENT_THROTTLE{71}; +const uint16_t CAN_ID_CURRENT_THROTTLE = CAN_BASE_ADDR + OFFSET_CAN_ID_CURRENT_THROTTLE; + +//u2 u3 u4 u10 are already used in adc & gpio +using m_oil_temp = Mandala; +using m_cool_temp = Mandala; +using m_oil_press = Mandala; +using m_egt_1 = Mandala; +using m_egt_2 = Mandala; +using m_egt_3 = Mandala; +using m_egt_4 = Mandala; + +using m_eng_ctr = Mandala; + +int main() +{ + schedule_periodic(task("on_ecu"), TASK_ECU_MS); + + //m_pwr_ign(); //subscribe + //m_sw_starter(); + m_eng_ctr(); + + receive(PORT_ID, "on_serial"); +} + +void setThrottleMS() +{ + uint8_t data[13]; + data[0] = CAN_ID_CURRENT_THROTTLE & 0xFF; + data[1] = (CAN_ID_CURRENT_THROTTLE >> 8) & 0xFF; + data[2] = (CAN_ID_CURRENT_THROTTLE >> 16) & 0xFF; + data[3] = (CAN_ID_CURRENT_THROTTLE >> 24) & 0xFF; + data[4] = 5; + + float ch_throttle = m_eng_ctr::value(); + //printf("thr:%f/n", ch_throttle); + memcpy(&data[5], &ch_throttle, 4); + data[9] = 1; + send(PORT_ID, data, 10, false); +} + +EXPORT void on_ecu() +{ + setThrottleMS(); + + //Calc LAMBDA +} + +//ECUDemandControl(uint16_t((m_eng_ctr::value() * 0.9f + 0.1f) * 1000.f)); ??????? + + +EXPORT void on_serial(const uint8_t *data, size_t size) +{ + //printf("%d", 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 + //printf("can_id:%x\n", can_id); + + uint8_t can_data[8] = {}; + for (uint8_t i = 0; i < 8; i++) { + can_data[i] = data[4 + i]; // 4 is data position + } + + + //CAN ID 0XFFFF + switch (can_id & 0xFFFF) { + case CAN_BASE_ADDR + 2: { + //printf("temps"); + float param; + int16_t raw; + + raw = (int16_t) (data[8] << 8) + data[9]; + param = float(raw); + float oil_temp = (param / 10.0f - 32.0f) * 5.0f / 9.0f; + m_oil_temp::publish(oil_temp); // Oil temperature [deg] + + raw = (int16_t) (data[10] << 8) + data[11]; + param = float(raw); + float coolant_temp = (param / 10.0f - 32.0f) * 5.0f / 9.0f; + m_cool_temp::publish(coolant_temp); // Coolant temperature [deg] + break; + } + + case CAN_BASE_ADDR + 15: { + //printf("oil pressure"); + uint16_t raw = (uint16_t) ((data[4] << 8) | data[5]); // raw param + float param = float(raw); + + //float oil_pressure = (float(param)*0.02822581)-3.27822581); //Oil pressure + float oil_pressure = (param * 0.02083333f) - 7.02083333f; + m_oil_press::publish(oil_pressure); + break; + } + + case CAN_BASE_ADDR + 22: { + //printf("egt"); + float egt[4]; + for (uint8_t i = 0; i < 4; i++) { + uint8_t idx = i * 2 + 4; + uint16_t raw_egt = (uint16_t) (data[idx] << 8) + data[idx + 1]; // EGT1 deg F s + egt[i] = float(raw_egt); + } + + m_egt_1::publish(egt[0] / 10.f); //EGT1 + m_egt_2::publish(egt[1] / 10.f); //EGT2 + m_egt_3::publish(egt[2] / 10.f); //EGT3 + m_egt_4::publish(egt[3] / 10.f); //EGT4 + break; + } + } +} diff --git a/drivers/subsistems-UVH170/UVH170.cpp b/drivers/subsistems-UVH170/UVH170.cpp index a4a3160..59c6e60 100644 --- a/drivers/subsistems-UVH170/UVH170.cpp +++ b/drivers/subsistems-UVH170/UVH170.cpp @@ -2,20 +2,6 @@ const uint8_t PORT_ID{1}; const uint8_t PACK_SIZE_CAN{12}; -const uint8_t TASK_ECU_MS{200}; - -const uint16_t CAN_BASE_ADDR{1520}; -const uint8_t OFFSET_CAN_ID_CURRENT_THROTTLE{71}; -const uint16_t CAN_ID_CURRENT_THROTTLE = CAN_BASE_ADDR + OFFSET_CAN_ID_CURRENT_THROTTLE; - -//u2 u3 u4 u10 are already used in adc & gpio -using m_oil_temp = Mandala; -using m_cool_temp = Mandala; -using m_oil_press = Mandala; -using m_egt_1 = Mandala; -using m_egt_2 = Mandala; -using m_egt_3 = Mandala; -using m_egt_4 = Mandala; //Vesc tail using m_vesc_tail_rpm = Mandala; @@ -144,7 +130,6 @@ void setCurrent(const uint8_t &, const float &); int main() { - schedule_periodic(task("on_ecu"), TASK_ECU_MS); schedule_periodic(task("on_start_eng"), 100); task("uvhpu"); //GCS with terminal command `vmexec("uvhpu")` @@ -156,40 +141,16 @@ int main() receive(PORT_ID, "on_serial"); } -void setThrottleMS() -{ - uint8_t data[13]; - data[0] = CAN_ID_CURRENT_THROTTLE & 0xFF; - data[1] = (CAN_ID_CURRENT_THROTTLE >> 8) & 0xFF; - data[2] = (CAN_ID_CURRENT_THROTTLE >> 16) & 0xFF; - data[3] = (CAN_ID_CURRENT_THROTTLE >> 24) & 0xFF; - data[4] = 5; - - float ch_throttle = 0.0; - //printf("thr:%f/n", ch_throttle); - memcpy(&data[5], &ch_throttle, 4); - data[9] = 1; - send(PORT_ID, data, 10, false); -} - EXPORT void on_start_eng() { - //ECUDemandControl(uint16_t((m_eng_ctr::value() * 0.9f + 0.1f) * 1000.f)); - bool on_power_ignition = (bool) m_pwr_ign::value(); if (on_power_ignition && (uint32_t) m_sw_starter::value()) { setRPM(VESC_GEN_ID, -40000); + //setCurrent(VESC_GEN_ID, 0.0f); } } -EXPORT void on_ecu() -{ - setThrottleMS(); - - //Calc LAMBDA -} - void serializeInt(uint8_t *data, uint8_t index, int32_t value) { for (uint8_t i = 0; i < 4; i++) { @@ -391,50 +352,6 @@ EXPORT void on_serial(const uint8_t *data, size_t size) //CAN ID 0XFFFF switch (can_id & 0xFFFF) { - case CAN_BASE_ADDR + 2: { - //printf("temps"); - float param; - int16_t raw; - - raw = (int16_t) (data[8] << 8) + data[9]; - param = float(raw); - float oil_temp = (param / 10.0f - 32.0f) * 5.0f / 9.0f; - m_oil_temp::publish(oil_temp); // Oil temperature [deg] - - raw = (int16_t) (data[10] << 8) + data[11]; - param = float(raw); - float coolant_temp = (param / 10.0f - 32.0f) * 5.0f / 9.0f; - m_cool_temp::publish(coolant_temp); // Coolant temperature [deg] - break; - } - - case CAN_BASE_ADDR + 15: { - //printf("oil pressure"); - uint16_t raw = (uint16_t) ((data[4] << 8) | data[5]); // raw param - float param = float(raw); - - //float oil_pressure = (float(param)*0.02822581)-3.27822581); //Oil pressure - float oil_pressure = (param * 0.02083333f) - 7.02083333f; - m_oil_press::publish(oil_pressure); - break; - } - - case CAN_BASE_ADDR + 22: { - //printf("egt"); - float egt[4]; - for (uint8_t i = 0; i < 4; i++) { - uint8_t idx = i * 2 + 4; - uint16_t raw_egt = (uint16_t) (data[idx] << 8) + data[idx + 1]; // EGT1 deg F s - egt[i] = float(raw_egt); - } - - m_egt_1::publish(egt[0] / 10.f); //EGT1 - m_egt_2::publish(egt[1] / 10.f); //EGT2 - m_egt_3::publish(egt[2] / 10.f); //EGT3 - m_egt_4::publish(egt[3] / 10.f); //EGT4 - break; - } - case UVHPU_PACK1: case UVHPU_PACK2: case UVHPU_PACK3: From bc47b05fb505ab86ea4b8a439d47164c1b56a5cf Mon Sep 17 00:00:00 2001 From: Eugene Shilin Date: Thu, 23 Apr 2026 13:56:11 +0300 Subject: [PATCH 15/39] ecu script fix --- drivers/subsistems-UVH170/ECU.cpp | 25 +++++++++++-------------- 1 file changed, 11 insertions(+), 14 deletions(-) diff --git a/drivers/subsistems-UVH170/ECU.cpp b/drivers/subsistems-UVH170/ECU.cpp index 6bfedd8..62a989d 100644 --- a/drivers/subsistems-UVH170/ECU.cpp +++ b/drivers/subsistems-UVH170/ECU.cpp @@ -1,6 +1,6 @@ #include -const uint8_t PORT_ID{1}; +const uint8_t PORT_ID{2}; const uint8_t PACK_SIZE_CAN{12}; const uint8_t TASK_ECU_MS{200}; @@ -18,19 +18,19 @@ using m_egt_3 = Mandala; using m_egt_4 = Mandala; using m_eng_ctr = Mandala; +using m_pwr_ign = Mandala; int main() { schedule_periodic(task("on_ecu"), TASK_ECU_MS); - //m_pwr_ign(); //subscribe - //m_sw_starter(); + m_pwr_ign(); //subscribe m_eng_ctr(); receive(PORT_ID, "on_serial"); } -void setThrottleMS() +void setThrottleIgn() { uint8_t data[13]; data[0] = CAN_ID_CURRENT_THROTTLE & 0xFF; @@ -39,23 +39,20 @@ void setThrottleMS() data[3] = (CAN_ID_CURRENT_THROTTLE >> 24) & 0xFF; data[4] = 5; - float ch_throttle = m_eng_ctr::value(); - //printf("thr:%f/n", ch_throttle); + float ch_throttle = m_eng_ctr::value(); // -1.0 ... +1.0 memcpy(&data[5], &ch_throttle, 4); - data[9] = 1; - send(PORT_ID, data, 10, false); + //printf("thr:%.2f", ch_throttle); + + uint8_t power_eng = m_pwr_ign::value(); // 0 / 1 + data[9] = power_eng; + send(PORT_ID, data, 10, true); } EXPORT void on_ecu() { - setThrottleMS(); - - //Calc LAMBDA + setThrottleIgn(); } -//ECUDemandControl(uint16_t((m_eng_ctr::value() * 0.9f + 0.1f) * 1000.f)); ??????? - - EXPORT void on_serial(const uint8_t *data, size_t size) { //printf("%d", size); From ff67bb172de3f39ed0381f6b95dd6b77924bbc7f Mon Sep 17 00:00:00 2001 From: Eugene Shilin Date: Thu, 23 Apr 2026 15:35:51 +0300 Subject: [PATCH 16/39] script for fuel sensor --- drivers/subsistems-UVH170/dut.cpp | 143 ++++++++++++++++++++++++++++++ 1 file changed, 143 insertions(+) create mode 100644 drivers/subsistems-UVH170/dut.cpp diff --git a/drivers/subsistems-UVH170/dut.cpp b/drivers/subsistems-UVH170/dut.cpp new file mode 100644 index 0000000..0ed1e82 --- /dev/null +++ b/drivers/subsistems-UVH170/dut.cpp @@ -0,0 +1,143 @@ +//id 170 +//UART: 19200 8 N 1 + +#include + +static constexpr const port_id_t port_fuel_id{3}; +const uint16_t TASK_FUEL_MS{500}; //msec +const uint8_t ADR_FUEL_SENS1{170}; +const uint8_t MSG_FUEL_SIZE{9}; //FUEL +uint8_t snd_fuel_buf[MSG_FUEL_SIZE] = {}; +uint8_t ADR_FUEL[1] = {ADR_FUEL_SENS1}; +const float V_MAX1 = 16.7f; +const uint8_t TIME_SA{5}; //sec + +struct _fuel +{ + _fuel(float v_max) + : V_MAX(v_max) + , liters(0.f) + , percent(0.f) + , time_ans(0) + {} + + void set_percent(float p) + { + percent = p; + liters = (p * V_MAX) / 100.f; + time_ans = time_ms(); + } + + float V_MAX; // max tank liters + float liters; // liters + float percent; // percent + uint32_t time_ans; +}; + +_fuel fuel[1] = {_fuel(V_MAX1)}; + + +//fuel +using m_fuel1 = Mandala; + +using m_fuel_p = Mandala; +using m_fuel_l = Mandala; + +using m_warn1 = Mandala; + + +int main() +{ + + schedule_periodic(task("on_fuel"), TASK_FUEL_MS); + + receive(port_fuel_id, "on_fuel_serial"); + + //fuel + m_fuel_p(); + m_fuel_l(); + m_fuel1(); + m_warn1(); + + return 0; +} + +EXPORT uint8_t calcCRC(const uint8_t *buf, size_t size) +{ + uint8_t crc = 0x00; + + for (uint8_t j = 0; j < size; j++) { + uint8_t i = buf[j] ^ crc; + crc = 0; + if (i & 0x01) + crc ^= 0x5e; + if (i & 0x02) + crc ^= 0xbc; + if (i & 0x04) + crc ^= 0x61; + if (i & 0x08) + crc ^= 0xc2; + if (i & 0x10) + crc ^= 0x9d; + if (i & 0x20) + crc ^= 0x23; + if (i & 0x40) + crc ^= 0x46; + if (i & 0x80) + crc ^= 0x8c; + } + return crc % 256; +} + + + +void check_answer_time() +{ + uint32_t now = time_ms(); + + m_warn1::publish((fuel[0].time_ans + TIME_SA * 1000 < now) ? true : false); +} + +EXPORT void on_fuel() +{ + check_answer_time(); + + m_fuel1::publish(fuel[0].liters); + + m_fuel_l::publish(fuel[0].liters); + m_fuel_p::publish(fuel[0].percent); + + + //request fuel sensors + snd_fuel_buf[0] = 0x31; + snd_fuel_buf[2] = 0x06; + + snd_fuel_buf[1] = ADR_FUEL[0]; + + snd_fuel_buf[3] = calcCRC(snd_fuel_buf, 3); + send(port_fuel_id, snd_fuel_buf, 4, true); +} + +EXPORT void on_fuel_serial(const uint8_t *data, size_t size) +{ + //typePack(0)[0x3E] adr(1) fmt(2) data(3) crc(8) + if (size != MSG_FUEL_SIZE) { + return; + } + + if ((data[0] != 0x3E) || (data[2] != 0x06)) { + return; + } + + if (data[8] != calcCRC(data, 8)) { + return; + } + + float fuel_percent = (float) (data[4] | (data[5] << 8)) / 10.f; + + //printf("fuel: %.2f", fuel_percent); + + if (data[1] == ADR_FUEL[0]) { + fuel[0].set_percent(fuel_percent); + } +} From 2177777db016faf2c5baaeaaa03d756efc529b64 Mon Sep 17 00:00:00 2001 From: Eugene Shilin Date: Thu, 23 Apr 2026 15:46:56 +0300 Subject: [PATCH 17/39] comment --- drivers/subsistems-UVH170/ECU.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/drivers/subsistems-UVH170/ECU.cpp b/drivers/subsistems-UVH170/ECU.cpp index 62a989d..0eb0b1b 100644 --- a/drivers/subsistems-UVH170/ECU.cpp +++ b/drivers/subsistems-UVH170/ECU.cpp @@ -37,7 +37,7 @@ void setThrottleIgn() data[1] = (CAN_ID_CURRENT_THROTTLE >> 8) & 0xFF; data[2] = (CAN_ID_CURRENT_THROTTLE >> 16) & 0xFF; data[3] = (CAN_ID_CURRENT_THROTTLE >> 24) & 0xFF; - data[4] = 5; + data[4] = 5; //size of useful payload float ch_throttle = m_eng_ctr::value(); // -1.0 ... +1.0 memcpy(&data[5], &ch_throttle, 4); From e0013687984bd0d440300d87d61ee79325573b20 Mon Sep 17 00:00:00 2001 From: Eugene Shilin Date: Thu, 23 Apr 2026 15:54:17 +0300 Subject: [PATCH 18/39] map thr from 0..1 to -1..1 --- drivers/subsistems-UVH170/ECU.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/drivers/subsistems-UVH170/ECU.cpp b/drivers/subsistems-UVH170/ECU.cpp index 0eb0b1b..2bf1852 100644 --- a/drivers/subsistems-UVH170/ECU.cpp +++ b/drivers/subsistems-UVH170/ECU.cpp @@ -39,11 +39,11 @@ void setThrottleIgn() data[3] = (CAN_ID_CURRENT_THROTTLE >> 24) & 0xFF; data[4] = 5; //size of useful payload - float ch_throttle = m_eng_ctr::value(); // -1.0 ... +1.0 + float ch_throttle = m_eng_ctr::value() * 2.f - 1.f; // -1.0 ... +1.0 memcpy(&data[5], &ch_throttle, 4); //printf("thr:%.2f", ch_throttle); - uint8_t power_eng = m_pwr_ign::value(); // 0 / 1 + uint8_t power_eng = (bool) m_pwr_ign::value(); // 0 / 1 data[9] = power_eng; send(PORT_ID, data, 10, true); } From b588892665b19b34d3c76a81d5afd9d5dda1db03 Mon Sep 17 00:00:00 2001 From: Eugene Shilin Date: Thu, 23 Apr 2026 17:08:48 +0300 Subject: [PATCH 19/39] fuel pressure added --- drivers/subsistems-UVH170/UVH170.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/drivers/subsistems-UVH170/UVH170.cpp b/drivers/subsistems-UVH170/UVH170.cpp index 59c6e60..5ac6ef0 100644 --- a/drivers/subsistems-UVH170/UVH170.cpp +++ b/drivers/subsistems-UVH170/UVH170.cpp @@ -29,6 +29,10 @@ using m_eng_ctr = Mandala; // AGL using m_agl = Mandala; +//fuel pressure ADC +using m_fps_adc = Mandala; +using m_fps = Mandala; + //VESC //------------------------------------------------------------------------------------- #define VESC_TAIL_ID 0x24 // VESC ID 36 @@ -137,6 +141,7 @@ int main() m_pwr_ign(); //subscribe m_sw_starter(); m_eng_ctr(); + m_fps_adc(); receive(PORT_ID, "on_serial"); } @@ -149,6 +154,9 @@ EXPORT void on_start_eng() setRPM(VESC_GEN_ID, -40000); //setCurrent(VESC_GEN_ID, 0.0f); } + + float fuel_pressure = (m_fps_adc::value() - 0.2f) / 0.6429f; //bar + m_fps::publish(fuel_pressure); } void serializeInt(uint8_t *data, uint8_t index, int32_t value) From 83c165f5809a7a86a9d891a4de78daad3126a64a Mon Sep 17 00:00:00 2001 From: Eugene Shilin Date: Thu, 23 Apr 2026 17:41:16 +0300 Subject: [PATCH 20/39] anti glitch logic for rpm value --- drivers/subsistems-UVH170/UVH170.cpp | 26 +++++++++++++++++++++++--- 1 file changed, 23 insertions(+), 3 deletions(-) diff --git a/drivers/subsistems-UVH170/UVH170.cpp b/drivers/subsistems-UVH170/UVH170.cpp index 5ac6ef0..60533a3 100644 --- a/drivers/subsistems-UVH170/UVH170.cpp +++ b/drivers/subsistems-UVH170/UVH170.cpp @@ -25,6 +25,7 @@ using m_uvhpy_ibat_filt = Mandala; using m_pwr_ign = Mandala; using m_sw_starter = Mandala; using m_eng_ctr = Mandala; +using m_rpm = Mandala; // AGL using m_agl = Mandala; @@ -132,9 +133,13 @@ UVHPU _uvhpu{}; void setRPM(const uint8_t &, const int32_t &); void setCurrent(const uint8_t &, const float &); +float rpm_prev = 0.0f; +static uint32_t same_counter = 0; +const uint32_t SAME_LIMIT = 30; //3 sec at 100ms interval + int main() { - schedule_periodic(task("on_start_eng"), 100); + schedule_periodic(task("on_main"), 100); task("uvhpu"); //GCS with terminal command `vmexec("uvhpu")` @@ -142,21 +147,36 @@ int main() m_sw_starter(); m_eng_ctr(); m_fps_adc(); + m_rpm(); receive(PORT_ID, "on_serial"); } -EXPORT void on_start_eng() +EXPORT void on_main() { + //power ignition logic bool on_power_ignition = (bool) m_pwr_ign::value(); - if (on_power_ignition && (uint32_t) m_sw_starter::value()) { setRPM(VESC_GEN_ID, -40000); //setCurrent(VESC_GEN_ID, 0.0f); } + // calculate fuel pressure from ADC value float fuel_pressure = (m_fps_adc::value() - 0.2f) / 0.6429f; //bar m_fps::publish(fuel_pressure); + + //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 < 500.f) { + m_rpm::publish(0.0f); + same_counter = 0; // сбрасываем, чтобы не зациклиться + } + } else { + rpm_prev = rpm_main; + same_counter = 0; + } } void serializeInt(uint8_t *data, uint8_t index, int32_t value) From bd3bb7a9ac06a5521d5dbe555fe41f0b1b76d467 Mon Sep 17 00:00:00 2001 From: Eugene Shilin Date: Mon, 18 May 2026 11:02:21 +0300 Subject: [PATCH 21/39] fuel script merged to main script --- drivers/subsistems-UVH170/UVH170.cpp | 117 +++++++++++++++++++++- drivers/subsistems-UVH170/dut.cpp | 143 --------------------------- 2 files changed, 116 insertions(+), 144 deletions(-) delete mode 100644 drivers/subsistems-UVH170/dut.cpp diff --git a/drivers/subsistems-UVH170/UVH170.cpp b/drivers/subsistems-UVH170/UVH170.cpp index 60533a3..e8df0c2 100644 --- a/drivers/subsistems-UVH170/UVH170.cpp +++ b/drivers/subsistems-UVH170/UVH170.cpp @@ -1,7 +1,10 @@ #include -const uint8_t PORT_ID{1}; +const port_id_t PORT_ID{1}; +const port_id_t PORT_ID_FUEL{3}; + const uint8_t PACK_SIZE_CAN{12}; +const uint8_t MSG_FUEL_SIZE{9}; //FUEL //Vesc tail using m_vesc_tail_rpm = Mandala; @@ -34,6 +37,41 @@ using m_agl = Mandala; using m_fps_adc = Mandala; using m_fps = Mandala; +// DUT +using m_fuel_p = Mandala; +using m_fuel_l = Mandala; +using m_warn = Mandala; + +const uint16_t TASK_FUEL_MS{500}; //msec + +uint8_t snd_fuel_buf[MSG_FUEL_SIZE] = {}; +uint8_t ADR_FUEL = 170; +const float V_MAX1 = 16.7f; +const uint8_t TIME_SA{5}; //sec +struct _fuel +{ + _fuel(float v_max) + : V_MAX(v_max) + , liters(0.f) + , percent(0.f) + , time_ans(0) + {} + + void set_percent(float p) + { + percent = p; + liters = (p * V_MAX) / 100.f; + time_ans = time_ms(); + } + + float V_MAX; // max tank liters + float liters; // liters + float percent; // percent + uint32_t time_ans; +}; + +_fuel fuel = _fuel(V_MAX1); + //VESC //------------------------------------------------------------------------------------- #define VESC_TAIL_ID 0x24 // VESC ID 36 @@ -140,6 +178,7 @@ const uint32_t SAME_LIMIT = 30; //3 sec at 100ms interval int main() { schedule_periodic(task("on_main"), 100); + schedule_periodic(task("on_fuel"), TASK_FUEL_MS); task("uvhpu"); //GCS with terminal command `vmexec("uvhpu")` @@ -150,6 +189,7 @@ int main() m_rpm(); receive(PORT_ID, "on_serial"); + receive(PORT_ID_FUEL, "on_fuel_serial"); } EXPORT void on_main() @@ -179,6 +219,57 @@ EXPORT void on_main() } } +EXPORT uint8_t calcCRC(const uint8_t *buf, size_t size) +{ + uint8_t crc = 0x00; + + for (uint8_t j = 0; j < size; j++) { + uint8_t i = buf[j] ^ crc; + crc = 0; + if (i & 0x01) + crc ^= 0x5e; + if (i & 0x02) + crc ^= 0xbc; + if (i & 0x04) + crc ^= 0x61; + if (i & 0x08) + crc ^= 0xc2; + if (i & 0x10) + crc ^= 0x9d; + if (i & 0x20) + crc ^= 0x23; + if (i & 0x40) + crc ^= 0x46; + if (i & 0x80) + crc ^= 0x8c; + } + return crc % 256; +} + +void check_answer_time() +{ + uint32_t now = time_ms(); + + m_warn::publish((fuel.time_ans + TIME_SA * 1000 < now) ? true : false); +} + +EXPORT void on_fuel() +{ + check_answer_time(); + + m_fuel_l::publish(fuel.liters); + m_fuel_p::publish(fuel.percent); + + //request fuel sensors + snd_fuel_buf[0] = 0x31; + snd_fuel_buf[2] = 0x06; + + snd_fuel_buf[1] = ADR_FUEL; + + snd_fuel_buf[3] = calcCRC(snd_fuel_buf, 3); + send(PORT_ID_FUEL, snd_fuel_buf, 4, true); +} + void serializeInt(uint8_t *data, uint8_t index, int32_t value) { for (uint8_t i = 0; i < 4; i++) { @@ -393,3 +484,27 @@ EXPORT void on_serial(const uint8_t *data, size_t size) } } } + +EXPORT void on_fuel_serial(const uint8_t *data, size_t size) +{ + //typePack(0)[0x3E] adr(1) fmt(2) data(3) crc(8) + if (size != MSG_FUEL_SIZE) { + return; + } + + if ((data[0] != 0x3E) || (data[2] != 0x06)) { + return; + } + + if (data[8] != calcCRC(data, 8)) { + return; + } + + float fuel_percent = (float) (data[4] | (data[5] << 8)) / 10.f; + + //printf("fuel: %.2f", fuel_percent); + + if (data[1] == ADR_FUEL) { + fuel.set_percent(fuel_percent); + } +} diff --git a/drivers/subsistems-UVH170/dut.cpp b/drivers/subsistems-UVH170/dut.cpp deleted file mode 100644 index 0ed1e82..0000000 --- a/drivers/subsistems-UVH170/dut.cpp +++ /dev/null @@ -1,143 +0,0 @@ -//id 170 -//UART: 19200 8 N 1 - -#include - -static constexpr const port_id_t port_fuel_id{3}; -const uint16_t TASK_FUEL_MS{500}; //msec -const uint8_t ADR_FUEL_SENS1{170}; -const uint8_t MSG_FUEL_SIZE{9}; //FUEL -uint8_t snd_fuel_buf[MSG_FUEL_SIZE] = {}; -uint8_t ADR_FUEL[1] = {ADR_FUEL_SENS1}; -const float V_MAX1 = 16.7f; -const uint8_t TIME_SA{5}; //sec - -struct _fuel -{ - _fuel(float v_max) - : V_MAX(v_max) - , liters(0.f) - , percent(0.f) - , time_ans(0) - {} - - void set_percent(float p) - { - percent = p; - liters = (p * V_MAX) / 100.f; - time_ans = time_ms(); - } - - float V_MAX; // max tank liters - float liters; // liters - float percent; // percent - uint32_t time_ans; -}; - -_fuel fuel[1] = {_fuel(V_MAX1)}; - - -//fuel -using m_fuel1 = Mandala; - -using m_fuel_p = Mandala; -using m_fuel_l = Mandala; - -using m_warn1 = Mandala; - - -int main() -{ - - schedule_periodic(task("on_fuel"), TASK_FUEL_MS); - - receive(port_fuel_id, "on_fuel_serial"); - - //fuel - m_fuel_p(); - m_fuel_l(); - m_fuel1(); - m_warn1(); - - return 0; -} - -EXPORT uint8_t calcCRC(const uint8_t *buf, size_t size) -{ - uint8_t crc = 0x00; - - for (uint8_t j = 0; j < size; j++) { - uint8_t i = buf[j] ^ crc; - crc = 0; - if (i & 0x01) - crc ^= 0x5e; - if (i & 0x02) - crc ^= 0xbc; - if (i & 0x04) - crc ^= 0x61; - if (i & 0x08) - crc ^= 0xc2; - if (i & 0x10) - crc ^= 0x9d; - if (i & 0x20) - crc ^= 0x23; - if (i & 0x40) - crc ^= 0x46; - if (i & 0x80) - crc ^= 0x8c; - } - return crc % 256; -} - - - -void check_answer_time() -{ - uint32_t now = time_ms(); - - m_warn1::publish((fuel[0].time_ans + TIME_SA * 1000 < now) ? true : false); -} - -EXPORT void on_fuel() -{ - check_answer_time(); - - m_fuel1::publish(fuel[0].liters); - - m_fuel_l::publish(fuel[0].liters); - m_fuel_p::publish(fuel[0].percent); - - - //request fuel sensors - snd_fuel_buf[0] = 0x31; - snd_fuel_buf[2] = 0x06; - - snd_fuel_buf[1] = ADR_FUEL[0]; - - snd_fuel_buf[3] = calcCRC(snd_fuel_buf, 3); - send(port_fuel_id, snd_fuel_buf, 4, true); -} - -EXPORT void on_fuel_serial(const uint8_t *data, size_t size) -{ - //typePack(0)[0x3E] adr(1) fmt(2) data(3) crc(8) - if (size != MSG_FUEL_SIZE) { - return; - } - - if ((data[0] != 0x3E) || (data[2] != 0x06)) { - return; - } - - if (data[8] != calcCRC(data, 8)) { - return; - } - - float fuel_percent = (float) (data[4] | (data[5] << 8)) / 10.f; - - //printf("fuel: %.2f", fuel_percent); - - if (data[1] == ADR_FUEL[0]) { - fuel[0].set_percent(fuel_percent); - } -} From 24c02d57d30b648c106a54c46d328a56a96062b6 Mon Sep 17 00:00:00 2001 From: Eugene Shilin Date: Tue, 19 May 2026 12:29:20 +0300 Subject: [PATCH 22/39] fix ECU vcp message --- drivers/subsistems-UVH170/ECU.cpp | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/drivers/subsistems-UVH170/ECU.cpp b/drivers/subsistems-UVH170/ECU.cpp index 2bf1852..1497e37 100644 --- a/drivers/subsistems-UVH170/ECU.cpp +++ b/drivers/subsistems-UVH170/ECU.cpp @@ -1,8 +1,8 @@ #include -const uint8_t PORT_ID{2}; +const uint8_t PORT_ID{33}; const uint8_t PACK_SIZE_CAN{12}; -const uint8_t TASK_ECU_MS{200}; +const uint8_t TASK_ECU_MS{50}; const uint16_t CAN_BASE_ADDR{1520}; const uint8_t OFFSET_CAN_ID_CURRENT_THROTTLE{71}; @@ -37,15 +37,14 @@ void setThrottleIgn() data[1] = (CAN_ID_CURRENT_THROTTLE >> 8) & 0xFF; data[2] = (CAN_ID_CURRENT_THROTTLE >> 16) & 0xFF; data[3] = (CAN_ID_CURRENT_THROTTLE >> 24) & 0xFF; - data[4] = 5; //size of useful payload - float ch_throttle = m_eng_ctr::value() * 2.f - 1.f; // -1.0 ... +1.0 - memcpy(&data[5], &ch_throttle, 4); + float ch_throttle = m_eng_ctr::value(); // 0 ... +1.0 //printf("thr:%.2f", ch_throttle); + memcpy(&data[4], &ch_throttle, 4); // copy float bytes to data uint8_t power_eng = (bool) m_pwr_ign::value(); // 0 / 1 data[9] = power_eng; - send(PORT_ID, data, 10, true); + send(PORT_ID, data, 9, true); } EXPORT void on_ecu() From 50b428b8b7aa63c7a47bbf746ab799ca8ccb5b5e Mon Sep 17 00:00:00 2001 From: Eugene Shilin Date: Tue, 19 May 2026 12:39:24 +0300 Subject: [PATCH 23/39] fix ignition field in message --- drivers/subsistems-UVH170/ECU.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/drivers/subsistems-UVH170/ECU.cpp b/drivers/subsistems-UVH170/ECU.cpp index 1497e37..e4f1a5f 100644 --- a/drivers/subsistems-UVH170/ECU.cpp +++ b/drivers/subsistems-UVH170/ECU.cpp @@ -43,7 +43,7 @@ void setThrottleIgn() memcpy(&data[4], &ch_throttle, 4); // copy float bytes to data uint8_t power_eng = (bool) m_pwr_ign::value(); // 0 / 1 - data[9] = power_eng; + data[8] = power_eng; send(PORT_ID, data, 9, true); } From b60d48cc2688b5ecaf3c51ae348d51da8e8d9c67 Mon Sep 17 00:00:00 2001 From: Eugene Shilin Date: Tue, 19 May 2026 13:56:00 +0300 Subject: [PATCH 24/39] eng starter procedure implemented --- drivers/subsistems-UVH170/UVH170.cpp | 31 +++++++++++++++++++++++++--- 1 file changed, 28 insertions(+), 3 deletions(-) diff --git a/drivers/subsistems-UVH170/UVH170.cpp b/drivers/subsistems-UVH170/UVH170.cpp index e8df0c2..f83dcb1 100644 --- a/drivers/subsistems-UVH170/UVH170.cpp +++ b/drivers/subsistems-UVH170/UVH170.cpp @@ -167,11 +167,19 @@ UVHPU _uvhpu{}; //------------------------------------------------------------------------------------- #define AGL_CAN_ID 0x00090002 -//------------------------------------------------------------------------------------- +//------------- STARTER CONFIG --------------------------------------------------------- +const uint8_t STARTER_RPM_TIME = 30; //3 sec at 100ms interval +const uint8_t STARTER_CURRENT_TIME = 5; //0.5 sec at 100ms interval +const int32_t STARTER_RPM = 20000; // ~1000 rpm for starter +const float STARTER_CURRENT = 20.f; // 20A for starter + void setRPM(const uint8_t &, const int32_t &); void setCurrent(const uint8_t &, const float &); +uint8_t currentStarterCnt = STARTER_CURRENT_TIME; +uint8_t rpmStarterCnt = STARTER_RPM_TIME; float rpm_prev = 0.0f; +bool starter_active = false; static uint32_t same_counter = 0; const uint32_t SAME_LIMIT = 30; //3 sec at 100ms interval @@ -197,8 +205,25 @@ EXPORT void on_main() //power ignition logic bool on_power_ignition = (bool) m_pwr_ign::value(); if (on_power_ignition && (uint32_t) m_sw_starter::value()) { - setRPM(VESC_GEN_ID, -40000); - //setCurrent(VESC_GEN_ID, 0.0f); + starter_active = true; + } + + if (starter_active) { + m_eng_ctr::publish(0.0f); //throttle to 0 during starter + if (currentStarterCnt > 0) { //current phase of starter + setCurrent(VESC_GEN_ID, -STARTER_CURRENT); + currentStarterCnt--; + } else { //rpm phase of starter + if (rpmStarterCnt > 0) { + setRPM(VESC_GEN_ID, STARTER_RPM); + rpmStarterCnt--; + } else { + starter_active = false; + currentStarterCnt = STARTER_CURRENT_TIME; + rpmStarterCnt = STARTER_RPM_TIME; + setRPM(VESC_GEN_ID, 0); + } + } } // calculate fuel pressure from ADC value From 7e2181ed4de6182b0fea0560dc82c853599451d6 Mon Sep 17 00:00:00 2001 From: Eugene Shilin Date: Tue, 19 May 2026 14:03:53 +0300 Subject: [PATCH 25/39] starter procedure optimized --- drivers/subsistems-UVH170/UVH170.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/drivers/subsistems-UVH170/UVH170.cpp b/drivers/subsistems-UVH170/UVH170.cpp index f83dcb1..a030d3e 100644 --- a/drivers/subsistems-UVH170/UVH170.cpp +++ b/drivers/subsistems-UVH170/UVH170.cpp @@ -213,7 +213,8 @@ EXPORT void on_main() if (currentStarterCnt > 0) { //current phase of starter setCurrent(VESC_GEN_ID, -STARTER_CURRENT); currentStarterCnt--; - } else { //rpm phase of starter + } + if (currentStarterCnt == 0) { //rpm phase of starter if (rpmStarterCnt > 0) { setRPM(VESC_GEN_ID, STARTER_RPM); rpmStarterCnt--; From 0bdb372d64f14ee595ef73b2b3aebcc74fa625ad Mon Sep 17 00:00:00 2001 From: Eugene Shilin Date: Thu, 21 May 2026 13:56:19 +0300 Subject: [PATCH 26/39] ECU values refactored --- drivers/subsistems-UVH170/ECU.cpp | 85 ++++++++++++++++++++++--------- 1 file changed, 60 insertions(+), 25 deletions(-) diff --git a/drivers/subsistems-UVH170/ECU.cpp b/drivers/subsistems-UVH170/ECU.cpp index e4f1a5f..53db38c 100644 --- a/drivers/subsistems-UVH170/ECU.cpp +++ b/drivers/subsistems-UVH170/ECU.cpp @@ -9,9 +9,18 @@ const uint8_t OFFSET_CAN_ID_CURRENT_THROTTLE{71}; const uint16_t CAN_ID_CURRENT_THROTTLE = CAN_BASE_ADDR + OFFSET_CAN_ID_CURRENT_THROTTLE; //u2 u3 u4 u10 are already used in adc & gpio -using m_oil_temp = Mandala; -using m_cool_temp = Mandala; -using m_oil_press = Mandala; +//ECU values +using m_mat = Mandala; +using m_clt = Mandala; +using m_baro = Mandala; +using m_map = Mandala; +using m_ego = Mandala; +using m_tps = Mandala; +using m_vbat = Mandala; +using m_pw1 = Mandala; +using m_pw2 = Mandala; +using m_rpm = Mandala; + using m_egt_1 = Mandala; using m_egt_2 = Mandala; using m_egt_3 = Mandala; @@ -61,44 +70,38 @@ EXPORT void on_serial(const uint8_t *data, size_t size) 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 - //printf("can_id:%x\n", can_id); + //printf("can_id:%d\n", can_id - 1520); uint8_t can_data[8] = {}; for (uint8_t i = 0; i < 8; i++) { can_data[i] = data[4 + i]; // 4 is data position } - //CAN ID 0XFFFF switch (can_id & 0xFFFF) { + // baro, map, mat, clt case CAN_BASE_ADDR + 2: { - //printf("temps"); - float param; - int16_t raw; - - raw = (int16_t) (data[8] << 8) + data[9]; - param = float(raw); - float oil_temp = (param / 10.0f - 32.0f) * 5.0f / 9.0f; - m_oil_temp::publish(oil_temp); // Oil temperature [deg] - - raw = (int16_t) (data[10] << 8) + data[11]; - param = float(raw); - float coolant_temp = (param / 10.0f - 32.0f) * 5.0f / 9.0f; - m_cool_temp::publish(coolant_temp); // Coolant temperature [deg] + float baro = ((int16_t) data[4] << 8 | data[5]) / 10.f; // kPa + float map = ((int16_t) data[6] << 8 | data[7]) / 10.f; // kPa + float mat_F = ((int16_t) data[8] << 8 | data[9]) / 10.f; // deg F + float clt_F = ((int16_t) data[10] << 8 | data[11]) / 10.f; // deg F + + float mat_C = (mat_F - 32.0f) * 5.0f / 9.0f; + float clt_C = (clt_F - 32.0f) * 5.0f / 9.0f; + + m_mat::publish(mat_C); // manifold air temperature [deg C] + m_clt::publish(clt_C); // Coolant temperature [deg C] + m_baro::publish(baro); // Barometric pressure [kPa] + m_map::publish(map); // Manifold absolute pressure [kPa] break; } case CAN_BASE_ADDR + 15: { - //printf("oil pressure"); - uint16_t raw = (uint16_t) ((data[4] << 8) | data[5]); // raw param - float param = float(raw); - - //float oil_pressure = (float(param)*0.02822581)-3.27822581); //Oil pressure - float oil_pressure = (param * 0.02083333f) - 7.02083333f; - m_oil_press::publish(oil_pressure); + // oil_press.not used break; } + // egt case CAN_BASE_ADDR + 22: { //printf("egt"); float egt[4]; @@ -114,5 +117,37 @@ EXPORT void on_serial(const uint8_t *data, size_t size) m_egt_4::publish(egt[3] / 10.f); //EGT4 break; } + + //pw1 pw2 rpm + case CAN_BASE_ADDR + 0: { + //uint16_t seconds = data[4] << 8 | data[5]; + uint16_t pw1 = ((uint16_t) data[6] << 8 | data[7]); //us + uint16_t pw2 = ((uint16_t) data[8] << 8 | data[9]); //us + uint16_t rpm = data[10] << 8 | data[11]; + + m_pw1::publish((uint32_t) pw1 / 1000); // pulse width 1 [ms] + m_pw2::publish((uint32_t) pw2 / 1000); // pulse width 2 [ms] + m_rpm::publish((uint32_t) rpm); // engine rpm + + break; + } + + //tps, Vbat + case CAN_BASE_ADDR + 3: { + float tps = ((int16_t) (data[4] << 8) | data[5]) / 10.f; // % + float vbat = ((int16_t) data[6] << 8 | data[7]) / 10.f; // V + float ego = ((int16_t) (data[8] << 8) | data[9]) / 10.f; // lambda + //printf("ego: %.2f", ego); + + m_tps::publish(tps); // Throttle position [%] + m_vbat::publish(vbat); // Battery voltage [V] + m_ego::publish(ego); // Exhaust gas oxygen sensor [lambda] + break; + } + //?? + case CAN_BASE_ADDR + 72: { + //not used + break; + } } } From 37cf26c3b17c2d8802bff2073366b978eee98a06 Mon Sep 17 00:00:00 2001 From: Eugene Shilin Date: Fri, 22 May 2026 10:23:57 +0300 Subject: [PATCH 27/39] change user variables --- drivers/subsistems-UVH170/ECU.cpp | 16 ++++++++-------- drivers/subsistems-UVH170/UVH170.cpp | 4 ++-- 2 files changed, 10 insertions(+), 10 deletions(-) diff --git a/drivers/subsistems-UVH170/ECU.cpp b/drivers/subsistems-UVH170/ECU.cpp index 53db38c..7cf15e7 100644 --- a/drivers/subsistems-UVH170/ECU.cpp +++ b/drivers/subsistems-UVH170/ECU.cpp @@ -17,14 +17,14 @@ using m_map = Mandala; using m_ego = Mandala; using m_tps = Mandala; using m_vbat = Mandala; -using m_pw1 = Mandala; -using m_pw2 = Mandala; -using m_rpm = Mandala; - -using m_egt_1 = Mandala; -using m_egt_2 = Mandala; -using m_egt_3 = Mandala; -using m_egt_4 = Mandala; + +using m_pw1 = Mandala; +using m_pw2 = Mandala; +using m_rpm = Mandala; +using m_egt_1 = Mandala; +using m_egt_2 = Mandala; +using m_egt_3 = Mandala; +using m_egt_4 = Mandala; using m_eng_ctr = Mandala; using m_pwr_ign = Mandala; diff --git a/drivers/subsistems-UVH170/UVH170.cpp b/drivers/subsistems-UVH170/UVH170.cpp index a030d3e..27671f0 100644 --- a/drivers/subsistems-UVH170/UVH170.cpp +++ b/drivers/subsistems-UVH170/UVH170.cpp @@ -21,7 +21,7 @@ using m_vesc_gen_motor_temp = Mandala; using m_vesc_gen_fet_temp = Mandala; //Uvhpy -using m_uvhpy_status = Mandala; +using m_uvhpy_status = Mandala; using m_uvhpy_ibat_filt = Mandala; //Engine @@ -40,7 +40,7 @@ using m_fps = Mandala; // DUT using m_fuel_p = Mandala; using m_fuel_l = Mandala; -using m_warn = Mandala; +using m_warn = Mandala; const uint16_t TASK_FUEL_MS{500}; //msec From 1c1520f59b360b3740562302dfab3e5193fac5b2 Mon Sep 17 00:00:00 2001 From: Eugene Shilin Date: Fri, 22 May 2026 10:48:57 +0300 Subject: [PATCH 28/39] starter time decreased --- drivers/subsistems-UVH170/UVH170.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/drivers/subsistems-UVH170/UVH170.cpp b/drivers/subsistems-UVH170/UVH170.cpp index 27671f0..ad5e9b3 100644 --- a/drivers/subsistems-UVH170/UVH170.cpp +++ b/drivers/subsistems-UVH170/UVH170.cpp @@ -168,7 +168,7 @@ UVHPU _uvhpu{}; #define AGL_CAN_ID 0x00090002 //------------- STARTER CONFIG --------------------------------------------------------- -const uint8_t STARTER_RPM_TIME = 30; //3 sec at 100ms interval +const uint8_t STARTER_RPM_TIME = 20; //2 sec at 100ms interval const uint8_t STARTER_CURRENT_TIME = 5; //0.5 sec at 100ms interval const int32_t STARTER_RPM = 20000; // ~1000 rpm for starter const float STARTER_CURRENT = 20.f; // 20A for starter From 92b33f40bcb3c077a3016ee29168523ab834d8e3 Mon Sep 17 00:00:00 2001 From: Eugene Shilin Date: Fri, 22 May 2026 11:07:41 +0300 Subject: [PATCH 29/39] starter time 3sec --- drivers/subsistems-UVH170/UVH170.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/drivers/subsistems-UVH170/UVH170.cpp b/drivers/subsistems-UVH170/UVH170.cpp index ad5e9b3..27671f0 100644 --- a/drivers/subsistems-UVH170/UVH170.cpp +++ b/drivers/subsistems-UVH170/UVH170.cpp @@ -168,7 +168,7 @@ UVHPU _uvhpu{}; #define AGL_CAN_ID 0x00090002 //------------- STARTER CONFIG --------------------------------------------------------- -const uint8_t STARTER_RPM_TIME = 20; //2 sec at 100ms interval +const uint8_t STARTER_RPM_TIME = 30; //3 sec at 100ms interval const uint8_t STARTER_CURRENT_TIME = 5; //0.5 sec at 100ms interval const int32_t STARTER_RPM = 20000; // ~1000 rpm for starter const float STARTER_CURRENT = 20.f; // 20A for starter From 4d2145cb35c63a6af4990aeabc0b204b39b47847 Mon Sep 17 00:00:00 2001 From: Eugene Shilin Date: Fri, 22 May 2026 11:13:17 +0300 Subject: [PATCH 30/39] throttle during start moved to config --- drivers/subsistems-UVH170/UVH170.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/drivers/subsistems-UVH170/UVH170.cpp b/drivers/subsistems-UVH170/UVH170.cpp index 27671f0..4873896 100644 --- a/drivers/subsistems-UVH170/UVH170.cpp +++ b/drivers/subsistems-UVH170/UVH170.cpp @@ -172,6 +172,7 @@ const uint8_t STARTER_RPM_TIME = 30; //3 sec at 100ms interval const uint8_t STARTER_CURRENT_TIME = 5; //0.5 sec at 100ms interval const int32_t STARTER_RPM = 20000; // ~1000 rpm for starter const float STARTER_CURRENT = 20.f; // 20A for starter +const float STARTER_THROTTLE = 0.01f; // 1% throttle during starter void setRPM(const uint8_t &, const int32_t &); void setCurrent(const uint8_t &, const float &); @@ -209,8 +210,8 @@ EXPORT void on_main() } if (starter_active) { - m_eng_ctr::publish(0.0f); //throttle to 0 during starter - if (currentStarterCnt > 0) { //current phase of starter + m_eng_ctr::publish(STARTER_THROTTLE); //throttle to 1% during starter + if (currentStarterCnt > 0) { //current phase of starter setCurrent(VESC_GEN_ID, -STARTER_CURRENT); currentStarterCnt--; } From c7af2aad9c12bfe840a33b9ac1bd5bdf3fe181f2 Mon Sep 17 00:00:00 2001 From: Eugene Shilin Date: Mon, 25 May 2026 17:05:50 +0300 Subject: [PATCH 31/39] power unit user variables --- drivers/subsistems-UVH170/UVH170.cpp | 23 ++++++++++++++++++----- 1 file changed, 18 insertions(+), 5 deletions(-) diff --git a/drivers/subsistems-UVH170/UVH170.cpp b/drivers/subsistems-UVH170/UVH170.cpp index 4873896..ea5a63b 100644 --- a/drivers/subsistems-UVH170/UVH170.cpp +++ b/drivers/subsistems-UVH170/UVH170.cpp @@ -20,9 +20,15 @@ using m_vesc_gen_curr_in = Mandala; using m_vesc_gen_motor_temp = Mandala; using m_vesc_gen_fet_temp = Mandala; -//Uvhpy -using m_uvhpy_status = Mandala; -using m_uvhpy_ibat_filt = Mandala; +//Uvhpu +using m_uvhpu_vbat = Mandala; +using m_uvhpu_vbat_filt = Mandala; +using m_uvhpu_pbat = Mandala; +using m_uvhpu_status = Mandala; +using m_uvhpu_ibat_filt = Mandala; +using m_uvhpu_cbat = Mandala; +using m_uvhpu_ebat = Mandala; +using m_uvhpu_ibat = Mandala; //Engine using m_pwr_ign = Mandala; @@ -317,6 +323,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); break; } case UVHPU_PACK2: { @@ -325,11 +334,14 @@ 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); 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: { @@ -338,7 +350,8 @@ 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_uvhpu_ibat_filt::publish(_uvhpu.MSG5.ibat_filt); + m_uvhpu_vbat_filt::publish(_uvhpu.MSG5.vbat_filt); break; } case UVHPU_PACK6: { From 227c889865bbf2dcd80c19cdf35962ca9c6c2bc1 Mon Sep 17 00:00:00 2001 From: Eugene Shilin Date: Tue, 26 May 2026 14:08:51 +0300 Subject: [PATCH 32/39] user variables changed --- drivers/subsistems-UVH170/UVH170.cpp | 30 +++++++++++++++------------- 1 file changed, 16 insertions(+), 14 deletions(-) diff --git a/drivers/subsistems-UVH170/UVH170.cpp b/drivers/subsistems-UVH170/UVH170.cpp index ea5a63b..d4f658a 100644 --- a/drivers/subsistems-UVH170/UVH170.cpp +++ b/drivers/subsistems-UVH170/UVH170.cpp @@ -12,23 +12,24 @@ 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; //Vesc gen using m_vesc_gen_rpm = Mandala; using m_vesc_gen_curr_in = Mandala; using m_vesc_gen_motor_temp = Mandala; -using m_vesc_gen_fet_temp = Mandala; +using m_vesc_gen_fet_temp = Mandala; //Uvhpu -using m_uvhpu_vbat = Mandala; -using m_uvhpu_vbat_filt = Mandala; -using m_uvhpu_pbat = Mandala; -using m_uvhpu_status = Mandala; -using m_uvhpu_ibat_filt = Mandala; -using m_uvhpu_cbat = Mandala; -using m_uvhpu_ebat = Mandala; -using m_uvhpu_ibat = Mandala; +using m_uvhpu_vbat = Mandala; +//using m_uvhpu_vbat_filt = Mandala; +using m_uvhpu_pbat = Mandala; +using m_uvhpu_status = Mandala; +//using m_uvhpu_ibat_filt = Mandala; +using m_uvhpu_cbat = Mandala; +using m_uvhpu_ebat = Mandala; +using m_uvhpu_ibat = Mandala; +using m_uvhpu_tbat = Mandala; //Engine using m_pwr_ign = Mandala; @@ -37,7 +38,7 @@ using m_eng_ctr = Mandala; using m_rpm = Mandala; // AGL -using m_agl = Mandala; +using m_agl = Mandala; //fuel pressure ADC using m_fps_adc = Mandala; @@ -46,7 +47,7 @@ using m_fps = Mandala; // DUT using m_fuel_p = Mandala; using m_fuel_l = Mandala; -using m_warn = Mandala; +using m_warn = Mandala; const uint16_t TASK_FUEL_MS{500}; //msec @@ -336,6 +337,7 @@ void processUVHPUackage(const uint32_t &can_id, const uint8_t *data) 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: { @@ -350,8 +352,8 @@ void processUVHPUackage(const uint32_t &can_id, const uint8_t *data) } case UVHPU_PACK5: { memcpy(&_uvhpu.MSG5.ibat_filt, data, 8); - m_uvhpu_ibat_filt::publish(_uvhpu.MSG5.ibat_filt); - m_uvhpu_vbat_filt::publish(_uvhpu.MSG5.vbat_filt); + //m_uvhpu_ibat_filt::publish(_uvhpu.MSG5.ibat_filt); + //m_uvhpu_vbat_filt::publish(_uvhpu.MSG5.vbat_filt); break; } case UVHPU_PACK6: { From 35d7c8f178939ea723264a5b49225589268552e1 Mon Sep 17 00:00:00 2001 From: Eugene Shilin Date: Thu, 28 May 2026 15:58:48 +0300 Subject: [PATCH 33/39] pu hold mechanism --- drivers/subsistems-UVH170/UVH170.cpp | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/drivers/subsistems-UVH170/UVH170.cpp b/drivers/subsistems-UVH170/UVH170.cpp index d4f658a..ee4d595 100644 --- a/drivers/subsistems-UVH170/UVH170.cpp +++ b/drivers/subsistems-UVH170/UVH170.cpp @@ -22,14 +22,14 @@ using m_vesc_gen_fet_temp = Mandala; //Uvhpu using m_uvhpu_vbat = Mandala; -//using m_uvhpu_vbat_filt = Mandala; using m_uvhpu_pbat = Mandala; using m_uvhpu_status = Mandala; -//using m_uvhpu_ibat_filt = 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 using m_pwr_ign = Mandala; @@ -203,6 +203,7 @@ int main() m_eng_ctr(); m_fps_adc(); m_rpm(); + m_procedure(); receive(PORT_ID, "on_serial"); receive(PORT_ID_FUEL, "on_fuel_serial"); @@ -251,6 +252,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); + } } EXPORT uint8_t calcCRC(const uint8_t *buf, size_t size) From 32c4e983e19ea632818fbc27daa3f906059aded9 Mon Sep 17 00:00:00 2001 From: Eugene Shilin Date: Thu, 28 May 2026 16:01:04 +0300 Subject: [PATCH 34/39] rotor rpm variable changed --- drivers/subsistems-UVH170/UVH170.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/drivers/subsistems-UVH170/UVH170.cpp b/drivers/subsistems-UVH170/UVH170.cpp index ee4d595..d662fcf 100644 --- a/drivers/subsistems-UVH170/UVH170.cpp +++ b/drivers/subsistems-UVH170/UVH170.cpp @@ -35,7 +35,7 @@ using m_procedure = Mandala; using m_pwr_ign = Mandala; using m_sw_starter = Mandala; using m_eng_ctr = Mandala; -using m_rpm = Mandala; +using m_rotor_rpm = Mandala; // AGL using m_agl = Mandala; @@ -202,7 +202,7 @@ int main() m_sw_starter(); m_eng_ctr(); m_fps_adc(); - m_rpm(); + m_rotor_rpm(); m_procedure(); receive(PORT_ID, "on_serial"); @@ -241,12 +241,12 @@ EXPORT void on_main() m_fps::publish(fuel_pressure); //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 < 500.f) { - m_rpm::publish(0.0f); - same_counter = 0; // сбрасываем, чтобы не зациклиться + m_rotor_rpm::publish(0.0f); + same_counter = 0; } } else { rpm_prev = rpm_main; From b069c80c049bf289b8f682d4db2c4743d8f83b51 Mon Sep 17 00:00:00 2001 From: Eugene Shilin Date: Wed, 3 Jun 2026 11:27:54 +0300 Subject: [PATCH 35/39] change ecu port id to work with plugin --- drivers/subsistems-UVH170/ECU.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/drivers/subsistems-UVH170/ECU.cpp b/drivers/subsistems-UVH170/ECU.cpp index 7cf15e7..f1fcb40 100644 --- a/drivers/subsistems-UVH170/ECU.cpp +++ b/drivers/subsistems-UVH170/ECU.cpp @@ -1,6 +1,6 @@ #include -const uint8_t PORT_ID{33}; +const uint8_t PORT_ID{8}; const uint8_t PACK_SIZE_CAN{12}; const uint8_t TASK_ECU_MS{50}; From f4fbc53eedba4ac4637d98bc10bd187e2065f5f7 Mon Sep 17 00:00:00 2001 From: Eugene Shilin Date: Tue, 25 Aug 2026 10:21:36 +0100 Subject: [PATCH 36/39] ECU variables updated --- drivers/subsistems-UVH170/ECU.cpp | 30 +++++++++++++----------------- 1 file changed, 13 insertions(+), 17 deletions(-) diff --git a/drivers/subsistems-UVH170/ECU.cpp b/drivers/subsistems-UVH170/ECU.cpp index f1fcb40..894efa7 100644 --- a/drivers/subsistems-UVH170/ECU.cpp +++ b/drivers/subsistems-UVH170/ECU.cpp @@ -2,7 +2,7 @@ const uint8_t PORT_ID{8}; const uint8_t PACK_SIZE_CAN{12}; -const uint8_t TASK_ECU_MS{50}; +const uint8_t TASK_ECU_MS{10}; const uint16_t CAN_BASE_ADDR{1520}; const uint8_t OFFSET_CAN_ID_CURRENT_THROTTLE{71}; @@ -10,21 +10,20 @@ const uint16_t CAN_ID_CURRENT_THROTTLE = CAN_BASE_ADDR + OFFSET_CAN_ID_CURRENT_T //u2 u3 u4 u10 are already used in adc & gpio //ECU values -using m_mat = Mandala; -using m_clt = Mandala; +using m_ego = Mandala; +using m_clt = Mandala; using m_baro = Mandala; using m_map = Mandala; -using m_ego = Mandala; using m_tps = Mandala; using m_vbat = Mandala; +using m_mat = Mandala; -using m_pw1 = Mandala; -using m_pw2 = Mandala; -using m_rpm = Mandala; -using m_egt_1 = Mandala; -using m_egt_2 = Mandala; -using m_egt_3 = Mandala; -using m_egt_4 = Mandala; +using m_egt_1 = Mandala; +using m_egt_2 = Mandala; +using m_egt_3 = Mandala; +using m_egt_4 = Mandala; + +using m_rpm = Mandala; using m_eng_ctr = Mandala; using m_pwr_ign = Mandala; @@ -121,13 +120,10 @@ EXPORT void on_serial(const uint8_t *data, size_t size) //pw1 pw2 rpm case CAN_BASE_ADDR + 0: { //uint16_t seconds = data[4] << 8 | data[5]; - uint16_t pw1 = ((uint16_t) data[6] << 8 | data[7]); //us - uint16_t pw2 = ((uint16_t) data[8] << 8 | data[9]); //us + //uint16_t pw1 = ((uint16_t) data[6] << 8 | data[7]); //us + //uint16_t pw2 = ((uint16_t) data[8] << 8 | data[9]); //us uint16_t rpm = data[10] << 8 | data[11]; - - m_pw1::publish((uint32_t) pw1 / 1000); // pulse width 1 [ms] - m_pw2::publish((uint32_t) pw2 / 1000); // pulse width 2 [ms] - m_rpm::publish((uint32_t) rpm); // engine rpm + m_rpm::publish((uint32_t) rpm); // engine rpm break; } From 01330095b41c3339997f895c7620ffd17bd76639 Mon Sep 17 00:00:00 2001 From: Eugene Shilin Date: Tue, 25 Aug 2026 10:25:23 +0100 Subject: [PATCH 37/39] main script updated --- drivers/subsistems-UVH170/UVH170.cpp | 134 +++++++++++++++++---------- 1 file changed, 83 insertions(+), 51 deletions(-) diff --git a/drivers/subsistems-UVH170/UVH170.cpp b/drivers/subsistems-UVH170/UVH170.cpp index d662fcf..68c5f6e 100644 --- a/drivers/subsistems-UVH170/UVH170.cpp +++ b/drivers/subsistems-UVH170/UVH170.cpp @@ -1,59 +1,63 @@ #include -const port_id_t PORT_ID{1}; -const port_id_t PORT_ID_FUEL{3}; +const port_id_t PORT_ID{11}; +const port_id_t PORT_ID_FUEL{12}; const uint8_t PACK_SIZE_CAN{12}; const uint8_t MSG_FUEL_SIZE{9}; //FUEL //Vesc tail -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; +using m_vesc_tail_curr_in = Mandala; +using m_vesc_tail_rpm = Mandala; +using m_vesc_tail_duty = Mandala; +using m_vesc_tail_temp_fet = Mandala; +using m_vesc_tail_temp_motor = Mandala; //Vesc gen using m_vesc_gen_rpm = Mandala; using m_vesc_gen_curr_in = Mandala; using m_vesc_gen_motor_temp = Mandala; -using m_vesc_gen_fet_temp = Mandala; +using m_vesc_gen_fet_temp = 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_status = Mandala; +using m_uvhpu_ebat = Mandala; + using m_uvhpu_hold = Mandala; using m_procedure = Mandala; +using m_uvhpu_vsys = Mandala; + //Engine using m_pwr_ign = Mandala; using m_sw_starter = Mandala; using m_eng_ctr = Mandala; -using m_rotor_rpm = Mandala; +using m_rotor_rpm = Mandala; // AGL using m_agl = Mandala; //fuel pressure ADC -using m_fps_adc = Mandala; +using m_fps_adc = Mandala; using m_fps = Mandala; // DUT using m_fuel_p = Mandala; -using m_fuel_l = Mandala; +using m_fuel_l = Mandala; using m_warn = Mandala; +// trim rudder +using m_reg_yaw = Mandala; +using m_trim_rudder_l = Mandala; + const uint16_t TASK_FUEL_MS{500}; //msec uint8_t snd_fuel_buf[MSG_FUEL_SIZE] = {}; uint8_t ADR_FUEL = 170; -const float V_MAX1 = 16.7f; +const float V_MAX1 = 12.9f; const uint8_t TIME_SA{5}; //sec struct _fuel { @@ -175,11 +179,11 @@ UVHPU _uvhpu{}; #define AGL_CAN_ID 0x00090002 //------------- STARTER CONFIG --------------------------------------------------------- -const uint8_t STARTER_RPM_TIME = 30; //3 sec at 100ms interval -const uint8_t STARTER_CURRENT_TIME = 5; //0.5 sec at 100ms interval -const int32_t STARTER_RPM = 20000; // ~1000 rpm for starter -const float STARTER_CURRENT = 20.f; // 20A for starter -const float STARTER_THROTTLE = 0.01f; // 1% throttle during starter +const uint8_t STARTER_RPM_TIME = 70; //10 = 1 sec +const uint8_t STARTER_CURRENT_TIME = 0; //0.5 sec at 100ms interval +const int32_t STARTER_RPM = 23000; // ~1000 rpm for starter +//const float STARTER_CURRENT = 200.f; // starter current +const float STARTER_THROTTLE = 0.02f; // 2% throttle during starter void setRPM(const uint8_t &, const int32_t &); void setCurrent(const uint8_t &, const float &); @@ -191,10 +195,22 @@ bool starter_active = false; static uint32_t same_counter = 0; const uint32_t SAME_LIMIT = 30; //3 sec at 100ms interval +constexpr const uint16_t SERVO_TASK_MS{20}; +constexpr const float F = 2.f; +constexpr const float Ampl = 1.f; +constexpr const float w = 2.f * PI * F; +constexpr const float T = 1.f / F; + +float t = 0.f; + +using m_sin_test = Mandala; + int main() { schedule_periodic(task("on_main"), 100); + schedule_periodic(task("on_start_eng"), 100); schedule_periodic(task("on_fuel"), TASK_FUEL_MS); + //schedule_periodic(task("on_test_servo"), SERVO_TASK_MS); task("uvhpu"); //GCS with terminal command `vmexec("uvhpu")` @@ -204,36 +220,30 @@ int main() m_fps_adc(); m_rotor_rpm(); m_procedure(); + m_reg_yaw(); receive(PORT_ID, "on_serial"); receive(PORT_ID_FUEL, "on_fuel_serial"); } -EXPORT void on_main() +EXPORT void on_test_servo() { - //power ignition logic - bool on_power_ignition = (bool) m_pwr_ign::value(); - if (on_power_ignition && (uint32_t) m_sw_starter::value()) { - starter_active = true; + float ctrl = Ampl * sin(w * t); + + t += SERVO_TASK_MS / 1000.f; + if (t >= T) { + t = 0.f; } - if (starter_active) { - m_eng_ctr::publish(STARTER_THROTTLE); //throttle to 1% during starter - if (currentStarterCnt > 0) { //current phase of starter - setCurrent(VESC_GEN_ID, -STARTER_CURRENT); - currentStarterCnt--; - } - if (currentStarterCnt == 0) { //rpm phase of starter - if (rpmStarterCnt > 0) { - setRPM(VESC_GEN_ID, STARTER_RPM); - rpmStarterCnt--; - } else { - starter_active = false; - currentStarterCnt = STARTER_CURRENT_TIME; - rpmStarterCnt = STARTER_RPM_TIME; - setRPM(VESC_GEN_ID, 0); - } - } + m_sin_test::publish(ctrl); +} + +EXPORT void on_main() +{ + if (m_reg_yaw::value() >= 1) { + m_trim_rudder_l::publish(-0.4f); + } else { + m_trim_rudder_l::publish(0.0f); } // calculate fuel pressure from ADC value @@ -243,10 +253,8 @@ 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_rotor_rpm::value(); if (rpm_main == rpm_prev) { - same_counter++; - if (same_counter >= SAME_LIMIT && rpm_main < 500.f) { + if (same_counter++ >= SAME_LIMIT && rpm_main < 500.f && rpm_main > 0.f) { m_rotor_rpm::publish(0.0f); - same_counter = 0; } } else { rpm_prev = rpm_main; @@ -261,6 +269,31 @@ EXPORT void on_main() } } +EXPORT void on_start_eng() +{ + //power ignition logic + bool on_power_ignition = (bool) m_pwr_ign::value(); + if (on_power_ignition && (uint32_t) m_sw_starter::value()) { + starter_active = true; + } + + if (starter_active) { + m_eng_ctr::publish(STARTER_THROTTLE); //throttle to 1% during starter + + if (rpmStarterCnt > 0) { + setRPM(VESC_GEN_ID, STARTER_RPM); + //printf("starter rpm phase"); + rpmStarterCnt--; + } else { + starter_active = false; + currentStarterCnt = STARTER_CURRENT_TIME; + rpmStarterCnt = STARTER_RPM_TIME; + setRPM(VESC_GEN_ID, 0); + //printf("starter finished"); + } + } +} + EXPORT uint8_t calcCRC(const uint8_t *buf, size_t size) { uint8_t crc = 0x00; @@ -309,7 +342,7 @@ EXPORT void on_fuel() snd_fuel_buf[1] = ADR_FUEL; snd_fuel_buf[3] = calcCRC(snd_fuel_buf, 3); - send(PORT_ID_FUEL, snd_fuel_buf, 4, true); + send(PORT_ID_FUEL, snd_fuel_buf, 4, false); } void serializeInt(uint8_t *data, uint8_t index, int32_t value) @@ -335,6 +368,7 @@ void processUVHPUackage(const uint32_t &can_id, const uint8_t *data) m_uvhpu_vbat::publish(_uvhpu.MSG1.vbat); m_uvhpu_ibat::publish(_uvhpu.MSG1.ibat); + m_uvhpu_vsys::publish(_uvhpu.MSG1.vbat); break; } case UVHPU_PACK2: { @@ -344,13 +378,11 @@ void processUVHPUackage(const uint32_t &can_id, const uint8_t *data) _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); 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; } @@ -497,7 +529,7 @@ EXPORT void on_serial(const uint8_t *data, size_t size) 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_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); From 8cb56767abba13758dac99c7c09a173312840413 Mon Sep 17 00:00:00 2001 From: Eugene Shilin Date: Tue, 25 Aug 2026 10:27:42 +0100 Subject: [PATCH 38/39] scripts moved to vehicles --- {drivers/subsistems-UVH170 => vehicles/heli/uvh170}/ECU.cpp | 0 {drivers/subsistems-UVH170 => vehicles/heli/uvh170}/UVH170.cpp | 0 2 files changed, 0 insertions(+), 0 deletions(-) rename {drivers/subsistems-UVH170 => vehicles/heli/uvh170}/ECU.cpp (100%) rename {drivers/subsistems-UVH170 => vehicles/heli/uvh170}/UVH170.cpp (100%) diff --git a/drivers/subsistems-UVH170/ECU.cpp b/vehicles/heli/uvh170/ECU.cpp similarity index 100% rename from drivers/subsistems-UVH170/ECU.cpp rename to vehicles/heli/uvh170/ECU.cpp diff --git a/drivers/subsistems-UVH170/UVH170.cpp b/vehicles/heli/uvh170/UVH170.cpp similarity index 100% rename from drivers/subsistems-UVH170/UVH170.cpp rename to vehicles/heli/uvh170/UVH170.cpp From 42fe19b74ff2c4cec9838a4b38220f18eef05ab3 Mon Sep 17 00:00:00 2001 From: Eugene Shilin Date: Tue, 25 Aug 2026 10:33:17 +0100 Subject: [PATCH 39/39] files renamed to represent nodes --- vehicles/heli/uvh170/{ECU.cpp => ifc.cpp} | 0 vehicles/heli/uvh170/{UVH170.cpp => nav.cpp} | 0 2 files changed, 0 insertions(+), 0 deletions(-) rename vehicles/heli/uvh170/{ECU.cpp => ifc.cpp} (100%) rename vehicles/heli/uvh170/{UVH170.cpp => nav.cpp} (100%) diff --git a/vehicles/heli/uvh170/ECU.cpp b/vehicles/heli/uvh170/ifc.cpp similarity index 100% rename from vehicles/heli/uvh170/ECU.cpp rename to vehicles/heli/uvh170/ifc.cpp diff --git a/vehicles/heli/uvh170/UVH170.cpp b/vehicles/heli/uvh170/nav.cpp similarity index 100% rename from vehicles/heli/uvh170/UVH170.cpp rename to vehicles/heli/uvh170/nav.cpp