static void* at_cmds_loop(void *arg)
{
unsigned long current, deadline;
INFO("AT commands thread starting (thread=%d)...\n", (int)pthread_self());
user_input = MYKONOS_NO_TRIM;
ocurrent = get_time_ms();
while (at_thread_alive) {
if (get_mask_from_state(mykonos_state, MYKONOS_NAVDATA_BOOTSTRAP)) {
boot_drone(nb_sequence++);
continue;
}
// INFO("Comm_attempt %i\n",mykonos_state);
// compute next loop iteration deadline
deadline = get_time_ms() + MYKONOS_REFRESH_MS;
// send pilot command
send_command(0);
// sleep until deadline
current = get_time_ms();
if (current < deadline) {
usleep(1000*(deadline-current));
}
}
INFO("AT commands thread stopping\n");
return NULL;
}
static void* at_cmds_loop(void *arg)
{
INFO("AT commands thread starting (thread=%d)...\n", (int)pthread_self());
user_input = MYKONOS_NO_TRIM;
ocurrent = get_time_ms();
while (at_thread_alive) {
if (get_mask_from_state(mykonos_state, ARDRONE_NAVDATA_BOOTSTRAP)) {
INFO("attempting to boot drone...\n");
boot_drone();
nb_sequence = 0;
continue;
}
send_command( nb_sequence++ );
// sleep until deadline
usleep(200 * 1000);
}
INFO("AT commands thread stopping\n");
return NULL;
}
static void boot_drone(int attempt)
{
char cmds[1000];
INFO ("Attempting to boot \n");
sprintf(cmds,"AT*CONFIG=%i,\"general:navdata_demo\",\"TRUE\"\r",nb_sequence++);
nb_sequence = nb_sequence+2;
at_write ((int8_t*)cmds, strlen (cmds));
printf (cmds);
at_comwdg();
if ( get_mask_from_state( mykonos_state,MYKONOS_NAVDATA_BOOTSTRAP )) {
at_write ((int8_t*)cmds, strlen (cmds));
INFO ("Attempting to boot \n");
int retry = 20;
int bcontinue = true;
int next = 0;
while (bcontinue && retry) {
if (next == 0) {
if (get_mask_from_state( mykonos_state, MYKONOS_COMMAND_MASK )) {
INFO ("[CONTROL] Processing the current command ... \n");
next++;
}else{
sprintf(cmds,"AT*CONFIG=%i,\"general:navdata_demo\",\"TRUE\"\r",nb_sequence++);
at_write ((int8_t*)cmds, strlen (cmds));
}
fprintf(stdout,"Mykonos 1\n");
}
else {
/* char str[AT_BUFFER_SIZE];
memset (str, 0, AT_BUFFER_SIZE);
sprintf (str, "AT*CTRL=%d,%d\r", ACK_CONTROL_MODE, 0);
at_write ((int8_t*)str, strlen (str));
fprintf(stdout,"Mykonos 2\n");*/
if ( !get_mask_from_state( mykonos_state, MYKONOS_COMMAND_MASK )) {
INFO ("[CONTROL] Ack control event OK, send navdata demo\n");
bcontinue = FALSE;
}
}
usleep (100000);
retry--;
}
}
at_comwdg();
}
static void boot_drone(void)
{
char str[AT_BUFFER_SIZE];
vp_os_memset (str, 0, AT_BUFFER_SIZE);
sprintf (str, "AT*CONFIG=%d,\"general:navdata_demo\",\"TRUE\"\r", nb_sequence++);
if ( get_mask_from_state( mykonos_state, ARDRONE_NAVDATA_BOOTSTRAP )) {
at_write ((int8_t*)str, strlen (str));
int retry = 20;
int bcontinue = TRUE;
int next = 0;
while (bcontinue && retry) {
if (next == 0) {
if ( get_mask_from_state( mykonos_state, ARDRONE_COMMAND_MASK )) {
INFO ("[CONTROL] Processing the current command ... \n");
next++;
}
}
else {
char str[AT_BUFFER_SIZE];
vp_os_memset (str, 0, AT_BUFFER_SIZE);
sprintf (str, "AT*CTRL=%d,%d,%d\r", nb_sequence++, ACK_CONTROL_MODE, 0);
at_write ((int8_t*)str, strlen (str));
if ( !get_mask_from_state( mykonos_state, ARDRONE_COMMAND_MASK )) {
INFO ("[CONTROL] Ack control event OK, send navdata demo\n");
bcontinue = FALSE;
}
}
sleep (1);
retry--;
}
}
}
/*!
Thread main function.
Reads status data received from the drone.
*/
void NavDataHandler::run()
{
qDebug() << "[NavDataHandler] run";
uint sequence = NAVDATA_SEQUENCE_DEFAULT - 1;
uint droneState = 0;
bool emergencyState = false;
uint batteryLevel = 4;
forever
{
qDebug() << "[NavDataHandler] Entering main event loop";
// Do the stuff but check if we need to abort first...
if (mAbort)
{
return;
}
QByteArray datagram;
// Copy the next datagram in the list to a variable that is local to the thread.
mMutex.lock();
if (!mDatagramList.empty())
{
datagram = mDatagramList.takeFirst();
}
mMutex.unlock();
int size = datagram.size();
if (size == 0)
{
qDebug() << "[NavDataHandler] datagram size = 0";
sequence = NAVDATA_SEQUENCE_DEFAULT - 1;
}
else
{
navdata_t* navdata = (navdata_t*) datagram.data();
if (navdata->header == NAVDATA_HEADER)
{
if (get_mask_from_state(navdata->ardrone_state, ARDRONE_NAVDATA_BOOTSTRAP))
{
qDebug() << "[NavDataHandler] ARDRONE_NAVDATA_BOOTSTRAP";
mDroneBooting = true;
emit sendInitSequence(1);
}
else if (mDroneBooting && get_mask_from_state(navdata->ardrone_state, ARDRONE_COMMAND_MASK))
{
qDebug() << "[NavDataHandler] ARDRONE_COMMAND_MASK";
mDroneBooting = false;
emit sendInitSequence(2);
}
else
{
// If drone status has changed, signal it to the other components.
if (droneState != navdata->ardrone_state)
{
droneState = navdata->ardrone_state;
emit updateARDroneState(droneState);
}
// If emergency state has changed, signal it to the other components.
if (emergencyState != get_mask_from_state(droneState, ARDRONE_EMERGENCY_MASK))
{
qDebug() << "[NavDataHandler] ARDRONE_EMERGENCY_MASK";
emergencyState = get_mask_from_state(droneState, ARDRONE_EMERGENCY_MASK);
emit updateEmergencyState(emergencyState);
}
// If the drone communication watchdog has been active then the sequence numbering
// has been restarted.
if (get_mask_from_state(droneState, ARDRONE_COM_WATCHDOG_MASK))
{
qDebug() << "[NavDataHandler] ARDRONE_COM_WATCHDOG_MASK";
sequence = NAVDATA_SEQUENCE_DEFAULT - 1;
}
// Check if this is not old data we have received.
if (navdata->sequence > sequence)
{
quint32 navdata_cks = 0;
navdata_unpacked_5_t navdata_unpacked;
bool ret = ardrone_navdata_unpack_all_5(&navdata_unpacked, navdata, &navdata_cks);
if (ret)
{
// Compute checksum.
quint32 cks = navdata_compute_cks((quint8*) datagram.data(), size - sizeof(navdata_cks_t));
// Compare checksums to see if data is valid.
if (cks == navdata_cks)
{
// Data is valid.
// Battery level is the only information we are using from this structure.
uint tmpBatLevel = 0;
uint tmpBat = navdata_unpacked.navdata_demo.vbat_flying_percentage;
if (tmpBat > 75)
{
tmpBatLevel = 3;
}
else if (tmpBat > 50)
{
tmpBatLevel = 2;
}
else if (tmpBat > 25)
{
tmpBatLevel = 1;
}
else
{
tmpBatLevel = 0;
}
if (tmpBatLevel != batteryLevel)
{
batteryLevel = tmpBatLevel;
emit updateBatteryLevel(batteryLevel);
}
}
else
{
qDebug() << "[NavDataHandler] Checksum failed!";
}
}
}
sequence = navdata->sequence;
}
}
}
// If we have more data in queue, then wait 10 milliseconds before continuing.
// Else wait until new data arrives.
/*mMutex.lock();
if (mDatagramList.count() > 0)
{
mCondition.wait(&mMutex, 10);
}
else
{
mCondition.wait(&mMutex);
}
mMutex.unlock();*/
}
}
