/************* at_set_flat_trim ****************
* Description : Calibration of the ARDrone.
*/
void at_trim( void )
{
pthread_mutex_lock( &at_cmd_lock );
snprintf(str, AT_BUFFER_SIZE, "AT*FTRIM=%d\r",nb_sequence++);
pthread_mutex_unlock( &at_cmd_lock );
at_write((int8_t*)str, strlen (str));
usleep(100000);
sprintf(str,"AT*CONFIG=%i,\"control:altitude_max\",\"10000\"\r",nb_sequence++);
at_write((int8_t*)str, strlen (str));
}
static void send_command(int nab_sequence)
{
unsigned long current;
// send command to drone
current = get_time_ms();
pthread_mutex_lock( &at_cmd_lock );
snprintf(str, AT_BUFFER_SIZE, "AT*PCMD=%d,%d,%d,%d,%d,%d\r",nb_sequence++,*(int*)&radiogp_cmd.hover,*(int*)&radiogp_cmd.pitch,*(int*)&radiogp_cmd.roll,*(int*)&radiogp_cmd.gaz,*(int*)&radiogp_cmd.yaw);
pthread_mutex_unlock( &at_cmd_lock );
at_write((int8_t*)str, strlen (str));
/* pthread_mutex_lock( &at_cmd_lock );
snprintf(str,AT_BUFFER_SIZE,"AT*COMWDG=%i\r",nb_sequence++);
pthread_mutex_unlock( &at_cmd_lock );
at_write((int8_t*)str, strlen (str));*/
//snprintf(str,AT_BUFFER_SIZE,"AT*COMWDG=%i\r",nb_sequence);
// check 30 ms overflow
if (current > ocurrent + 30) {
overflow += current - ocurrent - MYKONOS_REFRESH_MS;
}
ocurrent = current;
/* dump command every 2s */
if ((nb_sequence & 63) == 0) {
pthread_mutex_lock( &at_cmd_lock );
//INFO("seq=%d radgp(%f,%f,%f,%f) ui=0x%08x over=%d\n", nb_sequence,radiogp_cmd.pitch, radiogp_cmd.roll, radiogp_cmd.gaz, radiogp_cmd.yaw, user_input, overflow);
pthread_mutex_unlock( &at_cmd_lock );
overflow = 0;
}
}
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();
}
/************* at_ui_pad_start_pressed ****************
* Description : Takeoff/Landing, used with at_cmds_loop function.
*/
void at_ui_pad_start_pressed( void )
{
if (!at_thread)
return;
pthread_mutex_lock( &at_cmd_lock );
user_input ^= 1 << MYKONOS_UI_BIT_START;
pthread_mutex_unlock( &at_cmd_lock );
pthread_mutex_lock( &at_cmd_lock );
snprintf(str, AT_BUFFER_SIZE, "AT*REF=%d,%d\r",nb_sequence++,user_input);
pthread_mutex_unlock( &at_cmd_lock );
at_write((int8_t*)str, strlen (str));
}
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--;
}
}
}
static void send_command(int nb_sequence)
{
char str[AT_BUFFER_SIZE];
// send command to drone
pthread_mutex_lock( &at_cmd_lock );
snprintf(str, AT_BUFFER_SIZE, "AT*PCMD=%d,0,0,0,0,0\r", nb_sequence);
pthread_mutex_unlock( &at_cmd_lock );
//Send AT command
at_write((int8_t*)str, strlen (str));
/* dump command every 2s */
if ((nb_sequence % 50) == 0)
{
pthread_mutex_lock( &at_cmd_lock );
// INFO("MYKONOS state: %d \n", mykonos_state);
pthread_mutex_unlock( &at_cmd_lock );
}
}
/**
* Read sound to buffer with size of buffer by using Read function of audiotrack class
*
* @param void* buffer
* @param int size
*
* @return int
*/
int AndroidAudioTrack::write(const void* buffer, unsigned int size) {
if (mAudioTrack && at_write) {
return at_write(mAudioTrack, buffer, size);
}
return 0;
}
void at_zap(int cam)
{
char str[1000];
sprintf(str, "AT*CONFIG=%d,\"video:video_channel\",\"%d\"\r",nb_sequence++,cam);
at_write ((int8_t*)str, strlen (str));
}
void at_comwdg()
{
snprintf(str,AT_BUFFER_SIZE,"AT*COMWDG=%i\r",nb_sequence++);
at_write((int8_t*)str, strlen (str));
}
static void* navdata_loop(void *arg)
{
uint8_t msg[NAVDATA_BUFFER_SIZE];
navdata_unpacked_t navdata_unpacked;
unsigned int cks, navdata_cks, sequence = NAVDATA_SEQUENCE_DEFAULT-1;
int sockfd = -1, addr_in_size;
struct sockaddr_in *my_addr, *from;
INFO("NAVDATA thread starting (thread=%d)...\n", (int)pthread_self());
navdata_open_server();
addr_in_size = sizeof(struct sockaddr_in);
navdata_t* navdata = (navdata_t*) &msg[0];
from = (struct sockaddr_in *)vp_os_malloc(addr_in_size);
my_addr = (struct sockaddr_in *)vp_os_malloc(addr_in_size);
assert(from);
assert(my_addr);
vp_os_memset((char *)my_addr,(char)0,addr_in_size);
my_addr->sin_family = AF_INET;
my_addr->sin_addr.s_addr = htonl(INADDR_ANY);
my_addr->sin_port = htons( NAVDATA_PORT );
if((sockfd = socket (AF_INET, SOCK_DGRAM, 0)) < 0){
INFO ("socket: %s\n", strerror(errno));
goto fail;
};
if(bind(sockfd, (struct sockaddr *)my_addr, addr_in_size) < 0){
INFO ("bind: %s\n", strerror(errno));
goto fail;
};
{
struct timeval tv;
// 1 second timeout
tv.tv_sec = 1;
tv.tv_usec = 0;
setsockopt( sockfd, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv));
}
INFO("[NAVDATA] Ready to receive incomming data\n");
while ( nav_thread_alive ) {
int size;
size = recvfrom (sockfd, &msg[0], NAVDATA_BUFFER_SIZE, 0, (struct sockaddr *)from,
(socklen_t *)&addr_in_size);
//INFO ("SIZE %d \n", size);
if( size == 0)
{
INFO ("Navdata lost connection \n");
navdata_open_server();
sequence = NAVDATA_SEQUENCE_DEFAULT-1;
}
// if (size == 24)
// {
// const char cmds[] = "AT*COMWDG=0\r";
// at_write ((int8_t*)cmds, strlen( cmds ));
//
// const char cmds2[] = "AT*CONFIG=1,\"general:navdata_demo\",\"TRUE\"\r";
// at_write ((int8_t*)cmds2, strlen( cmds2 ));
// }
if( navdata->header == NAVDATA_HEADER )
{
mykonos_state = navdata->ardrone_state;
if(ardrone_get_mask_from_state(navdata->ardrone_state, ARDRONE_COM_WATCHDOG_MASK) )
{
// INFO ("[NAVDATA] Detect com watchdog\n");
sequence = NAVDATA_SEQUENCE_DEFAULT-1;
if( ardrone_get_mask_from_state(navdata->ardrone_state, ARDRONE_NAVDATA_BOOTSTRAP) == FALSE )
{
//INFO ("[NAVDATA] Send com watchdog\n");
const char cmds[] = "AT*COMWDG=0\r";
at_write ((int8_t*)cmds, strlen( cmds ));
}
}
if( navdata->sequence > sequence )
{
if ( ardrone_get_mask_from_state( mykonos_state, ARDRONE_NAVDATA_DEMO_MASK ))
{
ardrone_navdata_unpack_all(&navdata_unpacked, navdata, &navdata_cks);
cks = ardrone_navdata_compute_cks( &msg[0], size - sizeof(navdata_cks_t) );
if( cks != navdata_cks )
{
INFO("NAVADATA wrong CKS\n");
}
//INFO("VALUE = %f \n", navdata_unpacked.navdata_demo.theta);
}
}
else
{
INFO ("[Navdata] Sequence pb : %d (distant) / %d (local)\n", navdata->sequence, sequence);
}
sequence = navdata->sequence;
}
}
fail:
vp_os_free(from);
vp_os_free(my_addr);
if (sockfd >= 0){
close(sockfd);
}
if (navdata_udp_socket >= 0){
close(navdata_udp_socket);
navdata_udp_socket = -1;
}
INFO("NAVDATA thread stopping\n");
return NULL;
}
/************* at_set_flat_trim ****************
* Description : Calibration of the ARDrone.
*/
void at_set_flat_trim( void )
{
const char cmd[] = "AT*FTRIM\r";
at_write ((int8_t*)cmd, strlen (cmd));
}
