Exemplo n.º 1
0
// read config from uSD
int8_t configReadFile(char *fname) {
    char *fileBuf;
    int8_t fh;
    int ret;
    int p1;

    if (fname == 0)
	fname = CONFIG_FILE_NAME;

    if ((fh = filerGetHandle(fname)) < 0) {
	AQ_NOTICE("config: cannot get read file handle\n");
	return -1;
    }

    fileBuf = (char *)aqCalloc(CONFIG_FILE_BUF_SIZE, sizeof(char));

    p1 = 0;
    while ((ret = filerRead(fh, fileBuf, -1, CONFIG_FILE_BUF_SIZE)) > 0)
	p1 = configParseParams((char *)fileBuf, ret, p1);

    filerClose(fh);

    if (fileBuf)
	aqFree(fileBuf, CONFIG_FILE_BUF_SIZE, sizeof(char));

    return ret;
}
Exemplo n.º 2
0
// write config to uSD
int8_t configWriteFile(char *fname) {
    char buf[128];
    int8_t fh;
    int8_t ret;
    int n;
    int i;

    if (fname == 0)
	fname = CONFIG_FILE_NAME;

    if ((fh = filerGetHandle(fname)) < 0) {
	AQ_NOTICE("config: cannot get write file handle\n");
	return -1;
    }

    for (i = 0; i < CONFIG_NUM_PARAMS; i++) {
	n = configFormatParam(buf, i);
	ret = filerWrite(fh, buf, -1, n);

	if (ret < n) {
	    AQ_NOTICE("config: file write error\n");
	    ret = -1;
	    break;
	}
    }

    filerClose(fh);

    return ret;
}
Exemplo n.º 3
0
void loggerInit(void) {
    memset((void *)&loggerData, 0, sizeof(loggerData));

    loggerSetup();

    // skip the first 512 bytes (used exclusively by the USB MSC driver)
    loggerData.loggerBuf = (TCHAR *)(filerBuf + 512);
    loggerData.bufSize = ((FILER_BUF_SIZE-512) / loggerData.packetSize / FILER_FLUSH_THRESHOLD) * loggerData.packetSize * FILER_FLUSH_THRESHOLD;

    loggerData.logHandle = filerGetHandle(LOGGER_FNAME);
    filerStream(loggerData.logHandle, loggerData.loggerBuf, loggerData.bufSize);

    loggerDoHeader();
}
Exemplo n.º 4
0
// write config to uSD
int8_t configWriteFile(char *fname) {
    char *buf;
    int8_t fh;
    int8_t ret;
    int n;
    int i;

    if (fname == 0)
	fname = CONFIG_FILE_NAME;

    if ((fh = filerGetHandle(fname)) < 0) {
	AQ_NOTICE("config: cannot get write file handle\n");
	return -1;
    }

    if (!(buf = (char *)aqCalloc(128, sizeof(char)))) {
	AQ_NOTICE("config: Error writing to file, cannot allocate memory.\n");
	filerClose(fh);
	return -1;
    }

    for (i = 0; i < CONFIG_NUM_PARAMS; i++) {
	n = configFormatParam(buf, i);
	if (n)
	    ret = filerWrite(fh, buf, -1, n);

	if (!n || ret < n) {
	    ret = -1;
	    break;
	}
    }

    filerClose(fh);

    if (buf)
	aqFree(buf, 128, sizeof(char));

    if (ret > -1)
	AQ_NOTICE("config: Parameters saved to local storage file.\n");
    else
	AQ_NOTICE("config: Error writing parameters to file.\n");

    return ret;
}
Exemplo n.º 5
0
// read config from uSD
int8_t configReadFile(char *fname) {
    char *fileBuf;
    int8_t fh;
    int ret;
    int p1;

    if (fname == 0)
	fname = CONFIG_FILE_NAME;

    if ((fh = filerGetHandle(fname)) < 0) {
	AQ_NOTICE("config: cannot get read file handle\n");
	return -1;
    }

    if (!(fileBuf = (char *)aqCalloc(CONFIG_FILE_BUF_SIZE, sizeof(char)))) {
	AQ_NOTICE("config: Error reading from file, cannot allocate memory.\n");
	filerClose(fh);
	return -1;
    }

    p1 = 0;
    while ((ret = filerRead(fh, fileBuf, -1, CONFIG_FILE_BUF_SIZE)) > 0) {
	p1 = configParseParams((char *)fileBuf, ret, p1);
	if (p1 < 0) {
	    ret = -1;
	    break;
	}
    }

    filerClose(fh);

    if (fileBuf)
	aqFree(fileBuf, CONFIG_FILE_BUF_SIZE, sizeof(char));

    if (ret > -1)
	AQ_NOTICE("config: Parameters loaded from local storage file.\n");
    else
	AQ_NOTICE("config: Failed to read parameters from local file.");

    return ret;
}
Exemplo n.º 6
0
void motorsSetupLogging(void) {
    // setup logging
    motorsData.logHandle = filerGetHandle("ESC");
    filerStream(motorsData.logHandle, motorsLogBuf, sizeof(motorsLogBuf));
}
Exemplo n.º 7
0
void navUkfInit(void) {
    float Q[SIM_S];		// state variance
    float V[SIM_V];		// process variance
    float mag[3];

    memset((void *)&navUkfData, 0, sizeof(navUkfData));

    navUkfData.v0a[0] = 0.0f;
    navUkfData.v0a[1] = 0.0f;
    navUkfData.v0a[2] = -1.0f;

    // calculate mag vector based on inclination
    mag[0] = cosf(p[IMU_MAG_INCL] * DEG_TO_RAD);
    mag[1] = 0.0f;
    mag[2] = -sinf(p[IMU_MAG_INCL] * DEG_TO_RAD);

    // rotate local mag vector to align with true north
    navUkfData.v0m[0] = mag[0] * cosf(p[IMU_MAG_DECL] * DEG_TO_RAD) - mag[1] * sinf(p[IMU_MAG_DECL]  * DEG_TO_RAD);
    navUkfData.v0m[1] = mag[1] * cosf(p[IMU_MAG_DECL] * DEG_TO_RAD) + mag[0] * sinf(p[IMU_MAG_DECL]  * DEG_TO_RAD);
    navUkfData.v0m[2] = mag[2];

    navUkfData.kf = srcdkfInit(SIM_S, SIM_M, SIM_V, SIM_N, navUkfTimeUpdate);

    navUkfData.x = srcdkfGetState(navUkfData.kf);

    Q[0] = UKF_VEL_Q;
    Q[1] = UKF_VEL_Q;
    Q[2] = UKF_VEL_ALT_Q;
    Q[3] = UKF_POS_Q;
    Q[4] = UKF_POS_Q;
    Q[5] = UKF_POS_ALT_Q;
    Q[6] = UKF_ACC_BIAS_Q;
    Q[7] = UKF_ACC_BIAS_Q;
    Q[8] = UKF_ACC_BIAS_Q;
    Q[9] = UKF_GYO_BIAS_Q;
    Q[10] = UKF_GYO_BIAS_Q;
    Q[11] = UKF_GYO_BIAS_Q;
    Q[12] = UKF_QUAT_Q;
    Q[13] = UKF_QUAT_Q;
    Q[14] = UKF_QUAT_Q;
    Q[15] = UKF_QUAT_Q;
    Q[16] = UKF_PRES_ALT_Q;

    V[UKF_V_NOISE_ACC_BIAS_X] = UKF_ACC_BIAS_V;
    V[UKF_V_NOISE_ACC_BIAS_Y] = UKF_ACC_BIAS_V;
    V[UKF_V_NOISE_ACC_BIAS_Z] = UKF_ACC_BIAS_V;
    V[UKF_V_NOISE_GYO_BIAS_X] = UKF_GYO_BIAS_V;
    V[UKF_V_NOISE_GYO_BIAS_Y] = UKF_GYO_BIAS_V;
    V[UKF_V_NOISE_GYO_BIAS_Z] = UKF_GYO_BIAS_V;
    V[UKF_V_NOISE_RATE_X] = UKF_RATE_V;
    V[UKF_V_NOISE_RATE_Y] = UKF_RATE_V;
    V[UKF_V_NOISE_RATE_Z] = UKF_RATE_V;
    V[UKF_V_NOISE_VELN] = UKF_VEL_V;
    V[UKF_V_NOISE_VELE] = UKF_VEL_V;
    V[UKF_V_NOISE_VELD] = UKF_ALT_VEL_V;

    srcdkfSetVariance(navUkfData.kf, Q, V, 0, 0);

    navUkfInitState();

    navUkfData.flowRotCos = cosf(UKF_FLOW_ROT * DEG_TO_RAD);
    navUkfData.flowRotSin = sinf(UKF_FLOW_ROT * DEG_TO_RAD);

#ifdef UKF_LOG_FNAME
    navUkfData.logHandle = filerGetHandle(UKF_LOG_FNAME);
    filerStream(navUkfData.logHandle, ukfLog, UKF_LOG_BUF_SIZE);
#endif
}
Exemplo n.º 8
0
// read esc32 params from uSD
static int8_t esc32ReadFile(char *fname, canNodes_t *canNode) {
    char *fileBuf;
    char *lineBuf;
    char param[24];
    int paramId;
    float value, retValue;
    int8_t needsConfigWrite = 0;
    int8_t fh;
    char c;
    int ret;
    int i, p1, p2;
    int n;

    if (fname == 0)
	fname = ESC32_FILE_NAME;

    if ((fh = filerGetHandle(fname)) < 0) {
	AQ_NOTICE("ESC32: cannot get read file handle\n");
	return -1;
    }

    fileBuf = (char *)aqCalloc(ESC32_FILE_BUF_SIZE, sizeof(char));
    lineBuf = (char *)aqCalloc(ESC32_LINE_BUF_SIZE, sizeof(char));

    needsConfigWrite = 0;

    if (fileBuf && lineBuf) {
	p1 = 0;
	do {
	    ret = filerRead(fh, fileBuf, -1, ESC32_FILE_BUF_SIZE);

	    p2 = 0;
	    for (i = 0; i < ret; i++) {
		c = fileBuf[p2++];
		if (c == '\n' || p1 == (ESC32_LINE_BUF_SIZE-1)) {
		    lineBuf[p1] = 0;

		    n = sscanf(lineBuf, "#define DEFAULT_%23s %f", param, &value);
		    if (n != 2) {
			n = sscanf(lineBuf, "#define %23s %f", param, &value);
			if (n != 2)
			    n = sscanf(lineBuf, "%23s %f", param, &value);
		    }

		    if (n == 2) {
			// lookup parameter id
                        if (canNode)
                            paramId = canGetParamIdByName(canNode->networkId, (uint8_t *)param);
                        else
                            paramId = esc32OwGetParamId(param);

			// valid id?
			if (paramId >= 0) {
			    // read
                            if (canNode)
                                retValue = canGetParamById(canNode->networkId, paramId);
                            else
                                retValue = esc32OwReadParamById(paramId);

			    // current value differs from ours?
			    if (retValue != value) {
				// write
                                if (canNode)
                                    canSetParamById(canNode->networkId, paramId, value);
                                else
                                    esc32OwWriteParam(paramId, value);

				// read back
                                if (canNode)
                                    retValue = canGetParamById(canNode->networkId, paramId);
                                else
                                    retValue = esc32OwReadParamById(paramId);

				// successful write?
				if (retValue == value) {
				    needsConfigWrite++;
				}
				else {
				    AQ_NOTICE("ESC32: parameter write failed!\n");
				}
			    }
			}
		    }
		    p1 = 0;
		}
		else {
		    lineBuf[p1++] = c;
		}
	    }
	} while (ret > 0);

	filerClose(fh);
    }

    if (lineBuf)
	aqFree(lineBuf, ESC32_LINE_BUF_SIZE, sizeof(char));
    if (fileBuf)
	aqFree(fileBuf, ESC32_FILE_BUF_SIZE, sizeof(char));

    return needsConfigWrite;
}