void publish_step(std::string step)
{
ROS_INFO("%s\n", step.c_str());
int end = -1;
int start = 0;
int i;
for(i = 0; i < NUM_BODY_PARTS; i++)
{
start = end + 1;
end = step.find(";", start);
std::string command = step.substr(start, end-start);
command_move(command, i);
}
}
void send_cmd(int http_fd, char *buffer)
{
char error[] = {"HTTP/1.0 200 OK\r\nServer: ardrone\r\nContent-Type: text/html\r\n\r\n"
"<HTML>\n<HEAD>\n<TITLE>libarpilot</TITLE>\n</HEAD>\n"
"<BODY><H1>Unknown command</H1><P>"
"<A href=command.html>back to commands</A><BR>"
"</BODY></HTML>"};
if (!strncmp(buffer,"cmd=",4)) {
buffer += 4;
send_xml_status(http_fd);
if (!strncmp(buffer,"fwd",3)) {
command_move(0,-500,0,0);
return;
}
if (!strncmp(buffer,"rev",3)) {
command_move(0,500,0,0);
return;
}
if (!strncmp(buffer,"left",4)) {
command_move(-500,0,0,0);
return;
}
if (!strncmp(buffer,"right",5)) {
command_move(500,0,0,0);
return;
}
if (!strncmp(buffer,"stop",4)) {
command_hover();
return;
}
if (!strncmp(buffer,"up",2)) {
command_move(0,0,300,0);
return;
}
if (!strncmp(buffer,"down",4)) {
command_move(0,0,-300,0);
return;
}
if (!strncmp(buffer,"rotl",4)) {
command_move(0,0,0,-300);
return;
}
if (!strncmp(buffer,"rotr",4)) {
command_move(0,0,0,300);
return;
}
if (!strncmp(buffer,"start",5)) {
command_state(1);
return;
}
if (!strncmp(buffer,"land",4)) {
command_state(0);
return;
}
if (!strncmp(buffer,"emer",4)) {
command_state(666);
return;
}
if (!strncmp(buffer,"rec",3)) {
if (!is_recording)
command_record(1);
else
command_record(0);
return;
}
if (!strncmp(buffer,"lock",4)) {
if (!lockout_control)
lockout_control=1;
else
lockout_control=0;
return;
}
return; /*Ignore unknown commands*/
}
// Error
send(http_fd,error,sizeof(error),MSG_NOSIGNAL);
}
INT32 sys_upgrade_process(void (*callback)(INT32 type, INT32 process, UINT16 *str), UINT32 (*GetExitKey)(void), int nType)
{
UINT32 result = SUCCESS;
INT32 i;
INT32 retry_count=0;
BOOL bReboot = TRUE;
BOOL bBurn = TRUE;
char strTmp[30];
char strTmp1[30];
char strTmp2[30];
char strTmp3[30];
char strTmp4[30];
char strTmp5[30];
char strTmp6[30];
char strTmp7[30];
char strTmp8[30];
char strTmp9[30];
char strTmp10[30];
char strTmp11[30];
char strTmp12[30];
char strTmp13[30];
char strTmp14[30];
if(nType == 2 || nType == 3)
bReboot = FALSE;
if(nType == 3)
bBurn = FALSE;
get_exit_key = GetExitKey;
callback_fun = callback;
LV_pPanDev = (struct pan_device *) dev_get_by_type(NULL, HLD_DEV_TYPE_PAN);
if(NULL == LV_pPanDev)
{
PRINTF("dev_get_by_name failed\n");
return !SUCCESS;
}
if(pan_open(LV_pPanDev)!=SUCCESS)
{
PRINTF("pan_open failed\n");
return !SUCCESS;
}
//pan_display(LV_pPanDev, "up9 ", 4);
#ifdef THREE_DIGITS_PANEL
pan_display(LV_pPanDev, " up9", 4);
#else
pan_display(LV_pPanDev, "up9 ", 4);
#endif
#ifdef ENABLE_EROM
init_buffer();
// uart_reset();
osal_task_sleep(100);
uart_high_speed_config(UART_SPEED_NORMAL);
do
{
if(!sync_slave((UPGRADE_MULTI==upgrade_mode)?M2S_MODE:P2P_MODE, 1000))
return !SUCCESS; ;
//uart_high_speed_config(UART_HIGH_SPEED_2M);
if(!init_slave()) // after init slave, uart 6M high bitrate is applied
return !SUCCESS;;
}while(0);
#endif
MG_Setup_CRC_Table();
/* Check sci port */
ComUniStrToAsc((UINT8 *)OSD_GetUnicodeString(RS_MSG_UPGRADE_CHECK_SERIAL_PORT),strTmp);
callback(3,0,strTmp);
result = cmd_comtest(&g_protocol_version, NULL, GetExitKey);
if(result != SUCCESS)
{
pan_display_error(result);
ComUniStrToAsc((UINT8 *)OSD_GetUnicodeString(RS_CONNECT_FAILED),strTmp1);
callback(2,0,strTmp1);
return !SUCCESS;
}
p2p_delay();
ComUniStrToAsc((UINT8 *)OSD_GetUnicodeString(RS_COLLECTING_VERSION_INFO),strTmp2);
callback(2,0,strTmp2);
if((result = cmd_version(&g_protocol_version)) != SUCCESS)
{
pan_display_error(result);
ComUniStrToAsc((UINT8 *)OSD_GetUnicodeString(RS_UPDATE_FAILED_TRY_AGAIN),strTmp3);
callback(2, 0, strTmp3);
return !SUCCESS;
}
p2p_delay();
ComUniStrToAsc((UINT8 *)OSD_GetUnicodeString(RS_COMPARE_SLAVE_REORG),strTmp4);
callback(2,0,strTmp4);
if((result =SlaveReorg(callback)) != SUCCESS)
{
ComUniStrToAsc((UINT8 *)OSD_GetUnicodeString(RS_UPDATE_FAILED_TRY_AGAIN),strTmp5);
callback(2,0,strTmp5);
return !SUCCESS;
}
p2p_delay();
UINT32 addr = 0x0;
if(pslave_reorg_list[0].type == 2) //transfer bootloader
{
addr = 0x0;
}
else
{
#if (SYS_PROJECT_FE == PROJECT_FE_DVBT && SYS_SDRAM_SIZE == 2)
addr = 0x7800;
#else
#if (SYS_CHIP_MODULE == ALI_M3327C && SYS_SDRAM_SIZE == 2)
//addr = 0xc000;
//add logo data, so the address is changed
addr = 0x8000;
#else
addr = pslave_list[0].offset;
#endif
#endif
}
if((result = cmd_address(addr)) != SUCCESS)
{
pan_display_error(result);
ComUniStrToAsc((UINT8 *)OSD_GetUnicodeString(RS_UPDATE_FAILED_TRY_AGAIN),strTmp6);
callback(2, 0, strTmp6);
return !SUCCESS;
}
trans_size = 0;
prog = 0;
ComUniStrToAsc((UINT8 *)OSD_GetUnicodeString(RS_TRANSFERING_DATA),strTmp7);
callback(3,0,strTmp7);
#if (SYS_CHIP_MODULE != ALI_M3327C || SYS_SDRAM_SIZE != 2)
pan_display(LV_pPanDev, "u0 ", 4);
#endif
for(i=0; i<slave_reorg_number;i++)
{
p2p_delay();
if(pslave_reorg_list[i].type==1) //move
{
if(i==0) continue;
result = command_move(pslave_reorg_list[i].index, pslave_reorg_list[i].offset, callback);
if(result != SUCCESS)
{
pan_display_error(result);
ComUniStrToAsc((UINT8 *)OSD_GetUnicodeString(RS_MOVE_FAILED_TRY_AGAIN),strTmp8);
callback(2,0,strTmp8);
return !SUCCESS;
}
}
else //transfer
{
result = command_transfer(pslave_reorg_list[i].index,callback);
if(result != SUCCESS)
{
pan_display_error(result);
ComUniStrToAsc((UINT8 *)OSD_GetUnicodeString(RS_TRANSFER_FAILED_TRY_AGAIN),strTmp9);
callback(2,0,strTmp9);
return !SUCCESS;
}
}
}
#if (SYS_CHIP_MODULE != ALI_M3327C || SYS_SDRAM_SIZE != 2)
pan_display(LV_pPanDev, "100 ", 4);
#endif
p2p_delay();
/* Send command to burn flash */
ComUniStrToAsc((UINT8 *)OSD_GetUnicodeString(RS_MSG_UPGRADE_BURN_FLASH),strTmp10);
callback(3,0,strTmp10);
pan_display(LV_pPanDev, "burn ", 4);
#if (SYS_CHIP_MODULE != ALI_M3327C || SYS_SDRAM_SIZE != 2)
pan_display(LV_pPanDev, "b0 ", 4);
#endif
if((result = command_burn_new(callback, bBurn)) != SUCCESS)
{
pan_display_error(result);
ComUniStrToAsc((UINT8 *)OSD_GetUnicodeString(RS_UPDATE_FAILED_TRY_AGAIN),strTmp11);
callback(2,0,strTmp11);
return !SUCCESS;
}
#if (SYS_CHIP_MODULE != ALI_M3327C || SYS_SDRAM_SIZE != 2)
pan_display(LV_pPanDev, "100 ", 4);
#endif
pan_display(LV_pPanDev, "end ", 4);
/*reboot slaver*/
if(bReboot)
{
ComUniStrToAsc((UINT8 *)OSD_GetUnicodeString(RS_REBOOTING_SLAVER),strTmp12);
callback(2,0,strTmp12);
if(command_reboot(callback) != SUCCESS)
{
ComUniStrToAsc((UINT8 *)OSD_GetUnicodeString(RS_UPDATE_FAILED_TRY_AGAIN),strTmp13);
callback(2,0,strTmp13);
return !SUCCESS;
}
}
ComUniStrToAsc((UINT8 *)OSD_GetUnicodeString(RS_UPDATE_SUCCESSFUL),strTmp14);
callback(2, 0, strTmp14);
return SUCCESS;
}