/* Packs command structure to SerialCommand object */
void SBGC_cmd_control_pack(SBGC_cmd_control_t &p, SerialCommand &cmd) {
cmd.init(SBGC_CMD_CONTROL);
#ifdef SBGC_CMD_STRUCT_ALIGNED
memcpy(cmd.data, &p, sizeof(p));
cmd.len = sizeof(p);
#else
cmd.writeByte(p.mode);
cmd.writeWord(p.speedROLL);
cmd.writeWord(p.angleROLL);
cmd.writeWord(p.speedPITCH);
cmd.writeWord(p.anglePITCH);
cmd.writeWord(p.speedYAW);
cmd.writeWord(p.angleYAW);
#endif
}
/* Packs command structure to SerialCommand object */
void SBGC_cmd_servo_out_pack(SBGC_cmd_servo_out_t &p, SerialCommand &cmd) {
cmd.init(SBGC_CMD_SERVO_OUT);
#ifdef SBGC_CMD_STRUCT_ALIGNED
memcpy(cmd.data, &p, sizeof(p));
cmd.len = sizeof(p);
#else
for(uint8_t i=0; i<8; i++) {
cmd.writeWord(p.servo[i]);
}
#endif
}
/* Packs command structure to SerialCommand object */
void SBGC_cmd_control_ext_pack(SBGC_cmd_control_ext_t &p, SerialCommand &cmd) {
cmd.init(SBGC_CMD_CONTROL);
#ifdef SBGC_CMD_STRUCT_ALIGNED
memcpy(cmd.data, &p, sizeof(p));
cmd.len = sizeof(p);
#else
cmd.writeBuf(p.mode, 3);
for(uint8_t i=0; i<3; i++) {
cmd.writeWord(p.data[i].speed);
cmd.writeWord(p.data[i].angle);
}
#endif
}
