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);
}
}
