Example #1
0
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();
    }
}
Example #2
0
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();
    }
}
Example #3
0
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;
}