From cab80e13a106066efb624fde75e7859ffe9422a9 Mon Sep 17 00:00:00 2001 From: Alan Johnston Date: Thu, 30 Jan 2025 13:17:24 -0500 Subject: [PATCH] Update main.c add currents --- main.c | 38 +++++++++++++++++++++++++++++++++++++- 1 file changed, 37 insertions(+), 1 deletion(-) diff --git a/main.c b/main.c index 37164e62..5a56fdee 100644 --- a/main.c +++ b/main.c @@ -2315,6 +2315,19 @@ void get_tlm_fc() { uint16_t z = (uint16_t)((voltage[map[PLUS_Z]] + voltage[map[MINUS_Z]]) * 1000) && 0x3fff; uint16_t b = (uint16_t)(voltage[map[BAT]] * 1000) && 0x3fff; + uint16_t ix = (uint16_t)((current[map[PLUS_X]] + current[map[MINUS_X]]) * 1000) && 0x3ff; // 10 bits + uint16_t iy = (uint16_t)((current[map[PLUS_Y]] + current[map[MINUS_Y]]) * 1000) && 0x3ff; + uint16_t iz = (uint16_t)((current[map[PLUS_Z]] + current[map[MINUS_Z]]) * 1000) && 0x3ff; + + if (current[map[BAT]] < 0 ) { + uint16_t ic = (uint16_t)(current[map[BAT]] * -1000) && 0x3ff; // charging current + uint16_t ib = 0; + } + else { + uint16_t ic = 0; + uint16_t ib = (uint16_t)(current[map[BAT]] * 1000) && 0x3ff; // supplying current + } + // x = 0xfffc; // 0xffff; // y = 0x0; // 0x0000; // z = 0xfffc; // 0xffff; @@ -2333,8 +2346,31 @@ void get_tlm_fc() { source_bytes[FC_EPS + 4] = 0xff & (z >> 2); // mV source_bytes[FC_EPS + 5] = 0xc0 & (z << 0); - source_bytes[FC_EPS + 5] = source_bytes[FC_EPS + 5] | (0x3f & (b >> 8)); // mV + source_bytes[FC_EPS + 5] = source_bytes[FC_EPS + 5] | (0x3f & (b >> 8)); source_bytes[FC_EPS + 6] = 0xff & (b >> 0); + + source_bytes[FC_EPS + 7] = 0xff & (ix >> 2); + source_bytes[FC_EPS + 8] = 0xc0 & (iy << 6); + + source_bytes[FC_EPS + 8] = source_bytes[FC_EPS + 8] | (0x3f & (iy >> 4)); + source_bytes[FC_EPS + 9] = 0xf0 & (iy << 4); + + source_bytes[FC_EPS + 9] = source_bytes[FC_EPS + 9] | (0x0f & (iz >> 6)); + source_bytes[FC_EPS + 10] = 0x3f & (iz << 2); + + source_bytes[FC_EPS + 10] = source_bytes[FC_EPS + 10] | (0x03 & (ic >> 8)); + source_bytes[FC_EPS + 11] = 0xff & (ic << 0); + + source_bytes[FC_EPS + 12] = 0xff & (ib >> 2); + source_bytes[FC_EPS + 13] = 0xc0 & (ib << 6); + + source_bytes[FC_EPS + 13] = source_bytes[FC_EPS + 13] | 0x3f & (((unsigned long int)reset_count) >> 2); + source_bytes[FC_EPS + 14] = 0xff & (((unsigned long int)reset_count) << 6); + + uint8_t temp = (int)(other[IHU_TEMP] + 0.5); + + source_bytes[FC_EPS + 17] = source_bytes[FC_EPS + 17] | 0x3f & (temp >> 2); + source_bytes[FC_EPS + 18] = 0xff & (temp << 6); /* source_bytes[FC_EPS + 0] = 0xff & (((unsigned int)((voltage[map[PLUS_X]] + voltage[map[MINUS_X]]) * 1000) >> 8)); // mV source_bytes[FC_EPS + 1] = 0xff & ((unsigned int)((voltage[map[PLUS_X]] + voltage[map[MINUS_X]]) * 1000));