// 复位操作耗时1s
int VCReset(Command_t * command)
{
int ntry = 3;
int32_t ret;
// reset on release version 4.1010
command->data[0] = IV_I2C_CMD_GET_VERSION;
ret = send_cmd(command, APP_ID_CAPT | capt_nr, (IV_I2C_CMD_CTRL | 0x0100), 1);
printk("VCReset:ret=%d, %d, %d\n", ret, command->data[0], command->data[1]);
if(ret < 0) return 0;
// if (command->data[0] != 4 && command->data[1] != 0x10100) return 0;
if (!(command->data[0] == 4 && command->data[1] == 0x10100)) return 0;
while (ntry-- > 0) {
//check watchdog
ret = send_cmd(command, APP_ID_CAPT|capt_nr, CMD_GET(IVNR_CMD_WATCHDOG), 0);
printk("get watchdog:%d %d\n", ret, command->data[0]);
if( ret < 0 ) continue;
if (command->data[0] == 0) {
// start watchdog
command->data[0] = 1;
ret = send_cmd(command, APP_ID_CAPT|capt_nr, CMD_SET(IVNR_CMD_WATCHDOG), 1);
printk("set watchdog:%d %d\n", ret, command->data[0]);
if( ret < 0 ) continue;
}
// close app
command->data[0] = APP_ID_CAPT;
command->data[1] = 0;
// ret = send_cmd(command, APP_ID_STRM, STREAMER_APP_MODULE_APP_ENABLE, 2);
if (ret < 0) continue;
mdelay(1000);// 复位后要等1s才能操作净音模块,这里用了阻塞式等待
//check watchdog
ret = send_cmd(command, APP_ID_CAPT|capt_nr, CMD_GET(IVNR_CMD_WATCHDOG), 0);
printk("get watchdog:%d %d\n", ret, command->data[0]);
if( ret < 0 ) continue;
if (command->data[0] == 1) break;
}
return 0;
}
int VCGetFunc(Command_t * command)
{
int ret_val;
command->data[0] = IV_I2C_CMD_GET_FUNC;
ret_val = send_cmd(command, APP_ID_CAPT | capt_nr, CMD_GET(IV_I2C_CMD_CTRL), 1);
if (ret_val<0)
{
return ret_val; /* err code*/
}
return command->data[0];
}
int VCGetWakeupSign(Command_t * command, int * pmsecond)
{
int ret_val;
command->data[0] = IV_I2C_CMD_GET_WK_SIGN;
ret_val = send_cmd(command, APP_ID_CAPT | capt_nr, CMD_GET(IV_I2C_CMD_CTRL), 2);
if (ret_val<0)
{
return ret_val; /* err code*/
}
if(pmsecond)
{
*pmsecond = command->data[1];
}
return command->data[0];
}
int test_version(Command_t *command)
{
int ret_val;
ret_val = send_cmd(command, (uint32_t)APP_ID_CTRL, CMD_GET(CONTROL_APP_VERSION), 0);
printk ("version number: %d.%d.%d.%d\n",command->data[0],
command->data[1], command->data[2], command->data[3]);
if (ret_val<0)
{
printk ("failed to get FW version:%d\n", ret_val);
return ret_val; /* err code*/
}
return(0);
}
void Robot::Decode(uint8_t action, uint8_t speed){
switch(CMD_GET(action)){
case FORWARD:
if(MOTOR_NUM(action) == MOTOR1){
m_pM1->Forward(speed);
}
else if(MOTOR_NUM(action) == MOTOR2) {
m_pM2->Forward(speed);
}
break;
case REVERSE:
if(MOTOR_NUM(action) == MOTOR1) {
m_pM1->Reverse(speed);
}
else if(MOTOR_NUM(action) == MOTOR2) {
m_pM2->Reverse(speed);
}
break;
case FREE1:
if(MOTOR_NUM(action) == MOTOR1) {
m_pM1->Free();
}
else if(MOTOR_NUM(action) == MOTOR2) {
m_pM2->Free();
}
break;
case BRAKE1:
case BRAKE2:
if(MOTOR_NUM(action) == MOTOR1) {
m_pM1->Brake();
}
else if(MOTOR_NUM(action) == MOTOR2) {
m_pM2->Brake();
}
default:
Stop();
break;
}
}
int send_cmd (Command_t *cmd, uint32_t app_module_id, uint32_t command_id, uint32_t num_16b_words)
{
unsigned int *i2c_data = (unsigned int *)cmd;
int size = (num_16b_words + 2) * 4;
int elapsed_ms = 0;
int ret = 0;
// at least two words of header
if (num_16b_words > MAX_COMMAND_SIZE)
{
return(-1);
}
cmd->num_16b_words = (uint16_t)((command_id&CMD_GET(0)) ? MAX_COMMAND_SIZE : num_16b_words);
cmd->command_id = (uint16_t)command_id;
//#if 0
// cmd->reply = 0;
//#else
cmd->command_id = cmd->command_id & 0x7FFF;
//#endif
cmd->app_module_id = app_module_id;
// printk("==========command_id = %d\n", cmd->command_id);
// write words 1 to N-1 , to addresses 4 to 4+4*N-1
byte_order_swap_cmd_data((unsigned char *)&i2c_data[1], (size - 4));
ret = VCI2CWrite(0x4, (unsigned char *)(&i2c_data[1]), (size - 4));
// printk ("cmd->data: %d.%d.%d.%d\n",cmd->data[0],
// cmd->data[1], cmd->data[2], cmd->data[3]);
// printk("==========command_id = %d\n", cmd->command_id);
if (ret == 0) {
return -1;
}
// write word 0 to address 0o
byte_order_swap_cmd_head((unsigned char *)&i2c_data[0]);
ret = VCI2CWrite(0x0, (unsigned char *)(&i2c_data[0]), 4);
// printk ("cmd->data: %d.%d.%d.%d\n",cmd->data[0],
// cmd->data[1], cmd->data[2], cmd->data[3]);
// printk("==========command_id = %d\n", cmd->command_id);
if (ret == 0) {
return -1;
}
#if 1
ret = VCI2CRead(0x0, (unsigned char *)&i2c_data[0], 4);
// printk("==========VCI2CRead:0x00 =%d\n", ret);
// printk ("cmd->data: %d.%d.%d.%d\n",cmd->data[0],
// cmd->data[1], cmd->data[2], cmd->data[3]);
// printk("==========command_id = %d\n", cmd->command_id);
#endif
while (elapsed_ms < REPLY_POLL_TIMEOUT_MSEC)
{
// only read the first word and check the reply bit
// ret = VCI2CRead(0x0, (unsigned char *)&i2c_data[0], 4);
// printk("==========VCI2CRead:0x00 =%d\n", ret);
byte_order_swap_cmd_head((unsigned char *)&i2c_data[0]);
//#if 0
// if (cmd->reply==1)
//#else
if (ret != 0 && (cmd->command_id & 0x8000) == 0x8000)
//#endif
break;
// VCI2CSleep(REPLY_POLL_INTERVAL_MSEC);
elapsed_ms += REPLY_POLL_INTERVAL_MSEC;
}
m_time_out_ms += elapsed_ms;
//#if 0
// if (cmd->reply==1)
//#else
if ((cmd->command_id & 0x8000) == 0x8000)
//#endif
{
if (cmd->num_16b_words > 0)
{
ret = VCI2CRead(0x8, (unsigned char *)&i2c_data[2], (cmd->num_16b_words)*4);
// printk("==========VCI2CRead:0x08 =%d\n", ret);
byte_order_swap_cmd_data((unsigned char *)&i2c_data[2], (cmd->num_16b_words)*4);
if (ret == 0) {
return -1;
}
}
// VCI2CSleep(REPLY_POLL_INTERVAL_MSEC);
return(cmd->num_16b_words);
}
return(-1);
}
