Beispiel #1
0
// --------------------------------------------------------------------------
// ARDrone::getConfig()
// Description  : Get current configurations of AR.Drone.
// Return value : SUCCESS: 1  FAILURE: 0
// --------------------------------------------------------------------------
int ARDrone::getConfig(void)
{
    // Open the IP address and port
    TCPSocket sockConfig;
    if (!sockConfig.open(ip, ARDRONE_CONFIG_PORT)) {
        CVDRONE_ERROR("TCPSocket::open(port=%d) failed. (%s, %d)\n", ARDRONE_CONFIG_PORT, __FILE__, __LINE__);
        return 0;
    }

    // Send requests
    UDPSocket tmpCommand;
    tmpCommand.open(ip, ARDRONE_COMMAND_PORT);
    tmpCommand.sendf("AT*CTRL=%d,5,0\r", seq++);
    tmpCommand.sendf("AT*CTRL=%d,4,0\r", seq++);
    msleep(500);
    tmpCommand.close();

    // Receive data
    char buf[4096] = {'\0'};
    int size = sockConfig.receive((void*)&buf, sizeof(buf));

    // Received something
    if (size > 0) {
        // Clear config struct
        memset(&config, 0, sizeof(ARDRONE_CONFIG));

        // Parsing configurations
        char *token = strtok(buf, "\n");
        if (token != NULL) parse(token, &config);
        while (token != NULL) {
            token = strtok(NULL, "\n");
            if (token != NULL) parse(token, &config);
        }
    }

    #if 0
    // For debug
    printf("general.num_version_config = %d\n", config.general.num_version_config);
    printf("general.num_version_mb = %d\n", config.general.num_version_mb);
    printf("general.num_version_soft = %s\n", config.general.num_version_soft);
    printf("general.drone_serial = %s\n", config.general.drone_serial);
    printf("general:soft_build_date = %s\n", config.general.soft_build_date);
    printf("general:motor1_soft = %f\n", config.general.motor1_soft);
    printf("general:motor1_hard = %f\n", config.general.motor1_hard);
    printf("general:motor1_supplier = %f\n", config.general.motor1_supplier);
    printf("general:motor2_soft = %f\n", config.general.motor2_soft);
    printf("general:motor2_hard = %f\n", config.general.motor2_hard);
    printf("general:motor2_supplier = %f\n", config.general.motor2_supplier);
    printf("general:motor3_soft = %f\n", config.general.motor3_soft);
    printf("general:motor3_hard = %f\n", config.general.motor3_hard);
    printf("general:motor3_supplier = %f\n", config.general.motor3_supplier);
    printf("general:motor4_soft = %f\n", config.general.motor4_soft);
    printf("general:motor4_hard = %f\n", config.general.motor4_hard);
    printf("general:motor4_supplier = %f\n", config.general.motor4_supplier);
    printf("general.ardrone_name = %s\n", config.general.ardrone_name);
    printf("general.flying_time = %d\n", config.general.flying_time);
    printf("general.navdata_demo = %s\n", config.general.navdata_demo ? "true" : "false");
    printf("general.com_watchdog = %d\n", config.general.com_watchdog);
    printf("general.video_enable = %s\n", config.general.video_enable ? "true" : "false");
    printf("general.vision_enable = %s\n", config.general.vision_enable ? "true" : "false");
    printf("general.vbat_min = %d\n", config.general.vbat_min);
    printf("general.localtime = %d\n", config.general.localtime);
    printf("general.navdata_options = %d\n", config.general.navdata_options);
    printf("control:accs_offset = {%f, %f, %f}\n", config.control.accs_offset[0], config.control.accs_offset[1], config.control.accs_offset[2]);
    printf("control:accs_gains = { %f %f %f %f %f %f %f %f %f }\n", config.control.accs_gains[0], config.control.accs_gains[1], config.control.accs_gains[2], config.control.accs_gains[3], config.control.accs_gains[4], config.control.accs_gains[5], config.control.accs_gains[6], config.control.accs_gains[7], config.control.accs_gains[8]);
    printf("control:gyros_offset = { %f %f %f }\n", config.control.gyros_offset[0], config.control.gyros_offset[1], config.control.gyros_offset[2]);
    printf("control:gyros_gains = { %f %f %f }\n", config.control.gyros_gains[0], config.control.gyros_gains[1], config.control.gyros_gains[2]);
    printf("control:gyros110_offset = { %f %f }\n", config.control.gyros110_offset[0], config.control.gyros110_offset[1]);
    printf("control:gyros110_gains = { %f %f }\n", config.control.gyros110_gains[0], config.control.gyros110_gains[1]);
    printf("control:magneto_offset = { %f %f %f }\n", config.control.magneto_offset[0], config.control.magneto_offset[1], config.control.magneto_offset[2]);
    printf("control:magneto_radius = %f\n", config.control.magneto_radius);
    printf("control:gyro_offset_thr_x = %f\n", config.control.gyro_offset_thr_x);
    printf("control:gyro_offset_thr_y = %f\n", config.control.gyro_offset_thr_y);
    printf("control:gyro_offset_thr_z = %f\n", config.control.gyro_offset_thr_z);
    printf("control:pwm_ref_gyros = %d\n", config.control.pwm_ref_gyros);
    printf("control:osctun_value = %d\n", config.control.osctun_value);
    printf("control:osctun_test = %s\n", config.control.osctun_test ? "true" : "false");
    printf("control:altitude_max = %d\n", config.control.altitude_max);
    printf("control:altitude_min = %d\n", config.control.altitude_min);
    printf("control:outdoor = %s\n", config.control.outdoor ? "true" : "false");
    printf("control:flight_without_shell = %s\n", config.control.flight_without_shell ? "true" : "false");
    printf("control:autonomous_flight = %s\n", config.control.autonomous_flight ? "true" : "false");
    printf("control:flight_anim = %d,%d\n", config.control.flight_anim[0], config.control.flight_anim[1]);
    printf("control:control_level = %d\n", config.control.control_level);
    printf("control:euler_angle_max = %f\n", config.control.euler_angle_max);
    printf("control:control_iphone_tilt = %f\n", config.control.control_iphone_tilt);
    printf("control:control_vz_max = %f\n", config.control.control_vz_max);
    printf("control:control_yaw = %f\n", config.control.control_yaw);
    printf("control:manual_trim = %s\n", config.control.manual_trim ? "true" : "false");
    printf("control:indoor_euler_angle_max = %f\n", config.control.indoor_euler_angle_max);
    printf("control:indoor_control_vz_max = %f\n", config.control.indoor_control_vz_max);
    printf("control:indoor_control_yaw = %f\n", config.control.indoor_control_yaw);
    printf("control:outdoor_euler_angle_max = %f\n", config.control.outdoor_euler_angle_max);
    printf("control:outdoor_control_vz_max = %f\n", config.control.outdoor_control_vz_max);
    printf("control:outdoor_control_yaw = %f\n", config.control.outdoor_control_yaw);
    printf("control:flying_mode = %d\n", config.control.flying_mode);
    printf("control:hovering_range = %d\n", config.control.hovering_range);
    printf("network:ssid_single_player = %s\n", config.network.ssid_single_player);
    printf("network:ssid_multi_player = %s\n", config.network.ssid_multi_player);
    printf("network:wifi_mode = %d\n", config.network.wifi_mode);
    printf("network:wifi_rate = %d\n", config.network.wifi_rate);
    printf("network:owner_mac = %s\n", config.network.owner_mac);
    printf("pic:ultrasound_freq = %d\n", config.pic.ultrasound_freq);
    printf("pic:ultrasound_watchdog = %d\n", config.pic.ultrasound_watchdog);
    printf("pic:pic_version = %d\n", config.pic.pic_version);
    printf("video:camif_fps = %d\n", config.video.camif_fps);
    printf("video:camif_buffers = %d\n", config.video.camif_buffers);
    printf("video:num_trackers = %d\n", config.video.num_trackers);
    printf("video:video_storage_space = %d\n", config.video.video_storage_space);
    printf("video:video_on_usb = %s\n", config.video.video_on_usb ? "true" : "false");
    printf("video:video_file_index = %d\n", config.video.video_file_index);
    printf("video:bitrate = %d\n", config.video.bitrate);
    printf("video:bitrate_ctrl_mode = %d\n", config.video.bitrate_ctrl_mode);
    printf("video:bitrate_storage = %d\n", config.video.bitrate_storage);
    printf("video:codec_fps = %d\n", config.video.codec_fps);
    printf("video:video_codec = %d\n", config.video.video_codec);
    printf("video:video_slices = %d\n", config.video.video_slices);
    printf("video:video_live_socket = %d\n", config.video.video_live_socket);
    printf("video:max_bitrate = %d\n", config.video.max_bitrate);
    printf("video:video_channel = %d\n", config.video.video_channel);
    printf("leds:leds_anim = %d,%d,%d\n", config.leds.leds_anim[0], config.leds.leds_anim[1], config.leds.leds_anim[2]);
    printf("detect:enemy_colors = %d\n", config.detect.enemy_colors);
    printf("detect:enemy_without_shell = %d\n", config.detect.enemy_without_shell);
    printf("detect:groundstripe_colors = %d\n", config.detect.groundstripe_colors);
    printf("detect:detect_type = %d\n", config.detect.detect_type);
    printf("detect:detections_select_h = %d\n", config.detect.detections_select_h);
    printf("detect:detections_select_v_hsync = %d\n", config.detect.detections_select_v_hsync);
    printf("detect:detections_select_v = %d\n", config.detect.detections_select_v);
    printf("syslog:output = %d\n", config.syslog.output);
    printf("syslog:max_size = %d\n", config.syslog.max_size);
    printf("syslog:nb_files = %d\n", config.syslog.nb_files);
    printf("custom:application_desc = %s\n", config.custom.application_desc);
    printf("custom:profile_desc = %s\n", config.custom.profile_desc);
    printf("custom:session_desc = %s\n", config.custom.session_desc);
    printf("custom:application_id = %s\n", config.custom.application_id);
    printf("custom:profile_id = %s\n", config.custom.profile_id);
    printf("custom:session_id = %s\n", config.custom.session_id);
    printf("userbox:userbox_cmd = %d\n", config.userbox.userbox_cmd);
    printf("gps:latitude = %f\n", config.gps.latitude);
    printf("gps:longitude = %f\n", config.gps.longitude);
    printf("gps:altitude = %f\n", config.gps.altitude);
    #endif

    // Finalize
    sockConfig.close();

    return 1;
}