DEFINE_THREAD_ROUTINE(mobile_main, data)
{
C_RESULT res = C_FAIL;
vp_com_wifi_config_t *config = NULL;
mobile_main_param_t *param = (mobile_main_param_t *)data;
ardroneEngineCallback callback = param->callback;
vp_os_memset(drone_address, 0x0, sizeof(drone_address));
// TODO(johnb): Make this autodetect based on network interfaces
while(((config = (vp_com_wifi_config_t *)wifi_config()) != NULL) && (strcmp(config->itfName, WIFI_ITFNAME) != 0))
{
PRINT("Wait WIFI connection !\n");
vp_os_delay(250);
}
// Get drone_address
vp_os_memcpy(drone_address, config->server, strlen(config->server));
PRINT("Drone address %s\n", drone_address);
// Get iphone_mac_address
get_iphone_mac_address(config->itfName);
PRINT("Iphone MAC Address %s\n", iphone_mac_address);
res = ardrone_tool_setup_com( NULL );
if( FAILED(res) )
{
PRINT("Wifi initialization failed. It means either:\n");
PRINT("\t* you're not root (it's mandatory because you can set up wifi connection only as root)\n");
PRINT("\t* wifi device is not present (on your pc or on your card)\n");
PRINT("\t* you set the wrong name for wifi interface (for example rausb0 instead of wlan0) \n");
PRINT("\t* ap is not up (reboot card or remove wifi usb dongle)\n");
PRINT("\t* wifi device has no antenna\n");
}
else
{
START_THREAD(video_stage, NULL);
res = ardrone_tool_init(drone_address, strlen(drone_address), NULL, param->appName, param->usrName);
callback(ARDRONE_ENGINE_INIT_OK);
ardrone_tool_set_refresh_time(1000 / kAPS);
while( SUCCEED(res) && bContinue == TRUE )
{
ardrone_tool_update();
}
JOIN_THREAD(video_stage);
res = ardrone_tool_shutdown();
}
vp_os_free (data);
return (THREAD_RET)res;
}
//Init function
void ICACHE_FLASH_ATTR user_init()
{
uart_init(BIT_RATE_57600,BIT_RATE_57600);
//Set GPIO2 to output mode
PIN_FUNC_SELECT(PERIPHS_IO_MUX_GPIO2_U, FUNC_GPIO2);
//Set GPIO2 low
gpio_output_set(0, BIT2, BIT2, 0);
/*############ RESET SEQUENZE START #####################*/
//Set GPIO0 to output mode
PIN_FUNC_SELECT(PERIPHS_IO_MUX_GPIO0_U, FUNC_GPIO0);
//Set GPIO0 low
gpio_output_set(0, BIT0, BIT0, 0);
os_delay_us(250000); //250ms = 0,25sec!
//Set GPIO0 high
gpio_output_set(BIT0, 0, BIT0, 0);
os_delay_us(250000); //250ms = 0,25sec!
/*############ RESET SEQUENZE END #######################*/
//Timer LED
os_timer_disarm(&led_timer);
os_timer_setfn(&led_timer, (os_timer_func_t *)LedTimer, NULL);
os_timer_arm(&led_timer, 500, 1);
os_event_t *queue = os_malloc(sizeof(os_event_t) * recvTaskQueueLen);
system_os_task(recvTask, recvTaskPrio, queue, recvTaskQueueLen);
wifi_config();
network_init();
}
static void ICACHE_FLASH_ATTR
user_key_long_press_cb()
{
PRINTF("Long press occurred");
wifi_config(NULL);
}
int main(int argc, char **argv)
{
C_RESULT res;
const char* old_locale;
const char* appname = argv[0];
int argc_backup = argc;
char** argv_backup = argv;
bool_t show_usage = FAILED( ardrone_tool_check_argc_custom(argc) ) ? TRUE : FALSE;
argc--; argv++;
while( argc && *argv[0] == '-' )
{
if( !strcmp(*argv, "-?") || !strcmp(*argv, "-h") || !strcmp(*argv, "-help") || !strcmp(*argv, "--help") )
{
ardrone_tool_usage( appname );
exit( 0 );
}
else if( !ardrone_tool_parse_cmd_line_custom( *argv ) )
{
printf("Option %s not recognized\n", *argv);
show_usage = TRUE;
}
argc--; argv++;
}
if( show_usage || (argc != 0) )
{
ardrone_tool_usage( appname );
exit(-1);
}
/* After a first analysis, the arguments are restored so they can be passed to the user-defined functions */
argc=argc_backup;
argv=argv_backup;
old_locale = setlocale(LC_NUMERIC, "en_GB.UTF-8");
if( old_locale == NULL )
{
PRINT("You have to install new locales in your dev environment! (avoid the need of conv_coma_to_dot)\n");
PRINT("As root, do a \"dpkg-reconfigure locales\" and add en_GB.UTF8 to your locale settings\n");
PRINT("If you have any problem, feel free to contact Pierre Eline ([email protected])\n");
}
else
{
PRINT("Setting locale to %s\n", old_locale);
}
vp_com_wifi_config_t *config = (vp_com_wifi_config_t*)wifi_config();
if(config)
{
vp_os_memset( &wifi_ardrone_ip[0], 0, sizeof(wifi_ardrone_ip) );
printf("===================+> %s\n", config->server);
strcpy( &wifi_ardrone_ip[0], config->server);
}
if( &custom_main )
{
return custom_main(argc, argv);
}
else
{
res = ardrone_tool_setup_com( NULL );
if( FAILED(res) )
{
PRINT("Wifi initialization failed. It means either:\n");
PRINT("\t* you're not root (it's mandatory because you can set up wifi connection only as root)\n");
PRINT("\t* wifi device is not present (on your pc or on your card)\n");
PRINT("\t* you set the wrong name for wifi interface (for example rausb0 instead of wlan0) \n");
PRINT("\t* ap is not up (reboot card or remove wifi usb dongle)\n");
PRINT("\t* wifi device has no antenna\n");
}
else
{
res = ardrone_tool_init(argc, argv);
while( SUCCEED(res) && ardrone_tool_exit() == FALSE )
{
res = ardrone_tool_update();
}
res = ardrone_tool_shutdown();
}
}
if( old_locale != NULL )
{
setlocale(LC_NUMERIC, old_locale);
}
return SUCCEED(res) ? 0 : -1;
}
int ardrone_tool_main(int argc, char **argv)
{
C_RESULT res;
const char* old_locale;
const char* appname = argv[0];
int argc_backup = argc;
char** argv_backup = argv;
char * drone_ip_address = NULL;
struct in_addr drone_ip_address_in;
bool_t show_usage = FAILED( ardrone_tool_check_argc_custom(argc) ) ? TRUE : FALSE;
argc--; argv++;
while( argc && *argv[0] == '-' )
{
if( !strcmp(*argv, "-ip") && ( argc > 1 ) )
{
drone_ip_address = *(argv+1);
printf("Using custom ip address %s\n",drone_ip_address);
argc--; argv++;
}
else if( !strcmp(*argv, "-?") || !strcmp(*argv, "-h") || !strcmp(*argv, "-help") || !strcmp(*argv, "--help") )
{
ardrone_tool_usage( appname );
exit( 0 );
}
else if( !ardrone_tool_parse_cmd_line_custom( *argv ) )
{
printf("Option %s not recognized\n", *argv);
show_usage = TRUE;
}
argc--; argv++;
}
if( show_usage || (argc != 0) )
{
ardrone_tool_usage( appname );
exit(-1);
}
/* After a first analysis, the arguments are restored so they can be passed to the user-defined functions */
argc=argc_backup;
argv=argv_backup;
old_locale = setlocale(LC_NUMERIC, "en_GB.UTF-8");
if( old_locale == NULL )
{
PRINT("You have to install new locales in your dev environment! (avoid the need of conv_coma_to_dot)\n");
PRINT("As root, do a \"dpkg-reconfigure locales\" and add en_GB.UTF8 to your locale settings\n");
}
else
{
PRINT("Setting locale to %s\n", old_locale);
}
vp_com_wifi_config_t *config = (vp_com_wifi_config_t*)wifi_config();
if(config)
{
vp_os_memset( &wifi_ardrone_ip[0], 0, sizeof(wifi_ardrone_ip) );
if(drone_ip_address && inet_aton(drone_ip_address,&drone_ip_address_in)!=0)
{
/* If the drone IP address was given on the command line and is valid */
printf("===================+> %s\n", drone_ip_address);
strncpy( &wifi_ardrone_ip[0], drone_ip_address, sizeof(wifi_ardrone_ip)-1);
}
else
{
printf("===================+> %s\n", config->server);
strncpy( &wifi_ardrone_ip[0], config->server, sizeof(wifi_ardrone_ip)-1);
}
}
while (-1 == getDroneVersion (root_dir, wifi_ardrone_ip, &ardroneVersion))
{
printf ("Getting AR.Drone version ...\n");
vp_os_delay (250);
}
res = ardrone_tool_setup_com( NULL );
if( FAILED(res) )
{
PRINT("Wifi initialization failed. It means either:\n");
PRINT("\t* you're not root (it's mandatory because you can set up wifi connection only as root)\n");
PRINT("\t* wifi device is not present (on your pc or on your card)\n");
PRINT("\t* you set the wrong name for wifi interface (for example rausb0 instead of wlan0) \n");
PRINT("\t* ap is not up (reboot card or remove wifi usb dongle)\n");
PRINT("\t* wifi device has no antenna\n");
}
else
{
// Save appname/appid for reconnections
char *appname = NULL;
int lastSlashPos;
/* Cut the invoking name to the last / or \ character on the command line
* This avoids using differents app_id for applications called from different directories
* e.g. if argv[0] is "Build/Release/ardrone_navigation", appname will point to "ardrone_navigation" only
*/
for (lastSlashPos = strlen (argv[0])-1; lastSlashPos > 0 && argv[0][lastSlashPos] != '/' && argv[0][lastSlashPos] != '\\'; lastSlashPos--);
appname = &argv[0][lastSlashPos+1];
ardrone_gen_appid (appname, __SDK_VERSION__, app_id, app_name, sizeof (app_name));
res = ardrone_tool_init(wifi_ardrone_ip, strlen(wifi_ardrone_ip), NULL, appname, NULL, NULL, NULL, MAX_FLIGHT_STORING_SIZE, NULL);
while( SUCCEED(res) && ardrone_tool_exit() == FALSE )
{
res = ardrone_tool_update();
}
res = ardrone_tool_shutdown();
}
if( old_locale != NULL )
{
setlocale(LC_NUMERIC, old_locale);
}
return SUCCEED(res) ? 0 : -1;
}
C_RESULT vp_com_open_socket(vp_com_socket_t* sck, Read* read, Write* write)
{
C_RESULT res = VP_COM_OK;
int s = -1, type;
struct sockaddr_in name = { 0 };
switch( sck->protocol )
{
case VP_COM_TCP:
s = socket( AF_INET, SOCK_STREAM, 0 );
res = ( s < 0 ) ? VP_COM_ERROR : VP_COM_OK;
type = sck->type;
break;
case VP_COM_UDP:
s = socket( AF_INET, SOCK_DGRAM, 0 );
sck->scn = inet_addr(sck->serverHost); // Cache destination in int format
res = ( s < 0 ) ? VP_COM_ERROR : VP_COM_OK;
type = VP_COM_SERVER; // Make sure we will bind the socket in the connection-less protocol
break;
default:
type = VP_COM_CLIENT;
res = VP_COM_PARAMERROR;
break;
}
if( FAILED(res) )
{
PRINT("\nSocket opening failed\n");
}
VP_COM_CHECK( res );
name.sin_family = AF_INET;
name.sin_port = htons( sck->port );
switch( type )
{
case VP_COM_CLIENT:
name.sin_addr.s_addr = inet_addr(sck->serverHost);
if ( connect( s, (struct sockaddr*)&name, sizeof( name ) ) == -1 )
res = VP_COM_ERROR;
break;
case VP_COM_SERVER:
name.sin_addr.s_addr = INADDR_ANY;
if ( bind( s, (struct sockaddr*)&name, sizeof(struct sockaddr)) < 0 )
res = VP_COM_ERROR;
if ( sck->is_multicast == 1 )
{
if ( sck->multicast_base_addr == 0 )
{
PRINT("Error : multicast base address is not defined\n");
res = VP_COM_ERROR;
break;
}
in_addr_t multicast_address, drone_address;
vp_com_wifi_config_t *wifi_cfg = (vp_com_wifi_config_t*) wifi_config();
// compute remote address according to local address
drone_address = inet_addr(wifi_cfg->server);
multicast_address = htonl( sck->multicast_base_addr | (ntohl(drone_address) & 0xFF) );
struct ip_mreq mreq;
mreq.imr_interface.s_addr = inet_addr(wifi_cfg->localHost);
mreq.imr_multiaddr.s_addr = multicast_address;
if ( setsockopt( s, IPPROTO_IP, IP_ADD_MEMBERSHIP, &mreq, sizeof(mreq) ) < 0) {
PRINT("!!! Error enabling multicast !!!\n");
}
}
break;
default:
res = VP_COM_PARAMERROR;
break;
}
if(res == VP_COM_OK)
{
sck->priv = (void*) s;
switch( sck->protocol )
{
case VP_COM_TCP:
if(read) *read = (Read) vp_com_read_socket;
if(write) *write = (Write) vp_com_write_socket;
break;
case VP_COM_UDP:
if(read) *read = (Read) vp_com_read_udp_socket;
if(write) *write = (Write) vp_com_write_udp_socket;
break;
default:
if(read) *read = NULL;
if(write) *write = NULL;
break;
}
}
else
{
close( s );
}
if (sck->block != VP_COM_DEFAULT &&
sck->block != VP_COM_WAITALL &&
sck->block != VP_COM_DONTWAIT)
{
sck->block = VP_COM_DEFAULT;
}
return res;
}
DEFINE_THREAD_ROUTINE(app_main, data)
{
C_RESULT res = C_FAIL;
vp_com_wifi_config_t* config = NULL;
JNIEnv* env = NULL;
if (g_vm) {
(*g_vm)->AttachCurrentThread (g_vm, (JNIEnv **) &env, NULL);
}
bContinue = TRUE;
mobile_main_param_t *param = data;
video_recorder_thread_param_t video_recorder_param;
video_recorder_param.priority = VIDEO_RECORDER_THREAD_PRIORITY;
video_recorder_param.finish_callback = param->academy_download_callback_func;
vp_os_memset(&ardrone_info, 0x0, sizeof(ardrone_info_t));
while ((config = (vp_com_wifi_config_t *)wifi_config()) != NULL && strlen(config->itfName) == 0)
{
//Waiting for wifi initialization
vp_os_delay(250);
if (ardrone_tool_exit() == TRUE) {
if (param != NULL && param->callback != NULL) {
param->callback(env, param->obj, ARDRONE_MESSAGE_DISCONNECTED);
}
return 0;
}
}
vp_os_memcpy(&ardrone_info.drone_address[0], config->server, strlen(config->server));
while (-1 == getDroneVersion (param->root_dir, &ardrone_info.drone_address[0], &ardroneVersion))
{
LOGD (TAG, "Getting AR.Drone version");
vp_os_delay (250);
}
sprintf(&ardrone_info.drone_version[0], "%u.%u.%u", ardroneVersion.majorVersion, ardroneVersion.minorVersion, ardroneVersion.revision);
LOGD (TAG, "ARDrone Version : %s\n", &ardrone_info.drone_version[0]);
LOGI(TAG, "Drone Family: %d", ARDRONE_VERSION());
res = ardrone_tool_setup_com( NULL );
if( FAILED(res) )
{
LOGII("Setup com failed");
LOGW(TAG, "Wifi initialization failed. It means either:");
LOGW(TAG, "\t* you're not root (it's mandatory because you can set up wifi connection only as root)\n");
LOGW(TAG, "\t* wifi device is not present (on your pc or on your card)\n");
LOGW(TAG, "\t* you set the wrong name for wifi interface (for example rausb0 instead of wlan0) \n");
LOGW(TAG, "\t* ap is not up (reboot card or remove wifi usb dongle)\n");
LOGW(TAG, "\t* wifi device has no antenna\n");
if (param != NULL && param->callback != NULL) {
param->callback(env, param->obj, ARDRONE_MESSAGE_ERR_NO_WIFI);
}
}
else
{
LOGII("ardrone_tool_setup_com [OK]");
#define NB_IPHONE_PRE_STAGES 0
#define NB_IPHONE_POST_STAGES 2
//Alloc structs
specific_parameters_t * params = (specific_parameters_t *)vp_os_calloc(1, sizeof(specific_parameters_t));
specific_stages_t * iphone_pre_stages = (specific_stages_t*)vp_os_calloc(1, sizeof(specific_stages_t));
specific_stages_t * iphone_post_stages = (specific_stages_t*)vp_os_calloc(1, sizeof(specific_stages_t));
vp_api_picture_t * in_picture = (vp_api_picture_t*) vp_os_calloc(1, sizeof(vp_api_picture_t));
vp_api_picture_t * out_picture = (vp_api_picture_t*) vp_os_calloc(1, sizeof(vp_api_picture_t));
in_picture->width = STREAM_WIDTH;
in_picture->height = STREAM_HEIGHT;
out_picture->framerate = 20;
out_picture->format = PIX_FMT_RGB565;
out_picture->width = STREAM_WIDTH;
out_picture->height = STREAM_HEIGHT;
out_picture->y_buf = vp_os_malloc( STREAM_WIDTH * STREAM_HEIGHT * 2 );
out_picture->cr_buf = NULL;
out_picture->cb_buf = NULL;
out_picture->y_line_size = STREAM_WIDTH * 2;
out_picture->cb_line_size = 0;
out_picture->cr_line_size = 0;
//Define the list of stages size
iphone_pre_stages->length = NB_IPHONE_PRE_STAGES;
iphone_post_stages->length = NB_IPHONE_POST_STAGES;
//Alloc the lists
iphone_pre_stages->stages_list = NULL;
iphone_post_stages->stages_list = (vp_api_io_stage_t*)vp_os_calloc(iphone_post_stages->length,sizeof(vp_api_io_stage_t));
//Fill the POST-stages------------------------------------------------------
int postStageNumber = 0;
vp_os_memset (&vlat, 0x0, sizeof (vlat));
vlat.state = 0;
vlat.last_decoded_frame_info= (void *)&vec;
iphone_post_stages->stages_list[postStageNumber].type = VP_API_FILTER_DECODER;
iphone_post_stages->stages_list[postStageNumber].cfg = (void *)&vlat;
iphone_post_stages->stages_list[postStageNumber++].funcs = vp_stages_latency_estimation_funcs;
vp_os_memset (&ovsc, 0x0, sizeof (ovsc));
ovsc.video_decoder = &vec;
iphone_post_stages->stages_list[postStageNumber].type = VP_API_OUTPUT_LCD;
iphone_post_stages->stages_list[postStageNumber].cfg = (void *)&ovsc;
iphone_post_stages->stages_list[postStageNumber++].funcs = opengl_video_stage_funcs;
params->in_pic = in_picture;
params->out_pic = out_picture;
params->pre_processing_stages_list = iphone_pre_stages;
params->post_processing_stages_list = iphone_post_stages;
#if USE_THREAD_PRIORITIES
params->needSetPriority = 1;
params->priority = VIDEO_THREAD_PRIORITY;
#else
params->needSetPriority = 0;
params->priority = 0;
#endif
START_THREAD(video_stage, params);
if (IS_LEAST_ARDRONE2)
{
START_THREAD (video_recorder, (void *)&video_recorder_param);
LOGD(TAG, "Video recorder thread start [OK]");
}
res = ardrone_tool_init(&ardrone_info.drone_address[0], strlen(&ardrone_info.drone_address[0]), NULL, param->app_name, param->user_name, param->root_dir, param->flight_dir, param->flight_storing_size, param->academy_download_callback_func);
if(SUCCEED(res))
{
ardrone_tool_input_add(&virtual_gamepad);
if (param != NULL && param->callback != NULL) {
param->callback(env, param->obj, ARDRONE_MESSAGE_CONNECTED_OK);
}
} else {
if (param != NULL && param->callback != NULL) {
param->callback(env, param->obj, ARDRONE_MESSAGE_UNKNOWN_ERR);
}
bContinue = FALSE;
}
res = ardrone_tool_set_refresh_time(1000 / kAPS);
#if USE_THREAD_PRIORITIES
CHANGE_THREAD_PRIO (app_main, AT_THREAD_PRIORITY);
CHANGE_THREAD_PRIO (navdata_update, NAVDATA_THREAD_PRIORITY);
CHANGE_THREAD_PRIO (ardrone_control, NAVDATA_THREAD_PRIORITY);
#endif
while( SUCCEED(res) && bContinue == TRUE )
{
ardrone_tool_update();
}
JOIN_THREAD(video_stage);
if (IS_LEAST_ARDRONE2)
{
JOIN_THREAD (video_recorder);
}
/* Unregistering for the current device */
ardrone_tool_input_remove( &virtual_gamepad );
res = ardrone_tool_shutdown();
LOGD(TAG, "AR.Drone tool shutdown [OK]");
if (param != NULL && param->callback != NULL) {
param->callback(env, param->obj, ARDRONE_MESSAGE_DISCONNECTED);
}
}
vp_os_free (data);
data = NULL;
(*env)->DeleteGlobalRef(env, param->obj);
if (g_vm) {
(*g_vm)->DetachCurrentThread (g_vm);
}
LOGI(TAG, "app_main thread has been stopped.");
return (THREAD_RET) res;
}
