diff --git a/src/monocoque/devices/serial/moza.c b/src/monocoque/devices/serial/moza.c index 0108148..f98f5d3 100644 --- a/src/monocoque/devices/serial/moza.c +++ b/src/monocoque/devices/serial/moza.c @@ -13,67 +13,64 @@ #define MOZA_SERIAL_TEMPLATE {0x7e, 0x06, 0x41, 0x13, 0xfd, 0xde, 0x0, 0x0, 0x0, 0x0, 0x0} #define MOZA_NUM_AVAILABLE_LEDS 10 #define MOZA_BLINKING_BIT 7 +#define MOZA_PAYLOAD_SIZE 11 #define MOZA_MAGIC_VALUE 0x0d #define BIT(nr) (1UL << (nr)) unsigned char moza_checksum(unsigned char *data) { unsigned char ret = MOZA_MAGIC_VALUE; - for (short i = 0; i < sizeof(data)/sizeof(data[0]); i++) + for (short i = 0; i < MOZA_PAYLOAD_SIZE; i++) { ret += data[i]; } - return ret; + return ret % 0x100; } int moza_update(SerialDevice* serialdevice, unsigned short maxrpm, unsigned short rpm) { unsigned char bytes[] = MOZA_SERIAL_TEMPLATE; - int size = sizeof(bytes)/sizeof(bytes[0]); - + int size = MOZA_PAYLOAD_SIZE; float perct = (float)rpm/(float)maxrpm; - if (perct >= 0.8) + if (perct >= .1) bytes[9] |= BIT(0); - if (perct >= 0.83) + if (perct >= .2) bytes[9] |= BIT(1); - if (perct >= 0.86) + if (perct >= .3) bytes[9] |= BIT(2); - if (perct >= 0.89) + if (perct >= .4) bytes[9] |= BIT(3); - if (perct >= 0.91) - bytes[8] |= BIT(4); + if (perct >= .5) + bytes[9] |= BIT(4); - if (perct >= 0.92) - bytes[8] |= BIT(5); + if (perct >= .6) + bytes[9] |= BIT(4); - if (perct >= 0.93) + if (perct >= .7) + bytes[9] |= BIT(5); + + if (perct >= .8) bytes[8] |= BIT(6); - if (perct >= 0.94) + if (perct >= .9) bytes[8] |= BIT(7); - if (perct >= 0.96) - bytes[8] |= BIT(0); - - if (perct >= 0.96) - bytes[8] |= BIT(1); - // blinking - if (perct >= 0.97) - bytes[9] |= BIT(MOZA_BLINKING_BIT); + if (perct >= .95) + bytes[8] |= BIT(MOZA_BLINKING_BIT); bytes[10] = moza_checksum(bytes); int result = 1; if (serialdevice->port) { - slogd("copying %i bytes to moza device", size); + slogd("copying %i bytes to moza device", MOZA_PAYLOAD_SIZE); slogt("writing bytes %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x from rpm %i maxrpm %i", bytes[0], bytes[1], bytes[2], bytes[3], bytes[4], bytes[5], bytes[6], bytes[7], bytes[8], bytes[9], bytes[10], rpm, maxrpm); result = monocoque_serial_write(serialdevice->id, bytes, size, MOZA_TIMEOUT); }