void configInit(void) {
float ver;
#if 1
configLoadDefault();
#else
// start with what's in flash
configFlashRead();
// try to load any config params from uSD card
if (configReadFile(0) < 0)
// clear config if read error
memset(p, 0, sizeof(p));
else
supervisorConfigRead();
// get flash version
ver = *(float *)flashStartAddr();
if (isnan(ver))
ver = 0.0f;
// if compiled defaults are greater than flash version and loaded version
if (DEFAULT_CONFIG_VERSION > ver && DEFAULT_CONFIG_VERSION > p[CONFIG_VERSION])
configLoadDefault();
// if flash version greater than or equal to currently loaded version
else if (ver >= p[CONFIG_VERSION])
configFlashRead();
// if loaded version greater than flash version
if (p[CONFIG_VERSION] > ver)
configFlashWrite();
#endif
}
static void commandExecute(commandBufStruct_t *b) {
switch (b->commandId) {
case COMMAND_GPS_PACKET:
// first character is length
gpsSendPacket(*b->data, b->data+1);
commandAck(NULL, 0);
break;
// case COMMAND_ID_GPS_PASSTHROUGH:
// if (!(supervisorData.state & STATE_FLYING)) {
// telemetryDisable();
// commandAck(NULL, 0);
// gpsPassthrough(gpsData.gpsPort, downlinkData.serialPort);
// }
// else {
// commandNack(NULL, 0);
// }
// break;
case COMMAND_CONFIG_PARAM_READ:
{
unsigned int replyLength = configParameterRead(b->data);
if (replyLength)
commandAck(b->data, replyLength+8);
else
commandNack(NULL, 0);
}
break;
case COMMAND_CONFIG_PARAM_WRITE:
if (!(supervisorData.state & STATE_FLYING)) {
unsigned int replyLength = configParameterWrite(b->data);
if (replyLength)
commandAck(b->data, replyLength);
else
commandNack(NULL, 0);
}
else {
commandNack(NULL, 0);
}
break;
case COMMAND_CONFIG_FLASH_READ:
if (!(supervisorData.state & STATE_FLYING)) {
configFlashRead();
commandAck(NULL, 0);
}
else {
commandNack(NULL, 0);
}
break;
case COMMAND_CONFIG_FLASH_WRITE:
if (!(supervisorData.state & STATE_FLYING)) {
if (configFlashWrite())
commandAck(NULL, 0);
else
commandNack(NULL, 0);
}
else {
commandNack(NULL, 0);
}
break;
case COMMAND_CONFIG_FACTORY_RESET:
if (!(supervisorData.state & STATE_FLYING)) {
configLoadDefault();
commandAck(NULL, 0);
}
else {
commandNack(NULL, 0);
}
break;
case COMMAND_TELEMETRY_DISABLE:
telemetryDisable();
commandAck(NULL, 0);
break;
case COMMAND_TELEMETRY_ENABLE:
telemetryEnable();
commandAck(NULL, 0);
break;
case COMMAND_ACC_IN:
commandAccIn(b);
break;
default:
commandNack(NULL, 0);
break;
}
}
