Пример #1
0
//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);

}
Пример #2
0
// 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');
		}
	}

}