git.maemo.org Git - mardrone/blob - mardrone/ARDrone_SDK_Version_1_8_20110726/ARDroneLib/Soft/Lib/ardrone_tool/Control/ardrone_control_ack.c

   1 #include <config.h>
   2 #include <ardrone_api.h>
   3 #include <ardrone_tool/Control/ardrone_control_ack.h>
   4
   5 C_RESULT ardrone_control_ack_run( uint32_t ardrone_state, ardrone_control_ack_event_t* event )
   6 {
   7   C_RESULT res;
   8
   9   switch( event->ack_state )
  10   {
  11     case ACK_COMMAND_MASK_TRUE:
  12       res = ( ardrone_state & ARDRONE_COMMAND_MASK ) ? C_OK : C_FAIL;
  13       if( VP_SUCCEEDED(res) )
  14       {
  15           ardrone_at_update_control_mode( ACK_CONTROL_MODE, 0);
  16           event->ack_state = ACK_COMMAND_MASK_FALSE;
  17       }
  18       break;
  19
  20     case ACK_COMMAND_MASK_FALSE:
  21       ardrone_at_update_control_mode( ACK_CONTROL_MODE, 0);
  22
  23       res = ( ( ardrone_state & ARDRONE_COMMAND_MASK ) == 0 ) ? C_OK : C_FAIL;
  24       if( VP_SUCCEEDED(res) )
  25         event->status = ARDRONE_CONTROL_EVENT_FINISH_SUCCESS;
  26       break;
  27
  28     default:
  29       res = C_FAIL;
  30       event->ack_state = ACK_COMMAND_MASK_TRUE; // Go back to a know state (we may timeout)
  31       break;
  32   }
  33
  34   return res;
  35 }