ARDrone::ARDrone(std::string sessionId, std::string userId, std::string appId, const std::string ardIp, const unsigned short ardATCmdsPort, const unsigned short ardNavdataPort, const unsigned short ardVideoPort, const unsigned short ardControlPort, ARDroneConnections* connectionHandler, Navdata* navdata, Video* video) : m_sessionId(sessionId) , m_userId(userId) , m_appId(appId) , m_ardIp(ardIp) , m_ardATCmdsPort(ardATCmdsPort) , m_ardNavdataPort(ardNavdataPort) , m_ardVideoPort(ardVideoPort) , m_ardControlPort(ardControlPort) , m_altitudeMax(2.0f) , m_verticalSpeed(.7f) , m_rotationSpeed(3.0f) , m_euler_angle_max(0.26f) , m_isWithoutShell(false) , m_isOutdoor(false) , m_connectionsHandler(connectionHandler) , m_indexCmd(1) , m_navdata(navdata) , m_video(video) { m_video->setCallbackInitFunc([this]() {this->m_connectionsHandler->sendInitVideoData();}); AT_CONFIG("custom:session_id", sessionId); std::this_thread::sleep_for(std::chrono::milliseconds(100)); AT_CONFIG("custom:profile_id", userId); std::this_thread::sleep_for(std::chrono::milliseconds(100)); AT_CONFIG("custom:application_id", appId); std::this_thread::sleep_for(std::chrono::milliseconds(100)); setAltitudeMax(m_altitudeMax); setVerticalSpeed(m_verticalSpeed); setRotationSpeed(m_rotationSpeed); setSpeed(m_euler_angle_max); setIsWithoutShell(false); setIsOutdoor(false); // Initialize the ARDrone video stream (Codecs, etc..) setDefaultConfig(); initNavdata(); }
// -------------------------------------------------------------------------- // ARDrone::open(IP address of AR.Drone) // Description : Initialize the AR.Drone. // Return value : SUCCESS: 1 FAILURE: 0 // -------------------------------------------------------------------------- int ARDrone::open(const char *ardrone_addr) { // Initialize FFmpeg av_register_all(); avformat_network_init(); av_log_set_level(AV_LOG_QUIET); // Save IP address strncpy(ip, ardrone_addr, 16); // Get version information if (!getVersionInfo()) return 0; printf("AR.Drone Ver. %d.%d.%d\n", version.major, version.minor, version.revision); //// getVersionInfo() takes too long before realising the drone is not connected //// it call tcp open, which makes a socket and tries to connect to it //// using the connect method, which has too big of a timeout. //// making a smaller, 5 sec timeout before checking the verison to see if drone connected. //// if not, return 0 //if (!version.major) { // // create timeout and wait 5 sec // printf("Waiting for drone to connect...\n"); // msleep(5000); // // check again // if (!version.major) return 0; //} // Initialize AT command if (!initCommand()) return 0; // Initialize Navdata if (!initNavdata()) return 0; // Initialize Video if (!initVideo()) return 0; // Wait for updating state //msleep(500); // Get configurations if (!getConfig()) return 0; // Reset emergency resetWatchDog(); resetEmergency(); return 1; }
// -------------------------------------------------------------------------- // ARDrone::open(IP address of AR.Drone) // Description : Initialize the AR.Drone. // Return value : SUCCESS: 1 FAILURE: 0 // -------------------------------------------------------------------------- int ARDrone::open(const char *ardrone_addr) { #if _WIN32 // Initialize WSA WSAData wsaData; WSAStartup(MAKEWORD(1,1), &wsaData); #endif // Initialize FFmpeg av_register_all(); avformat_network_init(); av_log_set_level(AV_LOG_QUIET); // Save IP address strncpy(ip, ardrone_addr, 16); // Get version information if (!getVersionInfo()) return 0; printf("AR.Drone Ver. %d.%d.%d\n", version.major, version.minor, version.revision); // Get configurations if (!getConfig()) return 0; // Initialize AT command if (!initCommand()) return 0; // Initialize Navdata if (!initNavdata()) return 0; // Initialize Video if (!initVideo()) return 0; // Wait for updating state msleep(500); // Reset emergency resetWatchDog(); resetEmergency(); return 1; }
void* controlTask(void* arg) { pthread_mutex_t verrou_control; pthread_cond_t cond; pthread_cond_init(&cond, NULL); pthread_mutex_init(&verrou_control, NULL); struct timeval tp; struct timespec ts; float pitch_cmd = 0.0; float roll_cmd = 0.0; float angular_speed_cmd = 0.0; float vertical_speed_cmd = 0.0; initialize_at_com(); //initialisation du soket pour les commandes AT. initNavdata(); //initialisation du soket pour les navdata. Main_Nav = getNavdata(); //initialisation_uart(); //initialisation de la commuication Uart. //creation of graph FILE* file = fopen(NAME_MAP_DEMO, "r"); graph = createGraph(file); fclose(file); pthread_mutex_lock(&mutex_control); inC.flag_control_e = CONTROL_ENABLED_SCADE; inC.flag_control_s = STATE_MANUAL; inC.flag_takeoff = 0; inC.flag_land = 0; inC.flag_emergency = 0; inC.flag_calibH = 0; inC.flag_calibM = 0; inC.flag_pitchcalled = 0; inC.flag_rollcalled = 0; inC.flag_rollpitchcalled = 0; inC.flag_yawcalled = 0; system_state_machine_reset(&outC); pthread_mutex_unlock(&mutex_control); while(1) { //printf("Debut de la boucle while(1) du thread controlTask\n"); //récupération de la clock courante gettimeofday(&tp, NULL); ts.tv_sec = tp.tv_sec; ts.tv_nsec = tp.tv_usec * 1000; //application de la nouvelle période d'éxécution du thread_envoi_ordre ts.tv_nsec += CONTROLTASK_PERIOD_CONTROLE_MS * 1000000; ts.tv_sec += ts.tv_nsec / 1000000000L; ts.tv_nsec = ts.tv_nsec % 1000000000L; pthread_mutex_lock(&verrou_control); //printf("Passage du pthread_mutex_lock(&verrou_control) dans controlTask\n"); pthread_mutex_lock(&mutex_control); //printf("Passage du pthread_mutex_lock(&mutex_control) dans controlTask\n"); system_state_machine_reset(&outC); system_state_machine(&inC,&outC); //CODE SCADE //printf("ordre genere par la machine scade = %d\r", outC.order_called); SWITCH_DRONE_COMMANDE(outC.order_called);//SWITCH pour l'envoi des ordres pthread_mutex_unlock(&mutex_control); pthread_cond_timedwait(&cond, &verrou_control, &ts); pthread_mutex_unlock(&verrou_control); } }