void LidarController::loop()
{
while(1)
{
if (exchanger.receiveQueueSize())
{
handleIncomingMessage(exchanger.dequeueMessage());
}
}
}
void updateIncomingMessage(void) {
// Check for Message(s) And Handle If Necessary
String response_message = "";
while (communication.available()) { // read in message(s) until nothing in serial buffer
response_message += handleIncomingMessage();
}
// Append Responses From Message(s) Then Send
if (response_message != "") {
response_message = "\"GTYP\":\"Response\"," + response_message;
response_message += "\"GEND\":0";
communication.send(response_message);
}
}
void Quadcopter::loop()
{
while (1)
{
if (ascending)
{
if (altitude >= targetAltitude)
{
holdAltitude();
ascending = false;
std::cout << "Finished ascending to: "
<< targetAltitude << std::endl;
}
}
else if (descending)
{
if (altitude <= targetAltitude)
{
holdAltitude();
descending = false;
std::cout << "Finished descending to: "
<< targetAltitude << std::endl;
}
}
if (communicator.receiveQueueSize())
{
handleIncomingMessage(communicator.dequeueMessage());
}
if (!failsafe)
{
if (std::chrono::system_clock::now() - lastRCSent >=
RCHeartbeatInterval)
{
sendRCMessage();
}
if (isArmed()){
orient();
}
}
}
}
