diff --git a/readref.c b/readref.c index bb5919c..808d73e 100644 --- a/readref.c +++ b/readref.c @@ -139,21 +139,25 @@ int main() } printf("\n"); - // XXX: loop throught multiple drefs - if ((payloadBytes - 1) % 8 == 0) + // continue if payloadBytes is valid (1 byte + 8 bytes each dref) + if ((payloadBytes - 1) % sizeof(struct dref_struct_out) == 0) { - memset(mqttTopic, 0, sizeof(mqttTopic)); - memset(mqttPayload, 0, sizeof(mqttPayload)); + // loop throught all provided drefs + for (int i = 1; i < payloadBytes; i += sizeof(struct dref_struct_out)) + { + memset(mqttTopic, 0, sizeof(mqttTopic)); + memset(mqttPayload, 0, sizeof(mqttPayload)); - struct dref_struct_out drefRead; - memcpy(&drefRead, &payload[1], 8); + struct dref_struct_out drefRead; + memcpy(&drefRead, &payload[i], sizeof(struct dref_struct_out)); - printf("%d: %f\n", drefRead.dref_sender_index, drefRead.dref_flt_value); + printf("%d: %f\n", drefRead.dref_sender_index, drefRead.dref_flt_value); - sprintf(mqttTopic, "/xplane/rref/%d", drefRead.dref_sender_index); - sprintf(mqttPayload, "%f", drefRead.dref_flt_value); + sprintf(mqttTopic, "/xplane/rref/%d", drefRead.dref_sender_index); + sprintf(mqttPayload, "%f", drefRead.dref_flt_value); - sendMqtt(mqtt, mqttTopic, mqttPayload, strlen(mqttPayload)); + sendMqtt(mqtt, mqttTopic, mqttPayload, strlen(mqttPayload)); + } } else {