void telemetrySendNotice(const char *s) {
commTxBuf_t *txBuf;
uint8_t ckA, ckB;
uint8_t *ptr;
txBuf = commGetTxBuf(COMM_STREAM_TYPE_TELEMETRY, 64);
if (txBuf > 0) {
ptr = &txBuf->buf;
supervisorSendDataStart();
*ptr++ = 'A';
*ptr++ = 'q';
*ptr++ = 'I';
ckA = ckB = 0;
do {
*ptr++ = *s;
ckA += *s;
ckB += ckA;
} while (*(s++));
*ptr++ = ckA;
*ptr++ = ckB;
commSendTxBuf(txBuf, ptr - &txBuf->buf);
supervisorSendDataStop();
}
}
static void commandResponseSend(commandBufStruct_t *r, uint8_t len) {
commTxBuf_t *txBuf;
uint8_t *ptr;
char *c = (char *)r;
txBuf = commGetTxBuf(COMM_STREAM_TYPE_TELEMETRY, len + 3 + 5 + 2);
if (txBuf != 0) {
ptr = &txBuf->buf;
supervisorSendDataStart();
*ptr++ = 'A';
*ptr++ = 'q';
*ptr++ = 'R';
commandData.checkA = commandData.checkB = 0;
ptr = commandSendChar(ptr, commandData.requestId[0]);
ptr = commandSendChar(ptr, commandData.requestId[1]);
ptr = commandSendChar(ptr, commandData.requestId[2]);
ptr = commandSendChar(ptr, commandData.requestId[3]);
ptr = commandSendChar(ptr, len);
do {
ptr = commandSendChar(ptr, *c++);
} while (--len);
*ptr++ = commandData.checkA;
*ptr++ = commandData.checkB;
commSendTxBuf(txBuf, ptr - &txBuf->buf);
supervisorSendDataStop();
}
}
void telemetryDo(void) {
static unsigned long lastAqUpdate;
static unsigned long aqCounter;
commTxBuf_t *txBuf;
uint8_t *ptr;
telemetryData.loops++;
if (!(telemetryData.loops % (unsigned int)p[TELEMETRY_RATE])) {
aqCounter = counter;
// calculate idle time
telemetryData.idlePercent = (aqCounter - telemetryData.lastAqCounter) * (1e6 / (IMU_LASTUPD - lastAqUpdate)) / p[TELEMETRY_RATE] /
(rccClocks.SYSCLK_Frequency / minCycles) * 100.0f;
telemetryData.lastAqCounter = aqCounter;
if (telemetryData.telemetryEnable) {
txBuf = commGetTxBuf(COMM_STREAM_TYPE_TELEMETRY, 256);
// fail as we cannot block
if (txBuf != 0) {
supervisorSendDataStart();
ptr = &txBuf->buf;
*ptr++ = 'A';
*ptr++ = 'q';
*ptr++ = 'T';
telemetryData.ckA = telemetryData.ckB = 0;
ptr = telemtrySendFloat(ptr, AQ_ROLL);
ptr = telemtrySendFloat(ptr, AQ_PITCH);
ptr = telemtrySendFloat(ptr, AQ_YAW);
ptr = telemtrySendInt(ptr, RADIO_THROT);
ptr = telemtrySendInt(ptr, RADIO_RUDD);
ptr = telemtrySendInt(ptr, RADIO_PITCH);
ptr = telemtrySendInt(ptr, RADIO_ROLL);
ptr = telemtrySendInt(ptr, RADIO_FLAPS);
ptr = telemtrySendInt(ptr, RADIO_AUX4);
ptr = telemtrySendFloat(ptr, IMU_RATEX);
ptr = telemtrySendFloat(ptr, IMU_RATEY);
ptr = telemtrySendFloat(ptr, IMU_RATEZ);
ptr = telemtrySendFloat(ptr, IMU_ACCX);
ptr = telemtrySendFloat(ptr, IMU_ACCY);
ptr = telemtrySendFloat(ptr, IMU_ACCZ);
ptr = telemtrySendFloat(ptr, navData.holdHeading);
ptr = telemtrySendFloat(ptr, AQ_PRESSURE);
ptr = telemtrySendFloat(ptr, IMU_TEMP);
ptr = telemtrySendFloat(ptr, ALTITUDE);
ptr = telemtrySendFloat(ptr, analogData.vIn);
ptr = telemtrySendInt(ptr, IMU_LASTUPD - gpsData.lastPosUpdate); // us
ptr = telemtrySendFloat(ptr, UKF_POSN);
ptr = telemtrySendFloat(ptr, UKF_POSE);
ptr = telemtrySendFloat(ptr, ALT_POS);
ptr = telemtrySendFloat(ptr, gpsData.lat);
ptr = telemtrySendFloat(ptr, gpsData.lon);
ptr = telemtrySendFloat(ptr, gpsData.hAcc);
ptr = telemtrySendFloat(ptr, gpsData.heading);
ptr = telemtrySendFloat(ptr, gpsData.height);
ptr = telemtrySendFloat(ptr, gpsData.pDOP);
ptr = telemtrySendFloat(ptr, navData.holdCourse);
ptr = telemtrySendFloat(ptr, navData.holdDistance);
ptr = telemtrySendFloat(ptr, navData.holdAlt);
ptr = telemtrySendFloat(ptr, navData.holdTiltN);
ptr = telemtrySendFloat(ptr, navData.holdTiltE);
ptr = telemtrySendFloat(ptr, UKF_VELN);
ptr = telemtrySendFloat(ptr, UKF_VELE);
ptr = telemtrySendFloat(ptr, -VELOCITYD);
ptr = telemtrySendFloat(ptr, IMU_MAGX);
ptr = telemtrySendFloat(ptr, IMU_MAGY);
ptr = telemtrySendFloat(ptr, IMU_MAGZ);
ptr = telemtrySendInt(ptr, 1e6 / (IMU_LASTUPD - lastAqUpdate));
ptr = telemtrySendFloat(ptr, RADIO_QUALITY);
ptr = telemtrySendFloat(ptr, motorsData.value[0]);
ptr = telemtrySendFloat(ptr, motorsData.value[1]);
ptr = telemtrySendFloat(ptr, motorsData.value[2]);
ptr = telemtrySendFloat(ptr, motorsData.value[3]);
ptr = telemtrySendFloat(ptr, telemetryData.idlePercent);
ptr = telemtrySendFloat(ptr, UKF_ACC_BIAS_X);
ptr = telemtrySendFloat(ptr, UKF_ACC_BIAS_Y);
ptr = telemtrySendFloat(ptr, UKF_ACC_BIAS_Z);
ptr = telemtrySendFloat(ptr, supervisorData.flightTimeRemaining);
*ptr++ = telemetryData.ckA;
*ptr++ = telemetryData.ckB;
commSendTxBuf(txBuf, ptr - &txBuf->buf);
supervisorSendDataStop();
}
}
}
lastAqUpdate = IMU_LASTUPD;
}
