C_RESULT ardrone_control_soft_update_run( uint32_t ardrone_state, ardrone_control_soft_update_event_t* event )
{
  C_RESULT res = C_OK;

  switch( event->ack_received )
  {
  case 0: // First step - Reset command mask if needed
    if( ardrone_state & ARDRONE_COMMAND_MASK )
    {
      ardrone_at_update_control_mode(ACK_CONTROL_MODE, 0);
    }
    else
    {
      event->ack_received ++;
    }
    break;
  case 1:
    if( !(ardrone_state & ARDRONE_COMMAND_MASK) )
    {
        ardrone_at_update_control_mode(event->event, event->filesize);
    }
    else
    {
      event->ack_received ++;
    }
    break;

  case 2:
	if( (ardrone_state & ARDRONE_COMMAND_MASK) == 0)
	{
		if((ardrone_state & ARDRONE_FW_FILE_MASK) && (ardrone_state & ARDRONE_FW_VER_MASK))
		{
			event->status = ARDRONE_CONTROL_EVENT_FINISH_SUCCESS;
			event->error_state = FIRMWARE_NO_ERR;
		}
		else
	    {
	    	event->status = ARDRONE_CONTROL_EVENT_FINISH_FAILURE;
	    	if(ardrone_state & ARDRONE_FW_VER_MASK)
	    		event->error_state = FIRMWARE_FILE_ERR;
	    	else
	    		event->error_state = FIRMWARE_VERSION_ERR;
	    }
	}
    break;
  }

  return res;
}
Ejemplo n.º 2
0
/**
 * @brief Signal to the drone that the file has been received.
 *
 * @DESCRIPTION
 *
 *******************************************************************/
void ardrone_at_configuration_ack_ctrl_mode(void)
{
  if (!at_init)
     return;
 
  ardrone_at_update_control_mode( ACK_CONTROL_MODE );
}
Ejemplo n.º 3
0
void ardrone_at_custom_configuration_get_ctrl_mode(void)
{
  if (!at_init)
     return;

 ardrone_at_update_control_mode( CUSTOM_CFG_GET_CONTROL_MODE );
}
/**
 * \brief Runs a control event managing the configuration file.
 * This functions handles reading the configuration information
 * sent by the drone on the TCP 'control' socket on port 5559.
 * Called by the 'ardrone_control' thread loop in 'ardrone_control.c'.
 */
C_RESULT ardrone_control_configuration_run( uint32_t ardrone_state, ardrone_control_configuration_event_t* event )
{
	int32_t buffer_size;
	char *start_buffer, *buffer;
	char *value, *param, *c;
	bool_t ini_buffer_end, ini_buffer_more;
	C_RESULT res = C_OK;
	int bytes_to_read;

	/* Multiconfiguration support */
	static char *custom_configuration_list_buffer = NULL;
	static int  custom_configuration_list_buffer_size = 0;
	static int  custom_configuration_list_data_size = 0;
	#define CUSTOM_CFG_BLOCK_SIZE 1024

	int i;

	switch( event->config_state )
	{
		case CONFIG_REQUEST_INI:
			if( ardrone_state & ARDRONE_COMMAND_MASK )
			{
				/* If the ACK bit is set, we must ask the drone to clear it before asking the configuration. */
				DEBUG_CONFIG_RECEIVE("%s %s %i - Requesting ack. bit reset.\n",__FILE__,__FUNCTION__,__LINE__);
				ardrone_at_update_control_mode(ACK_CONTROL_MODE, 0);
			}
			else
			{
				DEBUG_CONFIG_RECEIVE("%s %s %i - Requesting the configuration.\n",__FILE__,__FUNCTION__,__LINE__);
				ini_buffer_index = 0;
				ardrone_at_configuration_get_ctrl_mode();
				set_state(event, CONFIG_RECEIVE_INI);
			}
			break;

		/* multi-configuration support */
		case CUSTOM_CONFIG_REQUEST:
			DEBUG_CONFIG_RECEIVE("%s %s %i\n",__FILE__,__FUNCTION__,__LINE__);
			if( ardrone_state & ARDRONE_COMMAND_MASK )
			{
				DEBUG_CONFIG_RECEIVE("%s %s %i - Requesting ack. bit reset.\n",__FILE__,__FUNCTION__,__LINE__);
				ardrone_at_update_control_mode(ACK_CONTROL_MODE, 0);
			}
			else
			{
				DEBUG_CONFIG_RECEIVE("%s %s %i - Requesting the custom config.\n",__FILE__,__FUNCTION__,__LINE__);
				custom_configuration_list_buffer = NULL;
				custom_configuration_list_buffer_size = 0;
				custom_configuration_list_data_size = 0;
				ardrone_at_custom_configuration_get_ctrl_mode();
				set_state(event, CUSTOM_CONFIG_RECEIVE);
			}
			break;

		case CONFIG_RECEIVE_INI:
			DEBUG_CONFIG_RECEIVE("%s %s %i - Trying to read the control socket.\n",__FILE__,__FUNCTION__,__LINE__);
			// Read data coming from ARDrone
			buffer_size = ARDRONE_CONTROL_CONFIGURATION_INI_BUFFER_SIZE - ini_buffer_index;
			res = ardrone_control_read( &ini_buffer[ini_buffer_index], &buffer_size );
			DEBUG_CONFIG_RECEIVE("Received <<%s>>\n",&ini_buffer[ini_buffer_index]);
			if(VP_SUCCEEDED(res))
			{      
				buffer_size += ini_buffer_index;

				ini_buffer[buffer_size]=0;  // Makes sure the buffer ends with a zero

				// Parse received data
				if( buffer_size > 0 )
				{
					//DEBUG_CONFIG_RECEIVE(" Searching value\n");

					ini_buffer_end  = ini_buffer[buffer_size-1] == '\0'; // End of configuration data
					ini_buffer_more = ini_buffer[buffer_size-1] != '\n'; // Need more configuration data to end parsing current line

					//if (ini_buffer_end) DEBUG_CONFIG_RECEIVE(" ini_buffer_end\n");
					//if (ini_buffer_more) DEBUG_CONFIG_RECEIVE(" ini_buffer_more\n");

					start_buffer = (char*)&ini_buffer[0];
					buffer = strchr(start_buffer, '\n');

					//DEBUG_CONFIG_RECEIVE("Le start buffer : <<%s>>\n",start_buffer);

					while( buffer != NULL )
					{
						//DEBUG_CONFIG_RECEIVE(" Found an '\\n' \n");

						value = start_buffer;
						param = strchr(value, '=');

						*buffer = '\0';
						*param  = '\0';

						// Remove spaces at end of strings
						c = param - 1;
						while( *c == ' ' )
						{
							*c = '\0';
							c  = c-1;
						}

						c = buffer-1;
						while( *c == ' ' )
						{
							*c = '\0';
							c  = c-1;
						}

						// Remove spaces at beginning of strings
						while( *value == ' ' )
						{
							value = value + 1;
						}

						param = param + 1;
						while( *param == ' ' )
						{
							param = param + 1;
						}

						DEBUG_CONFIG_RECEIVE(" Decoding from control stream : <%s>=<%s>\n",value,param);
						iniparser_setstring( event->ini_dict, value, param );

						start_buffer = buffer + 1;
						buffer = strchr(start_buffer, '\n');
					}

					if( ini_buffer_end )
					{
						/* Reading the condfiguration is finished, we ask the drone
						 * to clear the ACK bit */
						ardrone_at_update_control_mode(ACK_CONTROL_MODE, 0);
						set_state(event, CONFIG_RECEIVED);
					}
					else if( ini_buffer_more )
					{
						// Compute number of bytes to copy
						int32_t size = (int32_t)&ini_buffer[buffer_size] - (int32_t)start_buffer;
						vp_os_memcpy( &ini_buffer[0], start_buffer, size );
						ini_buffer_index = size;
					}
					else
					{
						/* The previous line was completely consumed -  next data
						 * from the network will be stored at the beginning of the
						 * buffer. */
						ini_buffer_index = 0;
					}
				}
			}
         else
         {
        	 res = C_FAIL;

        	 DEBUG_CONFIG_RECEIVE("%s %s %i - no data to read from the control socket.\n",__FILE__,__FUNCTION__,__LINE__);

            if(!(ardrone_state & ARDRONE_COMMAND_MASK))
				   set_state(event, CONFIG_REQUEST_INI);
         }
		break;

		case CUSTOM_CONFIG_RECEIVE:
			DEBUG_CONFIG_RECEIVE("%s %s %i - Trying to read the control socket.\n",__FILE__,__FUNCTION__,__LINE__);
			DEBUG_CONFIG_RECEIVE("%s %s %i\n",__FILE__,__FUNCTION__,__LINE__);
			/* Read data until a zero byte is received */

			/* Clear old data from the buffer when receiving the first bytes */
				if (custom_configuration_list_buffer!=NULL && custom_configuration_list_data_size==0){
					vp_os_memset(custom_configuration_list_buffer,0,custom_configuration_list_buffer_size);
				}

			/* Enlarge the buffer if needed */
				if (custom_configuration_list_data_size==custom_configuration_list_buffer_size)
				{
					custom_configuration_list_buffer_size += CUSTOM_CFG_BLOCK_SIZE;
					custom_configuration_list_buffer = vp_os_realloc(custom_configuration_list_buffer,custom_configuration_list_buffer_size);
				}
			/* Read data at the end of the buffer */
				bytes_to_read = custom_configuration_list_buffer_size - custom_configuration_list_data_size;
				res = ardrone_control_read( &custom_configuration_list_buffer[custom_configuration_list_data_size], &bytes_to_read );
				DEBUG_CONFIG_RECEIVE(" Reading %i bytes of the custom config. list\n",bytes_to_read);
				for (i=0;i<bytes_to_read;i++) { DEBUG_CONFIG_RECEIVE("<%i>",custom_configuration_list_buffer[i]); }

			/* Searches a zero */
				if (VP_SUCCEEDED(res))
				{
					custom_configuration_list_data_size += bytes_to_read;
					for (i=bytes_to_read;i>0;i--){
						if (custom_configuration_list_buffer[custom_configuration_list_data_size - i] == 0) {

							/* Reading the condfiguration is finished, we ask the drone
							 * to clear the ACK bit */
							DEBUG_CONFIG_RECEIVE("Finished receiving\n");
							ardrone_at_update_control_mode(ACK_CONTROL_MODE, 0);
							set_state(event, CUSTOM_CONFIG_RECEIVED);

							ardrone_control_reset_custom_configurations_list(available_configurations);

							ardrone_control_read_custom_configurations_list(custom_configuration_list_buffer,
																			custom_configuration_list_data_size,
																			available_configurations);

							/* Clean up */
								vp_os_sfree((void *)&custom_configuration_list_buffer);
								custom_configuration_list_buffer_size = 0;
								custom_configuration_list_data_size = 0;
							res = C_OK;
							break;
						}
					}

				}
				else{
					/* No data are available on the control socket. */
					DEBUG_CONFIG_RECEIVE("%s %s %i - no data to read from the control socket.\n",__FILE__,__FUNCTION__,__LINE__);
					/* Reset the buffer */
					custom_configuration_list_data_size = 0;
					/* The request for the configuration file should have been acknowledged by the drone.
					 * If not, another request is sent.	*/
					if(!(ardrone_state & ARDRONE_COMMAND_MASK))   set_state(event, CUSTOM_CONFIG_REQUEST);
				}

				break;


		case CONFIG_RECEIVED:
		case CUSTOM_CONFIG_RECEIVED:
				/* We finished reading the configuration, we wait for the drone to reset the ACK bit */
				if( ardrone_state & ARDRONE_COMMAND_MASK )
				{
					/* If the ACK bit is set, we must ask the drone to clear it before asking the configuration. */
					PRINT("%s %s %i - Requesting ack. bit reset.\n",__FILE__,__FUNCTION__,__LINE__);
					ardrone_at_update_control_mode(ACK_CONTROL_MODE, 0);
				}
				else
				{
					PRINT("%s %s %i - Finished.\n",__FILE__,__FUNCTION__,__LINE__);
					event->status = ARDRONE_CONTROL_EVENT_FINISH_SUCCESS;
				}
				res = C_OK;
				break;


		default:
         res = C_FAIL;
			break;
	}

	return res;
}