Mani Monajjemi
152 linhas
4.1 KiB
C
152 linhas
4.1 KiB
C
|
|
#include <VP_Os/vp_os_types.h>
|
|
#include <VP_Os/vp_os_malloc.h>
|
|
#include <VP_Os/vp_os_print.h>
|
|
|
|
#include <ardrone_api.h>
|
|
|
|
/* Uncomment to activate interesting printf's */
|
|
//#define DEBUG_NAVDATA_C
|
|
|
|
|
|
/********************************************************************
|
|
* @fn ardrone_navdata_compute_cks:
|
|
* @param nv Data to calculate the checksum.
|
|
* @param size Size of data calculate as follow : size-sizeof(navdata_cks_t).
|
|
* @return Retrieves the checksum from the navdata nv.
|
|
*******************************************************************/
|
|
uint32_t ardrone_navdata_compute_cks( uint8_t* nv, int32_t size )
|
|
{
|
|
int32_t i;
|
|
uint32_t cks;
|
|
uint32_t temp;
|
|
|
|
cks = 0;
|
|
|
|
for( i = 0; i < size; i++ )
|
|
{
|
|
temp = nv[i];
|
|
cks += temp;
|
|
}
|
|
|
|
return cks;
|
|
}
|
|
|
|
|
|
|
|
/********************************************************************
|
|
* @fn ardrone_navdata_search_option:
|
|
* @param navdata_options_ptr
|
|
* @param tag ID of the bloc to search for.
|
|
* @brief Jumps to a specified 'option' (block of navdata) inside
|
|
* a navdata packed buffer.
|
|
*******************************************************************/
|
|
navdata_option_t* ardrone_navdata_search_option( navdata_option_t* navdata_options_ptr, navdata_tag_t tag )
|
|
{
|
|
uint8_t* ptr;
|
|
|
|
while( navdata_options_ptr->tag != tag )
|
|
{
|
|
ptr = (uint8_t*) navdata_options_ptr;
|
|
ptr += navdata_options_ptr->size;
|
|
if (0 == navdata_options_ptr->size)
|
|
{
|
|
navdata_options_ptr = NULL;
|
|
break;
|
|
}
|
|
|
|
navdata_options_ptr = (navdata_option_t*) ptr;
|
|
}
|
|
|
|
return navdata_options_ptr;
|
|
}
|
|
|
|
|
|
/********************************************************************
|
|
* 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->nd_seq = navdata->sequence;
|
|
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;
|
|
}
|