C_RESULT ardrone_navdata_unpack_all(navdata_unpacked_t* navdata_unpacked, navdata_t* navdata, uint32_t* cks) { C_RESULT res; navdata_cks_t navdata_cks = { 0 }; navdata_option_t* navdata_option_ptr; navdata_option_ptr = (navdata_option_t*) &navdata->options[0]; vp_os_memset( navdata_unpacked, 0, sizeof(*navdata_unpacked) ); navdata_unpacked->ardrone_state = navdata->ardrone_state; navdata_unpacked->vision_defined = navdata->vision_defined; res = C_OK; while( navdata_option_ptr != NULL ) { // Check if we have a valid option if( navdata_option_ptr->size == 0 ) { PRINT("One option is not a valid because its size is zero\n"); navdata_option_ptr = NULL; res = C_FAIL; } else { switch( navdata_option_ptr->tag ) { case NAVDATA_DEMO_TAG: navdata_option_ptr = ardrone_navdata_unpack( navdata_option_ptr, navdata_unpacked->navdata_demo ); break; case NAVDATA_TIME_TAG: navdata_option_ptr = ardrone_navdata_unpack( navdata_option_ptr, navdata_unpacked->navdata_time ); break; case NAVDATA_RAW_MEASURES_TAG: navdata_option_ptr = ardrone_navdata_unpack( navdata_option_ptr, navdata_unpacked->navdata_raw_measures ); break; case NAVDATA_PHYS_MEASURES_TAG: navdata_option_ptr = ardrone_navdata_unpack( navdata_option_ptr, navdata_unpacked->navdata_phys_measures ); break; case NAVDATA_GYROS_OFFSETS_TAG: navdata_option_ptr = ardrone_navdata_unpack( navdata_option_ptr, navdata_unpacked->navdata_gyros_offsets ); break; case NAVDATA_EULER_ANGLES_TAG: navdata_option_ptr = ardrone_navdata_unpack( navdata_option_ptr, navdata_unpacked->navdata_euler_angles ); break; case NAVDATA_REFERENCES_TAG: navdata_option_ptr = ardrone_navdata_unpack( navdata_option_ptr, navdata_unpacked->navdata_references ); break; case NAVDATA_TRIMS_TAG: navdata_option_ptr = ardrone_navdata_unpack( navdata_option_ptr, navdata_unpacked->navdata_trims ); break; case NAVDATA_RC_REFERENCES_TAG: navdata_option_ptr = ardrone_navdata_unpack( navdata_option_ptr, navdata_unpacked->navdata_rc_references ); break; case NAVDATA_PWM_TAG: navdata_option_ptr = ardrone_navdata_unpack( navdata_option_ptr, navdata_unpacked->navdata_pwm ); break; case NAVDATA_ALTITUDE_TAG: navdata_option_ptr = ardrone_navdata_unpack( navdata_option_ptr, navdata_unpacked->navdata_altitude ); break; case NAVDATA_VISION_RAW_TAG: navdata_option_ptr = ardrone_navdata_unpack( navdata_option_ptr, navdata_unpacked->navdata_vision_raw ); break; case NAVDATA_VISION_OF_TAG: navdata_option_ptr = ardrone_navdata_unpack( navdata_option_ptr, navdata_unpacked->navdata_vision_of ); break; case NAVDATA_VISION_TAG: navdata_option_ptr = ardrone_navdata_unpack( navdata_option_ptr, navdata_unpacked->navdata_vision ); break; case NAVDATA_VISION_PERF_TAG: navdata_option_ptr = ardrone_navdata_unpack( navdata_option_ptr, navdata_unpacked->navdata_vision_perf ); break; case NAVDATA_TRACKERS_SEND_TAG: navdata_option_ptr = ardrone_navdata_unpack( navdata_option_ptr, navdata_unpacked->navdata_trackers_send ); break; case NAVDATA_VISION_DETECT_TAG: navdata_option_ptr = ardrone_navdata_unpack( navdata_option_ptr, navdata_unpacked->navdata_vision_detect ); break; case NAVDATA_WATCHDOG_TAG: navdata_option_ptr = ardrone_navdata_unpack( navdata_option_ptr, navdata_unpacked->navdata_watchdog ); break; case NAVDATA_ADC_DATA_FRAME_TAG: navdata_option_ptr = ardrone_navdata_unpack( navdata_option_ptr, navdata_unpacked->navdata_adc_data_frame ); break; case NAVDATA_CKS_TAG: navdata_option_ptr = ardrone_navdata_unpack( navdata_option_ptr, navdata_cks ); *cks = navdata_cks.cks; navdata_option_ptr = NULL; // End of structure break; default: PRINT("Tag %d is not a valid navdata option tag\n", (int) navdata_option_ptr->tag); navdata_option_ptr = NULL; res = C_FAIL; break; } } } return res; }
/******************************************************************** * ardrone_navdata_unpack_all: * @param navdata_unpacked navdata_unpacked in which to store the navdata. * @param navdata One packet read from the port NAVDATA. * @param Checksum of navdata * @brief Disassembles a buffer of received navdata, and dispatches * it inside 'navdata_unpacked' structure. * @DESCRIPTION * *******************************************************************/ C_RESULT ardrone_navdata_unpack_all(navdata_unpacked_t* navdata_unpacked, navdata_t* navdata, uint32_t* cks) { C_RESULT res; navdata_cks_t navdata_cks = { 0 }; navdata_option_t* navdata_option_ptr; navdata_option_ptr = (navdata_option_t*) &navdata->options[0]; vp_os_memset( navdata_unpacked, 0, sizeof(*navdata_unpacked) ); navdata_unpacked->ardrone_state = navdata->ardrone_state; navdata_unpacked->vision_defined = navdata->vision_defined; res = C_OK; #ifdef DEBUG_NAVDATA_C if (navdata_unpacked->ardrone_state & ARDRONE_COMMAND_MASK) { printf("[ACK]"); } printf("Received navdatas tags :"); #endif while( navdata_option_ptr != NULL ) { // Check if we have a valid option if( navdata_option_ptr->size == 0 ) { PRINT("One option is not a valid because its size is zero\n"); navdata_option_ptr = NULL; res = C_FAIL; } else { if( navdata_option_ptr->tag <= NAVDATA_NUM_TAGS){ #ifdef DEBUG_NAVDATA_C printf("[%d]",navdata_option_ptr->tag); #endif navdata_unpacked->last_navdata_refresh |= NAVDATA_OPTION_MASK(navdata_option_ptr->tag); } switch( navdata_option_ptr->tag ) { #define NAVDATA_OPTION(STRUCTURE,NAME,TAG) \ case TAG: \ navdata_option_ptr = ardrone_navdata_unpack( navdata_option_ptr, navdata_unpacked->NAME); \ break; #define NAVDATA_OPTION_DEMO(STRUCTURE,NAME,TAG) NAVDATA_OPTION(STRUCTURE,NAME,TAG) #define NAVDATA_OPTION_CKS(STRUCTURE,NAME,TAG) {} #include <navdata_keys.h> #ifdef POUBELLE case NAVDATA_DEMO_TAG: navdata_option_ptr = ardrone_navdata_unpack( navdata_option_ptr, navdata_unpacked->navdata_demo ); break; case NAVDATA_TIME_TAG: navdata_option_ptr = ardrone_navdata_unpack( navdata_option_ptr, navdata_unpacked->navdata_time ); break; case NAVDATA_RAW_MEASURES_TAG: navdata_option_ptr = ardrone_navdata_unpack( navdata_option_ptr, navdata_unpacked->navdata_raw_measures ); break; case NAVDATA_PHYS_MEASURES_TAG: navdata_option_ptr = ardrone_navdata_unpack( navdata_option_ptr, navdata_unpacked->navdata_phys_measures ); break; case NAVDATA_GYROS_OFFSETS_TAG: navdata_option_ptr = ardrone_navdata_unpack( navdata_option_ptr, navdata_unpacked->navdata_gyros_offsets ); break; case NAVDATA_EULER_ANGLES_TAG: navdata_option_ptr = ardrone_navdata_unpack( navdata_option_ptr, navdata_unpacked->navdata_euler_angles ); break; case NAVDATA_REFERENCES_TAG: navdata_option_ptr = ardrone_navdata_unpack( navdata_option_ptr, navdata_unpacked->navdata_references ); break; case NAVDATA_TRIMS_TAG: navdata_option_ptr = ardrone_navdata_unpack( navdata_option_ptr, navdata_unpacked->navdata_trims ); break; case NAVDATA_RC_REFERENCES_TAG: navdata_option_ptr = ardrone_navdata_unpack( navdata_option_ptr, navdata_unpacked->navdata_rc_references ); break; case NAVDATA_PWM_TAG: navdata_option_ptr = ardrone_navdata_unpack( navdata_option_ptr, navdata_unpacked->navdata_pwm ); break; case NAVDATA_ALTITUDE_TAG: navdata_option_ptr = ardrone_navdata_unpack( navdata_option_ptr, navdata_unpacked->navdata_altitude ); break; case NAVDATA_VISION_RAW_TAG: navdata_option_ptr = ardrone_navdata_unpack( navdata_option_ptr, navdata_unpacked->navdata_vision_raw ); break; case NAVDATA_VISION_OF_TAG: navdata_option_ptr = ardrone_navdata_unpack( navdata_option_ptr, navdata_unpacked->navdata_vision_of ); break; case NAVDATA_VISION_TAG: navdata_option_ptr = ardrone_navdata_unpack( navdata_option_ptr, navdata_unpacked->navdata_vision ); break; case NAVDATA_VISION_PERF_TAG: navdata_option_ptr = ardrone_navdata_unpack( navdata_option_ptr, navdata_unpacked->navdata_vision_perf ); break; case NAVDATA_TRACKERS_SEND_TAG: navdata_option_ptr = ardrone_navdata_unpack( navdata_option_ptr, navdata_unpacked->navdata_trackers_send ); break; case NAVDATA_VISION_DETECT_TAG: navdata_option_ptr = ardrone_navdata_unpack( navdata_option_ptr, navdata_unpacked->navdata_vision_detect ); break; case NAVDATA_WATCHDOG_TAG: navdata_option_ptr = ardrone_navdata_unpack( navdata_option_ptr, navdata_unpacked->navdata_watchdog ); break; case NAVDATA_ADC_DATA_FRAME_TAG: navdata_option_ptr = ardrone_navdata_unpack( navdata_option_ptr, navdata_unpacked->navdata_adc_data_frame ); break; case NAVDATA_VIDEO_STREAM_TAG: navdata_option_ptr = ardrone_navdata_unpack( navdata_option_ptr, navdata_unpacked->navdata_video_stream ); break; #endif case NAVDATA_CKS_TAG: navdata_option_ptr = ardrone_navdata_unpack( navdata_option_ptr, navdata_cks ); *cks = navdata_cks.cks; navdata_option_ptr = NULL; // End of structure break; default: PRINT("Tag %d is not a valid navdata option tag\n", (int) navdata_option_ptr->tag); navdata_option_ptr = NULL; res = C_FAIL; break; } } } #ifdef DEBUG_NAVDATA_C printf("\n"); #endif return res; }
/******************************************************************** * ardrone_navdata_unpack_all: * @param navdata_unpacked navdata_unpacked in which to store the navdata. * @param navdata One packet read from the port NAVDATA. * @param Checksum of navdata * @brief Disassembles a buffer of received navdata, and dispatches * it inside 'navdata_unpacked' structure. * @DESCRIPTION * *******************************************************************/ C_RESULT ardrone_navdata_unpack_all(navdata_unpacked_t* navdata_unpacked, navdata_t* navdata, uint32_t* cks) { C_RESULT res; navdata_cks_t navdata_cks = { 0 }; navdata_option_t* navdata_option_ptr; navdata_option_ptr = (navdata_option_t*) &navdata->options[0]; vp_os_memset( navdata_unpacked, 0, sizeof(*navdata_unpacked) ); navdata_unpacked->ardrone_state = navdata->ardrone_state; navdata_unpacked->vision_defined = navdata->vision_defined; res = C_OK; #ifdef DEBUG_NAVDATA_C if (navdata_unpacked->ardrone_state & ARDRONE_COMMAND_MASK) { printf("[ACK]"); } printf("Received navdatas tags :"); #endif while( navdata_option_ptr != NULL ) { // Check if we have a valid option if( navdata_option_ptr->size == 0 ) { PRINT("One option (%d) is not a valid option because its size is zero\n", navdata_option_ptr->tag); navdata_option_ptr = NULL; res = C_FAIL; } else { if( navdata_option_ptr->tag <= NAVDATA_NUM_TAGS){ #ifdef DEBUG_NAVDATA_C printf("[%d]",navdata_option_ptr->tag); #endif navdata_unpacked->last_navdata_refresh |= NAVDATA_OPTION_MASK(navdata_option_ptr->tag); } switch( navdata_option_ptr->tag ) { #define NAVDATA_OPTION(STRUCTURE,NAME,TAG) \ case TAG: \ navdata_option_ptr = ardrone_navdata_unpack( navdata_option_ptr, navdata_unpacked->NAME); \ break; #define NAVDATA_OPTION_DEMO(STRUCTURE,NAME,TAG) NAVDATA_OPTION(STRUCTURE,NAME,TAG) #define NAVDATA_OPTION_CKS(STRUCTURE,NAME,TAG) {} #include <navdata_keys.h> case NAVDATA_CKS_TAG: navdata_option_ptr = ardrone_navdata_unpack( navdata_option_ptr, navdata_cks ); *cks = navdata_cks.cks; navdata_option_ptr = NULL; // End of structure break; default: PRINT("Tag %d is an unknown navdata option tag\n", (int) navdata_option_ptr->tag); navdata_option_ptr = (navdata_option_t *)(((uint32_t)navdata_option_ptr) + navdata_option_ptr->size); break; } } } #ifdef DEBUG_NAVDATA_C printf("\n"); #endif return res; }