void ev3_sensor_config(sensor_port_t port, sensor_type_t type) {
ER ercd;
if (!has_inited) {
API_ERROR("Sensor ports must be initialized before reconfiguration");
return;
}
sensors[port] = type;
switch (type) {
case NONE_SENSOR:
case TOUCH_SENSOR:
// Do nothing for analog sensor or no sensor
break;
case ULTRASONIC_SENSOR:
case GYRO_SENSOR:
case COLOR_SENSOR:
// Configure UART sensor
ercd = uart_sensor_config(port, 0);
assert(ercd == E_OK);
// Wait UART sensor to finish initialization
while(!uart_sensor_data_ready(port));
break;
default:
API_ERROR("Invalid sensor type %d", type);
return;
}
}
void ev3_sensors_init(sensor_type_t type1, sensor_type_t type2, sensor_type_t type3, sensor_type_t type4) {
ER ercd;
if (has_inited) {
API_ERROR("Sensor ports already initialized");
return;
} else
has_inited = true;
sensors[Port1] = type1;
sensors[Port2] = type2;
sensors[Port3] = type3;
sensors[Port4] = type4;
for (int i = Port1; i < TNUM_SENSOR_PORT; ++i) {
switch (sensors[i]) {
case NONE_SENSOR:
case TOUCH_SENSOR:
// Do nothing for analog sensor or no sensor
break;
case ULTRASONIC_SENSOR:
case GYRO_SENSOR:
case COLOR_SENSOR:
// Configure UART sensor
ercd = uart_sensor_config(i, 0);
assert(ercd == E_OK);
break;
default:
API_ERROR("Invalid sensor type %d", sensors[i]);
return;
}
}
pUartSensorData = driver_data_pointer(DPID_UART_SENSOR);
pAnalogSensorData = driver_data_pointer(DPID_ANALOG_SENSOR);
for (int i = Port1; i < TNUM_SENSOR_PORT; ++i) {
switch (sensors[i]) {
case ULTRASONIC_SENSOR:
case GYRO_SENSOR:
case COLOR_SENSOR:
// Wait UART sensor to finish initialization
while(!uart_sensor_data_ready(i));
break;
default:
break;
}
}
// dump_uart_sensor_types();
}
bool
gcd_api_relXID( GCD_CCB *ccb )
{
IIAPI_RELXIDPARM parm;
parm.rl_tranIdHandle = ccb->xact.distXID;
ccb->xact.distXID = NULL;
IIapi_releaseXID( &parm );
if ( API_ERROR( parm.rl_status ) && GCD_global.gcd_trace_level >= 1 )
TRdisplay( "%4d GCD IIapi_releaseXID() status: %s\n", ccb->id,
gcu_lookup( apiMap, parm.rl_status ) );
return( API_ERROR( parm.rl_status ) ? FALSE : TRUE );
}
int _write(int file, char *ptr, int len) {
ID portid;
switch(file) {
case STDOUT_FILENO:
case STDERR_FILENO:
case SIO_STD_FILENO:
portid = SIO_PORT_DEFAULT;
break;
case SIO_UART_FILENO:
portid = SIO_PORT_UART;
break;
case SIO_BT_FILENO:
portid = SIO_PORT_BT;
break;
default:
errno = EBADF;
return -1;
}
ER_UINT erlen = serial_wri_dat(portid, ptr, len);
assert(erlen > 0);
if(erlen <= 0)
API_ERROR("erlen: %d\n", erlen);
return erlen;
}
ER ev3_sensor_config(sensor_port_t port, sensor_type_t type) {
ER ercd;
// lazy_initialize();
CHECK_PORT(port);
sensors[port] = type;
switch (type) {
case NONE_SENSOR:
case TOUCH_SENSOR:
// Do nothing for analog sensor or no sensor
break;
case ULTRASONIC_SENSOR:
case GYRO_SENSOR:
case COLOR_SENSOR:
// Configure UART sensor
ercd = uart_sensor_config(port, 0);
assert(ercd == E_OK);
// Wait UART sensor to finish initialization
// while(!uart_sensor_data_ready(port));
break;
default:
API_ERROR("Invalid sensor type %d", type);
return E_PAR;
}
ercd = E_OK;
error_exit:
return ercd;
}
II_FAR II_CALLBACK II_VOID
gcd_gqi_cmpl( II_PTR arg, II_PTR parm )
{
IIAPI_GETQINFOPARM *gqi = (IIAPI_GETQINFOPARM *)parm;
GCD_PCB *pcb = (GCD_PCB *)arg;
if ( ! API_ERROR( gqi->gq_genParm.gp_status ) )
{
if ( gqi->gq_flags & IIAPI_GQF_END_OF_DATA )
pcb->result.flags |= PCB_RSLT_EOD;
if ( gqi->gq_mask & IIAPI_GQ_CURSOR )
{
if ( ! (gqi->gq_cursorType & IIAPI_CURSOR_UPDATE) )
pcb->result.flags |= PCB_RSLT_READ_ONLY;
if ( gqi->gq_cursorType & IIAPI_CURSOR_SCROLL )
pcb->result.flags |= PCB_RSLT_SCROLL;
}
if ( gqi->gq_mask & IIAPI_GQ_ROW_STATUS )
{
pcb->result.flags |= PCB_RSLT_ROW_STATUS;
pcb->result.row_status = (u_i2)gqi->gq_rowStatus;
pcb->result.row_position = gqi->gq_rowPosition;
}
if ( gqi->gq_mask & IIAPI_GQ_ROW_COUNT )
{
pcb->result.flags |= PCB_RSLT_ROW_COUNT;
pcb->result.row_count = gqi->gq_rowCount;
}
if ( gqi->gq_mask & IIAPI_GQ_PROCEDURE_RET )
{
pcb->result.flags |= PCB_RSLT_PROC_RET;
pcb->result.proc_ret = gqi->gq_procedureReturn;
}
if ( gqi->gq_mask & IIAPI_GQ_TABLE_KEY )
{
pcb->result.flags |= PCB_RSLT_TBLKEY;
MEcopy( (PTR)gqi->gq_tableKey, sizeof( pcb->result.tblkey ),
(PTR)pcb->result.tblkey );
}
if ( gqi->gq_mask & IIAPI_GQ_OBJECT_KEY )
{
pcb->result.flags |= PCB_RSLT_OBJKEY;
MEcopy( (PTR)gqi->gq_objectKey, sizeof( pcb->result.objkey ),
(PTR)pcb->result.objkey );
}
}
gcd_gen_cmpl( arg, parm );
return;
}
FILE* ev3_serial_open_file(serial_port_t port) {
int fd;
if (port == EV3_SERIAL_DEFAULT)
fd = SIO_STD_FILENO;
else if (port == EV3_SERIAL_UART)
fd = SIO_UART_FILENO;
else if (port == EV3_SERIAL_BT)
fd = SIO_BT_FILENO;
else {
API_ERROR("Invalid port id %d.", port);
return NULL;
}
FILE *fp = fdopen(fd, "a+");
if (fp != NULL)
setbuf(fp, NULL); /* IMPORTANT! */
else
API_ERROR("fdopen() failed, fd: %d.", fd);
return fp;
}
II_FAR II_CALLBACK II_VOID
gcd_sp_cmpl( II_PTR arg, II_PTR parm )
{
GCD_PCB *pcb = (GCD_PCB *)arg;
IIAPI_SAVEPTPARM *sp = (IIAPI_SAVEPTPARM *)parm;
if ( ! API_ERROR( sp->sp_genParm.gp_status ) )
pcb->ccb->xact.savepoints->hndl = sp->sp_savePointHandle;
gcd_gen_cmpl( arg, parm );
return;
}
II_FAR II_CALLBACK II_VOID
gcd_start_cmpl( II_PTR arg, II_PTR parm )
{
IIAPI_XASTARTPARM *start = (IIAPI_XASTARTPARM *)parm;
GCD_PCB *pcb = (GCD_PCB *)arg;
if ( ! API_ERROR( start->xs_genParm.gp_status ) )
pcb->ccb->cib->tran = start->xs_tranHandle;
gcd_gen_cmpl( arg, parm );
return;
}
void DJI::onboardSDK::API::taskCallback(DJI::onboardSDK::API *This, Header *header)
{
unsigned short ack_data;
if(header->length - EXC_DATA_SIZE <= 2)
{
memcpy((unsigned char *)&ack_data,((unsigned char *)header)+sizeof(Header), (header->length - EXC_DATA_SIZE));
API_STATUS("%s,task running successfully,%d\n",__func__,ack_data);
//!@ todo
}
else
{
API_ERROR("%s,line %d:ERROR,ACK is exception,seesion id %d,sequence %d\n",
__func__,__LINE__,header->session_id,header->sequence_number);
}
}
void DJI::onboardSDK::API::sendToMobileCallback(DJI::onboardSDK::API *This, Header *header)
{
unsigned short ack_data = 0xFFFF;
if(header->length - EXC_DATA_SIZE <= 2)
{
memcpy((unsigned char *)&ack_data,((unsigned char *)header)+sizeof(Header), (header->length - EXC_DATA_SIZE));
if(This->toMobileResult)
This->toMobileResult(ack_data);
}
else
{
API_ERROR("%s,line %d:ERROR,ACK is exception,seesion id %d,sequence %d\n",
__func__,__LINE__,header->session_id,header->sequence_number);
}
}
int DJI::onboardSDK::API::setCamera(DJI::onboardSDK::CAMERA camera_cmd)
{
unsigned char send_data = 0;
if(camera_cmd != API_CAMERA_SHOT && camera_cmd != API_CAMERA_VIDEO_START
&& camera_cmd != API_CAMERA_VIDEO_STOP)
{
API_ERROR("%s,line %d,Param ERROR\n",__func__,__LINE__);
return -1;
}
send(0,1, SET_CONTROL, camera_cmd,
(unsigned char*)&send_data,sizeof(send_data),0,0,0);
return 0;
}
void DJI::onboardSDK::API::activateCallback(DJI::onboardSDK::API *This, Header *header)
{
//printf("%s: Entery",__func__);
volatile unsigned short ack_data;
if(header->length - EXC_DATA_SIZE <= 2)
{
memcpy((unsigned char *)&ack_data,((unsigned char *)header)+sizeof(Header), (header->length - EXC_DATA_SIZE));
if(ack_data == SDK_ACTIVATE_NEW_DEVICE)
{
API_STATUS("new device try again \n");
}
else
{
if(ack_data == SDK_ACTIVATE_SUCCESS)
{
API_STATUS("Activation Successfully\n");
This->driver->lockMSG();
This->broadcastData.activation= 1;
This->driver->freeMSG();
if(This->accountData.app_key)
This->setKey(This->accountData.app_key);
}
else
{
API_STATUS("%s,line %d,activate ERR code:0x%X\n",__func__,__LINE__,ack_data);
This->driver->lockMSG();
This->broadcastData.activation= 0;
This->driver->freeMSG();
}
if(This->activateResult)
{
This->activateResult(ack_data);
}
}
}
else
{
API_ERROR("%s,line %d:ERROR,ACK is exception,seesion id %d,sequence %d\n",
__func__,__LINE__,header->session_id,header->sequence_number);
}
}
II_FAR II_CALLBACK II_VOID
gcd_gDesc_cmpl( II_PTR arg, II_PTR parm )
{
IIAPI_GETDESCRPARM *gDesc = (IIAPI_GETDESCRPARM *)parm;
GCD_PCB *pcb = (GCD_PCB *)arg;
if ( API_ERROR( gDesc->gd_genParm.gp_status ) ||
gDesc->gd_genParm.gp_status == IIAPI_ST_NO_DATA )
pcb->data.desc.count = 0;
else
{
pcb->data.desc.count = gDesc->gd_descriptorCount;
pcb->data.desc.desc = gDesc->gd_descriptor;
}
gcd_gen_cmpl( arg, parm );
return;
}
static inline
int getDevType(motor_type_t type) {
switch(type) {
case NONE_MOTOR:
return TYPE_NONE;
break;
case MEDIUM_MOTOR:
return TYPE_MINITACHO;
break;
case LARGE_MOTOR:
case UNREGULATED_MOTOR: // TODO: check this
return TYPE_TACHO;
break;
default:
API_ERROR("Invalid motor type %d", type);
return TYPE_NONE;
}
}
II_FAR II_CALLBACK II_VOID
gcd_gCol_cmpl( II_PTR arg, II_PTR parm )
{
IIAPI_GETCOLPARM *gcol = (IIAPI_GETCOLPARM *)parm;
GCD_PCB *pcb = (GCD_PCB *)arg;
if ( API_ERROR( gcol->gc_genParm.gp_status ) ||
gcol->gc_genParm.gp_status == IIAPI_ST_NO_DATA )
{
pcb->data.data.row_cnt = 0;
pcb->data.data.col_cnt = 0;
pcb->data.data.more_segments = FALSE;
}
else
{
pcb->data.data.row_cnt = gcol->gc_rowsReturned;
pcb->data.data.more_segments = gcol->gc_moreSegments;
}
gcd_gen_cmpl( arg, parm );
return;
}
bool
gcd_api_regXID( GCD_CCB *ccb, IIAPI_TRAN_ID *tranID )
{
IIAPI_REGXIDPARM parm;
bool success = FALSE;
MEcopy( (PTR)tranID, sizeof( *tranID ), (PTR)&parm.rg_tranID );
IIapi_registerXID( &parm );
if ( ! API_ERROR( parm.rg_status ) )
{
ccb->xact.distXID = parm.rg_tranIdHandle;
success = TRUE;
}
else if ( GCD_global.gcd_trace_level >= 1 )
{
TRdisplay( "%4d GCD IIapi_registerXID() status: %s\n", ccb->id,
gcu_lookup( apiMap, parm.rg_status ) );
}
return( success );
}
void DJI::onboardSDK::API::setControlCallback(API* This,Header *header)
{
unsigned short ack_data = 0xFFFF;
if(header->length - EXC_DATA_SIZE <= 2)
{
memcpy((unsigned char *)&ack_data,((unsigned char *)header)+sizeof(Header), (header->length - EXC_DATA_SIZE));
if(This->setControlResult)
This->setControlResult(ack_data);
}
else
{
API_ERROR("%s,line %d:ERROR,ACK is exception,seesion id %d,sequence %d\n",
__func__,__LINE__,header->session_id,header->sequence_number);
}
switch(ack_data)
{
case 0x0000:
API_STATUS("%s,line %d, obtain control failed, Conditions did not satisfied",__func__,__LINE__);
case 0x0001:
API_STATUS("%s,line %d, release control successfully\n",__func__,__LINE__);
break;
case 0x0002:
API_STATUS("%s,line %d, obtain control successfully\n",__func__,__LINE__);
break;
case 0x0003:
API_STATUS("%s,line %d, obtain control running\n",__func__,__LINE__);
break;
case 0x0004:
API_STATUS("control request already done");
break;
default:
API_STATUS("%s,line %d, there is unkown error,ack=0x%X\n",__func__,__LINE__,ack_data);
break;
}
}
static STATUS
gcd_api_status( char *func_name, IIAPI_GENPARM *genParm,
GCD_CCB *ccb, GCD_RCB **rcb_ptr )
{
IIAPI_STATUS api_status = genParm->gp_status;
bool api_error = API_ERROR( genParm->gp_status );
bool dbms_error = FALSE;
STATUS status = OK;
if ( (api_error && GCD_global.gcd_trace_level >= 1) ||
GCD_global.gcd_trace_level >= 2 )
TRdisplay( "%4d GCD %s status: %s\n", ccb->id, func_name,
gcu_lookup( apiMap, genParm->gp_status ) );
if ( genParm->gp_errorHandle )
{
IIAPI_GETEINFOPARM get;
get.ge_errorHandle = genParm->gp_errorHandle;
IIapi_getErrorInfo( &get );
while( get.ge_status == IIAPI_ST_SUCCESS )
{
u_i2 msg_len = get.ge_message ? (u_i2)STlength(get.ge_message) : 0;
switch( get.ge_type )
{
case IIAPI_GE_ERROR :
switch( get.ge_errorCode )
{
case E_AP0001_CONNECTION_ABORTED :
ccb->cib->flags |= GCD_CONN_ABORT;
break;
case E_AP0002_TRANSACTION_ABORTED :
ccb->cib->flags |= GCD_XACT_ABORT;
break;
case E_AP0003_ACTIVE_TRANSACTIONS :
case E_AP0004_ACTIVE_QUERIES :
case E_AP0006_INVALID_SEQUENCE :
ccb->cib->flags |= GCD_LOGIC_ERR;
break;
}
if ( ! dbms_error )
{
status = get.ge_errorCode;
dbms_error = TRUE;
}
if ( GCD_global.gcd_trace_level >= 2 )
TRdisplay( "%4d GCD Error 0x%x '%s'\n", ccb->id,
get.ge_errorCode, get.ge_SQLSTATE );
if ( GCD_global.gcd_trace_level >= 3 && get.ge_message )
TRdisplay( "%4d GCD %s\n", ccb->id, get.ge_message );
if ( rcb_ptr )
gcd_send_error( ccb, rcb_ptr, MSG_ET_ERR, get.ge_errorCode,
get.ge_SQLSTATE, msg_len, get.ge_message );
break;
case IIAPI_GE_XAERR :
if ( GCD_global.gcd_trace_level >= 2 )
TRdisplay( "%4d GCD XA Error %d\n", ccb->id,
get.ge_errorCode );
if ( ! dbms_error )
{
status = FAIL;
dbms_error = TRUE;
}
if ( rcb_ptr )
gcd_send_error( ccb, rcb_ptr, MSG_ET_XA, get.ge_errorCode,
get.ge_SQLSTATE, msg_len, get.ge_message );
break;
case IIAPI_GE_WARNING :
if ( GCD_global.gcd_trace_level >= 2 )
TRdisplay( "%4d GCD Warning 0x%x '%s'\n",
ccb->id, get.ge_errorCode, get.ge_SQLSTATE );
if ( GCD_global.gcd_trace_level >= 3 && get.ge_message )
TRdisplay( "%4d GCD %s\n", ccb->id, get.ge_message );
if ( rcb_ptr )
gcd_send_error( ccb, rcb_ptr, MSG_ET_WRN, get.ge_errorCode,
get.ge_SQLSTATE, msg_len, get.ge_message );
break;
default :
if ( GCD_global.gcd_trace_level >= 2 )
TRdisplay( "%4d GCD User Message 0x%x\n",
ccb->id, get.ge_errorCode );
if ( GCD_global.gcd_trace_level >= 3 && get.ge_message )
TRdisplay( "%4d GCD %s\n", ccb->id, get.ge_message );
if ( rcb_ptr )
gcd_send_error( ccb, rcb_ptr, MSG_ET_MSG, get.ge_errorCode,
get.ge_SQLSTATE, msg_len, get.ge_message );
break;
}
IIapi_getErrorInfo( &get );
}
}
if ( dbms_error && ! status )
status = E_GC4809_INTERNAL_ERROR;
else if ( api_error && ! dbms_error )
{
status = (api_status == IIAPI_ST_OUT_OF_MEMORY)
? E_GC4808_NO_MEMORY : E_GC4809_INTERNAL_ERROR;
if ( rcb_ptr ) gcd_sess_error( ccb, rcb_ptr, status );
}
return( status );
}