void
FaceManager::listQueriedFaces(const Interest& request)
{
NFD_LOG_DEBUG("in listQueriedFaces");
const Name& query = request.getName();
const size_t queryNComps = query.size();
if (queryNComps < FACES_QUERY_DATASET_NCOMPS ||
!FACES_QUERY_DATASET_PREFIX.isPrefixOf(query))
{
NFD_LOG_DEBUG("query result: malformed");
sendNack(query);
return;
}
ndn::nfd::FaceQueryFilter faceFilter;
try
{
faceFilter.wireDecode(query[-1].blockFromValue());
}
catch (tlv::Error&)
{
NFD_LOG_DEBUG("query result: malformed filter");
sendNack(query);
return;
}
FaceQueryStatusPublisher
faceQueryStatusPublisher(m_faceTable, *m_face, query, faceFilter, m_keyChain);
faceQueryStatusPublisher.publish();
}
// Allows the reading or changing of direction for specified motor, but only allows change in
// direction if the change results in a percentage change of less than the outputRateLimit
void onCommandDirection()
{
if (isChannelCorrect(gParameters[0]))
{
uint8_t channel = convertToInt(gParameters[0]);
if (isReadCommand(gParameters[1]))
{
sendDirectionStatus(directions[channel]);
sendAck();
}
else if (!isPidEnabled)
{
int output = currentOutputs[channel];
int outputRateLimit = outputRateLimits[channel];
Direction direction;
if (isClockwiseCommandArg(gParameters[1]))
{
direction = Direction::Clockwise;
}
else if (isCounterClockwiseCommandArg(gParameters[1]))
{
direction = Direction::CounterClockwise;
}
else
{
sendDirectionError();
return;
}
if (direction != directions[channel] && output > outputRateLimit / 2)
{
Serial << F("Cannot make a change to output by more than ") << outputRateLimit << F("%") << NEWLINE;
sendNack();
}
else
{
applyMotorOutputs(channel, direction, output);
sendAck();
}
}
else
{
Serial.println(F("Cannot change direction while PID control is on."));
sendNack();
}
}
else
{
sendChannelError();
}
}
void onCommandMotorDriver()
{
if (isChannelCorrect(gParameters[0]))
{
uint8_t channel = convertToInt(gParameters[0]);
if (isReadCommand(gParameters[1]))
{
sendMotorDriverStatus(motorDriverTypes[channel]);
sendAck();
}
else
{
if (!isPidEnabled)
{
if (isAnalogVoltageCommandArg(gParameters[1]))
{
motorDriverTypes[channel] = AnalogVoltage;
sendAck();
}
else if (isFrequencyCommandArg(gParameters[1]))
{
if (channel == TILT_CHANNEL)
{
motorDriverTypes[channel] = Frequency;
sendAck();
}
else
{
Serial.println(F("Yaw (Channel 1) can only be set to Analog Voltage"));
sendNack();
}
}
else
{
sendMotorDriverError();
}
}
else
{
Serial.println(F("Cannot change driver type while PID control is on."));
sendNack();
}
}
}
else
{
sendChannelError();
}
}
void onCommandLoopInterval()
{
int loopInterval = convertToInt(gParameters[0]);
if (isReadCommand(gParameters[0]))
{
sendInt(pidLoopInterval);
sendAck();
}
else if (isIntWithinRange(loopInterval, PID_INTERVAL_MS_MIN, PID_INTERVAL_MS_MAX))
{
if (!isPidEnabled)
{
pidLoopInterval = loopInterval;
sendAck();
}
else
{
Serial.println(F("Cannot change loop interval while PID control is on."));
sendNack();
}
}
else
{
sendIntRangeError(PID_INTERVAL_MS_MIN, PID_INTERVAL_MS_MAX, MILLISECONDS_UNIT);
}
}
void onCommandPidControl()
{
if (isReadCommand(gParameters[0]))
{
sendOnOffStatus(isPidEnabled);
sendAck();
}
else if (isOnCommandArg(gParameters[0]))
{
if (!isPidEnabled)
{
enablePid();
sendAck();
}
else
{
Serial.println(F("PID control is already on."));
sendNack();
}
}
else if (isOffCommandArg(gParameters[0]))
{
disablePid();
sendAck();
}
else
{
sendOnOffError();
}
}
// This command is complex because it only allows setting of frequency for channel 1
// This is because only the tilt motor can be controlled by frequency
void onCommandFrequencyOutput()
{
if (isChannelCorrect(gParameters[0]))
{
uint8_t channel = convertToInt(gParameters[0]);
uint16_t frequency = convertToUint(gParameters[1]);
if (channel == TILT_CHANNEL)
{
if (isReadCommand(gParameters[1]))
{
sendInt(currentFrequency);
sendAck();
}
else
{
if (!isSafetyOn)
{
if (isIntWithinRange(frequency, MOTOR_MIN_FREQUENCY, MOTOR_MAX_FREQUENCY))
{
setFrequency(frequency);
sendAck();
}
else
{
sendIntRangeError(MOTOR_MIN_FREQUENCY, MOTOR_MAX_FREQUENCY, HERTZ_UNIT);
}
}
else
{
Serial.println(F("Cannot change frequency output while safety is on."));
sendNack();
}
}
}
else
{
Serial.println(F("Changing or reading of frequency only applies to channel 1"));
sendNack();
}
}
else
{
sendChannelError();
}
}
void onCommandEcho()
{
if (gParameters[0] != NULL)
{
Serial.println(gParameters[0]);
sendAck();
}
else
{
sendNack();
}
}
void
PrefixUpdateProcessor::onInterest(const ndn::Interest& request)
{
_LOG_TRACE("Received Interest: " << request);
if (!m_isEnabled) {
sendNack(request);
return;
}
m_validator.validate(request,
bind(&PrefixUpdateProcessor::onCommandValidated, this, _1),
bind(&PrefixUpdateProcessor::onCommandValidationFailed, this, _1, _2));
}
void USART1_IRQHandler(void)
{
int i=0;
int status=SUCCESS;
if(USART_GetITStatus(USART1, USART_IT_RXNE) != RESET) //interrupt called in when you receive a char on usart1
{
GPIO_WriteBit(GPIOA,GPIO_Pin_12,Bit_SET); //pause the RF trasmission
USART_ITConfig(USART1, USART_IT_RXNE, DISABLE);//disable the RXNE interrupts
for (i=0;i<32;i++){
GPIO_WriteBit(GPIOA,GPIO_Pin_12,Bit_SET); //pause the RF trasmission
status=UsartGetTimed(USART1, &Receivedpackage.bytes[i]); //Receive the char in the buffer
GPIO_WriteBit(GPIOA,GPIO_Pin_12,Bit_RESET); //Restart the RF trasmission
if (status==ERROR) break; //if the tramission Timed out than break
}
if (status==SUCCESS){ //if you received 32 chars
if(checkDatagram(&Receivedpackage)) //and the datagram is correct parse it
status=parseDatagram(&Receivedpackage);
else status=ERROR;
}
USART_ClearFlag(USART1, USART_IT_RXNE);
USART_ClearITPendingBit(USART1, USART_IT_RXNE);
USART_ITConfig(USART1, USART_IT_RXNE, ENABLE);
GPIO_WriteBit(GPIOA,GPIO_Pin_12,Bit_RESET); //Restart the RF trasmission
if (status==SUCCESS) //if everything is correct than send ACK
sendAck();
else //If not send NACK
sendNack();
}
/*if(USART_GetITStatus(USART1, USART_IT_TXE) != RESET)
{
/* Write one byte to the transmit data register //
USART_SendData(USART1, TxBuffer[TxCount++]);
if(TxCount == NbrOfDataToTransfer)
{
// Disable the USART1 Transmit interrupt
USART_ITConfig(USART1, USART_IT_TXE, DISABLE);
} */
}
void handleCommandUnknown(char* command)
{
char tmpstr[128];
char newstr[40];
sprintf(tmpstr, "Unknown command [%s]", command);
for (int i = 0; i < INT_PARAMETER_COUNT_MAX; i++)
{
if (strlen(gParameters[i]))
{
sprintf(newstr, " %d{%s}", i, gParameters[i]);
strcat(tmpstr, newstr);
}
}
Serial.println(tmpstr);
sendNack();
}
void onCommandDacVoltage()
{
if (isChannelCorrect(gParameters[0]))
{
uint8_t channel = convertToInt(gParameters[0]);
double voltage = atof(gParameters[1]);
if (isReadCommand(gParameters[1]))
{
sendDouble(currentVoltages[channel]);
sendAck();
}
else
{
if (!isSafetyOn)
{
if (isDoubleWithinRange(voltage, MOTOR_MIN_VOLTAGE, MOTOR_MAX_VOLTAGE))
{
setDacVoltage(channel, voltage);
sendAck();
}
else
{
sendDoubleRangeError(MOTOR_MIN_VOLTAGE, MOTOR_MAX_VOLTAGE, VOLTAGE_UNIT);
}
}
else
{
Serial.println(F("Cannot change voltage while safety is on."));
sendNack();
}
}
}
else
{
sendChannelError();
}
}
void onCommandOutput()
{
if (isChannelCorrect(gParameters[0]))
{
int channel = convertToInt(gParameters[0]);
int output = convertToInt(gParameters[1]);
if (isReadCommand(gParameters[1]))
{
sendInt(currentOutputs[channel]);
sendAck();
}
else if (!isPidEnabled)
{
if (isIntWithinRange(output, PID_OUTPUT_MIN, PID_OUTPUT_MAX))
{
const Direction direction = directions[channel];
applyMotorOutputs(channel, direction, output);
sendAck();
}
else
{
sendIntRangeError(PID_OUTPUT_MIN, PID_OUTPUT_MAX, PERCENTAGE_UNIT);
}
}
else
{
Serial.println(F("Cannot change percentage output while PID control is on."));
sendNack();
}
}
else
{
sendChannelError();
}
}
bufferStruct checkReceiveData(bufferStruct buffer){
rfm12_tick(); //periodic tick function - call that one once in a while
switch(state){
case STATE_TIMEOUT:
#if STATE_UART_DEBUG >= 2
uart_putstr ("tiOu\r\n");
#endif
++timoutCounter;
if(timoutCounter > 5){
#if STATE_UART_DEBUG >= 2
uart_putstr ("tiOuOverl\r\n");
#endif
state = STATE_FREE;
stopTimer();
timoutCounter = 0;
} else {
processNack();
}
break;
case STATE_SENDING_ACK:
if(rfm12_tx_status() == STATUS_FREE){
state = STATE_SENDING_DATA;
#if STATE_UART_DEBUG >= 2
uart_putstr ("finSeAck\r\n");
#endif
bufferStruct result;
result.bufferLength = getPayloadLength(backupData.bufferLength, backupData.buffer);
memcpy(result.buffer, getPayload(backupData.bufferLength, backupData.buffer), result.bufferLength);
uart_putc('%');
uart_putc(result.bufferLength);
uart_putc('%');
for(uint8_t i = 0; i < result.bufferLength; ++i){
uart_putc(result.buffer[i]);
}
uart_putc('%');
return result;
}
case STATE_SENDING_DATA:
if(rfm12_tx_status() == STATUS_FREE){
#if STATE_UART_DEBUG >= 2
uart_putstr ("finSeData\r\n");
#endif
state = STATE_FREE;
}
}
//uart_putstr (rfm12_rx_status());
if (rfm12_rx_status() == STATUS_COMPLETE){
bufferStruct tempBuffer;
tempBuffer.bufferLength = rfm12_rx_len();
memcpy(tempBuffer.buffer, rfm12_rx_buffer(), tempBuffer.bufferLength);
if(validateCrc(tempBuffer)){
#if STATE_UART_DEBUG >= 2
uart_putstr ("crcAck\r\n");
#endif
} else {
#if STATE_UART_DEBUG >= 2
uart_putstr ("crcNack\r\n");
#endif
rfm12_rx_clear();
sendNack(tempBuffer);
return buffer;
}
if(state > STATE_FREE){
if(getAck(tempBuffer.bufferLength, tempBuffer.buffer) & ACK){
#if STATE_UART_DEBUG >= 2
uart_putstr ("rxAck\r\n");
#endif
processAck();
} else if(getType(tempBuffer.bufferLength, tempBuffer.buffer) & NACK){
#if STATE_UART_DEBUG >= 2
uart_putstr ("rxNack\r\n");
#endif
processNack();
}
}
if(state == STATE_FREE){
switch(getType(tempBuffer.bufferLength, tempBuffer.buffer)){
case SEND_DATA:
#if STATE_UART_DEBUG >= 2
uart_putstr ("send\r\n");
#endif
buffer = receiveSendData(tempBuffer);
break;
// here we should implement the cases of repeating data therefore we would have to know all last data sent
// here we should implement the get back to in -> timeframe not yet implemented
}
}
// tell the implementation that the buffer
// can be reused for the next data.
rfm12_rx_clear();
}
return buffer;
}
void sendMotorDriverError()
{
Serial.println(F("Value must be ANALOGVOLTAGE / AV or FREQUENCY / F"));
sendNack();
}
void sendIntRangeError(int lowerLimit, int upperLimit, char* unit)
{
Serial << F("Value must be between ") << lowerLimit << unit << F(" and ") << upperLimit << unit << NEWLINE;
sendNack();
}
void sendDoubleRangeError(double lowerLimit, double upperLimit, char* unit)
{
Serial << F("Value must be between ") << lowerLimit << unit << F(" and ") << upperLimit << unit << NEWLINE;
sendNack();
}
void sendChannelError()
{
Serial.println(F("Invalid channel"));
sendNack();
}
void sendProfileError()
{
Serial.println(F("Invalid profile"));
sendNack();
}
void sendSyntaxError()
{
Serial.println(F("Check syntax"));
sendNack();
}
void sendReadOnlyError()
{
Serial.println(F("Read only commands have no additional arguments"));
sendNack();
}
void sendDirectionError()
{
Serial.println(F("Value must be CLOCKWISE / CW or COUNTERCLOCKWISE / CCW"));
sendNack();
}
void sendOneOrZeroError()
{
Serial.println(F("Value must be 0 or 1"));
sendNack();
}
void sendOnOffError()
{
Serial.println(F("Value must be ON or OFF"));
sendNack();
}
