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
}
/*
* Unpacks SerialCommand object to vars_buf[var_num].
* 'var_num' specifies the buffer capacity.
* On return, 'var_num' will be set to actual number of received variables.
* Returns 0 on success, PARSER_ERROR_XX code on fail.
*/
uint8_t SBGC_cmd_set_adj_vars_unpack(SBGC_cmd_set_adj_vars_var_t vars_buf[], uint8_t &vars_num, SerialCommand &cmd) {
uint8_t num = cmd.readByte(); // actual number of variables
if(num <= vars_num) {
vars_num = num;
#ifdef SBGC_CMD_STRUCT_ALIGNED
cmd.readBuf(vars_buf, sizeof(SBGC_cmd_set_adj_vars_var_t)*vars_num);
#else
for(uint8_t i=0; i<num; i++) {
vars_buf[i].id = cmd.readByte();
vars_buf[i].val = cmd.readLong();
}
#endif
if(cmd.checkLimit()) return 0;
else return PARSER_ERROR_WRONG_DATA_SIZE;
} else {
return PARSER_ERROR_BUFFER_IS_FULL;
}
}