unsigned int configParameterWrite(void *data) {
paramStruct_t *par = (paramStruct_t *)data;
memcpy((char *)&p[par->paramId], (char *)par->values, par->num * sizeof(float));
return configParameterRead(data);
}
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;
}
}
