bool initNavData(ardrone* dr)
{
static char atCtrl[] = "AT*CTRL=0\r", un[14] = {1,0,0,0,0,0,0,0,0,0,0,0,0,0};
/// Commence par envoyer un paquet sur le port des données de navigation, pour que le drone commence à écouter.
fprintf(stdout, "# - Send some bytes on navdata port\n");
if(!sendNavData(dr, "Hello World !"))
return false;
/// Puis demande l'envoi de donnée de navigation.
strcpy(dr->bufferLeft, "AT*CONFIG=");
strcpy(dr->bufferRight, ",\"general:navdata_demo\",\"TRUE\"\r");
fprintf(stdout, "#%5d - Start navdata flow %s%d%s\n", dr->ident+1, dr->bufferLeft, dr->ident+1, dr->bufferRight);
if(!sendAT(dr))
return false;
/// Ensuite envoie une trame de contrôle pour indiquer au drone qu'il peut envoyer des données.
if (sendto(dr->udpSocket_at, atCtrl, strlen(atCtrl), 0,
(struct sockaddr *) &(dr->addrDroneAT),
sizeof(dr->addrDroneAT)) < 0)
{
fprintf(stderr, "## Error : Impossible d'émettre la trame AT*CTRL=0\n", dr->ident);
return false;
}
else
fprintf(stdout, "# - AT*CTRL=0 emitted.\n");
/// Puis envoie une configuration pour demander au drone d'envoyer les données de navigation en multicast.
if(!sendNavData(dr, un))
{
fprintf(stderr, "## Error : Impossible d'émettre un 1\n");
return false;
}
else
fprintf(stdout, "# - 1 envoyé.\n");
return true;
}
void Socket_Manager::initConfig()
{
// TODO: the first argument of the first command (sequence number) must begin with 0 and not 1 as for AT*PCMD
int nb = 0;
/*
//first command use in the sdk. Seems have no effect
string unknow_cmd1 = "AT*PMODE="+toString(nb++)+",2";
string unknow_cmd2= "AT*MISC="+toString(nb++)+",2,20,2000,3000";
sendCmd((char*)unknow_cmd1.c_str());
sendCmd((char*)unknow_cmd2.c_str());
*/
//-----------------> INIT THE SESSION
string cmd1 = "AT*CONFIG_IDS="+toString(nb++)+",\""ARDRONE_SESSION_ID"\",\""ARDRONE_PROFILE_ID"\",\""ARDRONE_APPLOCATION_ID"\"\r";
string cmd2 = "AT*CONFIG="+toString(nb++)+",\"custom:session_id\",\""ARDRONE_SESSION_ID"\"\r";
sendCmd((char*)cmd1.c_str());
sendCmd((char*)cmd2.c_str());
Sleep(100);
string cmd3 = "AT*CONFIG_IDS="+toString(nb++)+",\""ARDRONE_SESSION_ID"\",\""ARDRONE_PROFILE_ID"\",\""ARDRONE_APPLOCATION_ID"\"\r";
string cmd4 = "AT*CONFIG="+toString(nb++)+",\"custom:profile_id\",\""ARDRONE_PROFILE_ID"\"\r";
sendCmd((char*)cmd3.c_str());
sendCmd((char*)cmd4.c_str());
Sleep(100);
string cmd5 = "AT*CONFIG_IDS="+toString(nb++)+",\""ARDRONE_SESSION_ID"\",\""ARDRONE_PROFILE_ID"\",\""ARDRONE_APPLOCATION_ID"\"\r";
string cmd6 = "AT*CONFIG="+toString(nb++)+",\"custom:application_id\",\""ARDRONE_APPLOCATION_ID"\"\r";
sendCmd((char*)cmd5.c_str());
sendCmd((char*)cmd6.c_str());
Sleep(100);
//-----------------> START Navdata
sendNavData((char *) "\x01\x00\x00\x00");
string cmd7 = "AT*CONFIG_IDS="+toString(nb++)+",\""ARDRONE_SESSION_ID"\",\""ARDRONE_PROFILE_ID"\",\""ARDRONE_APPLOCATION_ID"\"\r";
string cmd8 = "AT*CONFIG="+toString(nb++)+",\"general:navdata_demo\",\"TRUE\"\r";
string cmd9 = "AT*CTRL="+toString(nb++)+",0\r";
sendCmd((char*)cmd7.c_str());
sendCmd((char*)cmd8.c_str());
Sleep(100);
sendCmd((char*)cmd9.c_str());// Seed ACK
Sleep(100);
//-----------------> SET THE WEBCAM TO USE, 0 for horizontal or 1 for vertical
string cmd10 = "AT*CONFIG_IDS="+toString(nb++)+",\""ARDRONE_SESSION_ID"\",\""ARDRONE_PROFILE_ID"\",\""ARDRONE_APPLOCATION_ID"\"\r";
string cmd11 = "AT*CONFIG="+toString(nb++)+",\"video:video_channel\",\"0\"\r";
sendCmd((char*)cmd10.c_str());
sendCmd((char*)cmd11.c_str());
Sleep(100);
//-----------------> SET THE QUALITY, 129 for 360p or 131 for 720p
string cmd12 = "AT*CONFIG_IDS="+toString(nb++)+",\""ARDRONE_SESSION_ID"\",\""ARDRONE_PROFILE_ID"\",\""ARDRONE_APPLOCATION_ID"\"\r";
string cmd13 = "AT*CONFIG="+toString(nb++)+",\"video:video_codec\",\"131\"\r";
sendCmd((char*)cmd12.c_str());
sendCmd((char*)cmd13.c_str());
Sleep(100);
//string cmd14 = "AT*CONFIG_IDS="+toString(nb++)+",\""ARDRONE_SESSION_ID"\",\""ARDRONE_PROFILE_ID"\",\""ARDRONE_APPLOCATION_ID"\"\r";
//string cmd15 = "AT*CONFIG="+toString(nb++)+",\"video:bitrate\",\"4000\"\r";
//sendCmd((char*)cmd14.c_str());
//sendCmd((char*)cmd15.c_str());
//Sleep(100);
//-----------------> INIT THE ENSOR CAPTOR, must only use when the drone in on the ground
string cmd16 = "AT*FTRIM="+toString(nb++)+",\r";
sendCmd((char*)cmd16.c_str());
Sleep(100);
}
