//Ejemplo que pide los datos de la placa y pueden ser leidos desde el buffer
void exampleInfo(SBGC_Parser sbgc_parser)
{
sbgc_parser.send_command(SBGC_CMD_REALTIME_DATA, NULL, 0, 0);
//printf("Resultado envio -> %d (0 correcto)\n", sbgc_parser.send_command(SBGC_CMD_REALTIME_DATA, NULL, 0, 0));
if (sbgc_parser.read_cmd())
{
printf("\nParseado correctamente");
}
//printf("\nNumero de errores %d", sbgc_parser.in_cmd.data[8]);
//printf("\nBoard Info -> %d\nversion %db%d ", sbgc_parser.in_cmd.data[0], sbgc_parser.in_cmd.data[1], sbgc_parser.in_cmd.data[2]);
SBGC_cmd_realtime_data_t datos;
if(SBGC_cmd_realtime_data_unpack(datos, sbgc_parser.in_cmd) == PARSER_ERROR_WRONG_DATA_SIZE)
{
printf("ERROOOOOOOOOOOR EN DATOS");
}
Sleep(20);
}
void exampleAngles(SBGC_Parser sbgc_parser)
{
sbgc_parser.send_command(SBGC_CMD_GET_ANGLES, NULL, 0, 0);
Sleep(1000);
sbgc_parser.read_cmd();
Sleep(20);
}
//Apagar los motores
void exampleMotorsOFF(SBGC_Parser sbgc_parser)
{
printf("Resultado envio -> %d (0 correcto)\n", sbgc_parser.send_command(SBGC_CMD_MOTORS_OFF, NULL, 0, 0));
sbgc_parser.read_cmd();
Sleep(20);
}
//Ejemplo que pide los datos de la placa y pueden ser leidos desde el buffer
void exampleInfo(SBGC_Parser sbgc_parser)
{
printf("Resultado envio -> %d (0 correcto)\n" , sbgc_parser.send_command(SBGC_CMD_BOARD_INFO,NULL, 0, 0));
Sleep(20);
}
// Process incoming commands. Call it as frequently as possible, to prevent overrun of serial input buffer.
void simplebgc_process_inqueue() {
int i;
while(sbgc_parser.read_cmd()) {
SerialCommand &cmd = sbgc_parser.in_cmd;
uint8_t error = 0;
char tmp[12];
switch(cmd.id) {
case SBGC_CMD_GET_ANGLES:
WOFL_get_angles_t t;
memcpy(&t, cmd.data, cmd.len);//asume little endin
WOFL_get_angles_send(t, wofl_parser);
break;
// Receive realtime data
case SBGC_CMD_REALTIME_DATA_3:
case SBGC_CMD_REALTIME_DATA_4:
error = SBGC_cmd_realtime_data_unpack(rt_data, cmd);
if(!error) {
// Extract some usefull data
/*
rt_data.imu_angle[ROLL];
rt_data.target_angle[ROLL];
rt_data.imu_angle[PITCH];
rt_data.target_angle[PITCH];
rt_data.imu_angle[YAW];
rt_data.target_angle[YAW];
*/
memcpy(tmp,rt_data.rc_raw_data,12);
serialXPrint(2,":");
for(i=0;i<12;i++)
serialXWrite(2,tmp[i]);
serialXWrite(2,'\n');
} else {
sbgc_parser.onParseError(error);
}
break;
default:
serialPrint("unparsed command:");
serialWrite(cmd.id);
for(i=0;i<cmd.len;i++)
serialWrite(cmd.data[i]);
serialWrite('\n');
}
}
}
void simplebgc_init(){
com_obj.init(&stream);
sbgc_parser.init(&com_obj);
}
