Example #1
0
uint8_t Gimbal::SBGC_cmd_angles_unpack(angulos &p, SerialCommand &cmd) {

	for (uint8_t i = 0; i < 3; i++) {

		switch (i)
		{
		case 0:
			p.roll = cmd.readWord();
			p.roll = p.roll*ESCALA;
			break;
		case 1:
			p.pitch = cmd.readWord();
			p.pitch = p.pitch*ESCALA;
			break;
		case 2:
			p.yaw = cmd.readWord();
			p.yaw = p.yaw*ESCALA;
			break;
		}

		//Leemos dos veces, para consumir los datos rc_angle y rc_speed que no son necesarios.
		cmd.readWord();
		cmd.readWord();
	}

	if (cmd.checkLimit()) return 0;
	else return PARSER_ERROR_WRONG_DATA_SIZE;
}
Example #2
0
/*
* Unpacks SerialCommand object to command structure.
* Returns 0 on success, PARSER_ERROR_XX code on fail.
*/
uint8_t SBGC_cmd_realtime_data_unpack(SBGC_cmd_realtime_data_t &p, SerialCommand &cmd) {
	#ifdef SBGC_CMD_STRUCT_ALIGNED
		if(cmd.len <= sizeof(p)) {
			memcpy(&p, cmd.data, cmd.len);
			return 0;
		} else {
			return PARSER_ERROR_WRONG_DATA_SIZE;
		}
	#else
		for(uint8_t i=0; i<3; i++) {
			p.sensor_data[i].acc_data = cmd.readWord();
			p.sensor_data[i].gyro_data = cmd.readWord();
		}
		p.serial_error_cnt = cmd.readWord();
		p.system_error = cmd.readWord();
		cmd.skipBytes(4); // reserved
		cmd.readWordArr(p.rc_raw_data, SBGC_RC_NUM_CHANNELS);
		cmd.readWordArr(p.imu_angle, 3);
		cmd.readWordArr(p.frame_imu_angle, 3);
		cmd.readWordArr(p.target_angle, 3);
		p.cycle_time_us = cmd.readWord();
		p.i2c_error_count = cmd.readWord();
		cmd.readByte(); // reserved
		p.battery_voltage = cmd.readWord();
		p.state_flags1 = cmd.readByte();
		p.cur_imu = cmd.readByte();
		p.cur_profile = cmd.readByte();
		cmd.readBuf(p.motor_power, 3);
		
		if(cmd.id == SBGC_CMD_REALTIME_DATA_4) {
			cmd.readWordArr(p.rotor_angle, 3);
			cmd.readByte(); // reserved
			cmd.readWordArr(p.balance_error, 3);
			p.current = cmd.readWord();
			cmd.readWordArr(p.magnetometer_data, 3);
			p.imu_temp_celcius = cmd.readByte();
			p.frame_imu_temp_celcius = cmd.readByte();
			cmd.skipBytes(38);
		}
		
		
		if(cmd.checkLimit()) return 0;
		else return PARSER_ERROR_WRONG_DATA_SIZE;
	#endif
}