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;
}
/*
* 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
}
