void actuators_bebop_commit(void)
{
// Receive the status
actuators_bebop.i2c_trans.buf[0] = ACTUATORS_BEBOP_GET_OBS_DATA;
i2c_transceive(&i2c1, &actuators_bebop.i2c_trans, actuators_bebop.i2c_trans.slave_addr, 1, 13);
// Update status
electrical.vsupply = (actuators_bebop.i2c_trans.buf[9] + (actuators_bebop.i2c_trans.buf[8] << 8)) / 100;
// The 15th bit contains saturation information, so it needs to be removed to get the rpm
actuators_bebop.rpm_obs[0] = (actuators_bebop.i2c_trans.buf[1] + (actuators_bebop.i2c_trans.buf[0] << 8)) & ~(1<<15);
actuators_bebop.rpm_obs[1] = (actuators_bebop.i2c_trans.buf[3] + (actuators_bebop.i2c_trans.buf[2] << 8)) & ~(1<<15);
actuators_bebop.rpm_obs[2] = (actuators_bebop.i2c_trans.buf[5] + (actuators_bebop.i2c_trans.buf[4] << 8)) & ~(1<<15);
actuators_bebop.rpm_obs[3] = (actuators_bebop.i2c_trans.buf[7] + (actuators_bebop.i2c_trans.buf[6] << 8)) & ~(1<<15);
// When detected a suicide
actuators_bebop.i2c_trans.buf[10] = actuators_bebop.i2c_trans.buf[10] & 0x7;
if (actuators_bebop.i2c_trans.buf[11] == 2 && actuators_bebop.i2c_trans.buf[10] != 1) {
autopilot_set_motors_on(FALSE);
}
// Start the motors
if (actuators_bebop.i2c_trans.buf[10] != 4 && actuators_bebop.i2c_trans.buf[10] != 2 && autopilot_motors_on) {
// Reset the error
actuators_bebop.i2c_trans.buf[0] = ACTUATORS_BEBOP_CLEAR_ERROR;
i2c_transmit(&i2c1, &actuators_bebop.i2c_trans, actuators_bebop.i2c_trans.slave_addr, 1);
// Start the motors
actuators_bebop.i2c_trans.buf[0] = ACTUATORS_BEBOP_START_PROP;
#if BEBOP_VERSION2
actuators_bebop.i2c_trans.buf[1] = 0b00000110; // For Bebop version 2 some motors are reversed (FIXME: test final version)
#else
actuators_bebop.i2c_trans.buf[1] = 0b00000000;
#endif
i2c_transmit(&i2c1, &actuators_bebop.i2c_trans, actuators_bebop.i2c_trans.slave_addr, 2);
}
// Stop the motors
else if (actuators_bebop.i2c_trans.buf[10] == 4 && !autopilot_motors_on) {
actuators_bebop.i2c_trans.buf[0] = ACTUATORS_BEBOP_STOP_PROP;
i2c_transmit(&i2c1, &actuators_bebop.i2c_trans, actuators_bebop.i2c_trans.slave_addr, 1);
} else if (actuators_bebop.i2c_trans.buf[10] == 4 && autopilot_motors_on) {
// Send the commands
actuators_bebop.i2c_trans.buf[0] = ACTUATORS_BEBOP_SET_REF_SPEED;
actuators_bebop.i2c_trans.buf[1] = actuators_bebop.rpm_ref[0] >> 8;
actuators_bebop.i2c_trans.buf[2] = actuators_bebop.rpm_ref[0] & 0xFF;
actuators_bebop.i2c_trans.buf[3] = actuators_bebop.rpm_ref[1] >> 8;
actuators_bebop.i2c_trans.buf[4] = actuators_bebop.rpm_ref[1] & 0xFF;
actuators_bebop.i2c_trans.buf[5] = actuators_bebop.rpm_ref[2] >> 8;
actuators_bebop.i2c_trans.buf[6] = actuators_bebop.rpm_ref[2] & 0xFF;
actuators_bebop.i2c_trans.buf[7] = actuators_bebop.rpm_ref[3] >> 8;
actuators_bebop.i2c_trans.buf[8] = actuators_bebop.rpm_ref[3] & 0xFF;
actuators_bebop.i2c_trans.buf[9] = 0x00; //UNK?
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wcast-qual"
actuators_bebop.i2c_trans.buf[10] = actuators_bebop_checksum((uint8_t *)actuators_bebop.i2c_trans.buf, 9);
#pragma GCC diagnostic pop
i2c_transmit(&i2c1, &actuators_bebop.i2c_trans, actuators_bebop.i2c_trans.slave_addr, 11);
}
void actuators_bebop_commit(void)
{
// Receive the status
actuators_bebop.i2c_trans.buf[0] = ACTUATORS_BEBOP_GET_OBS_DATA;
i2c_transceive(&i2c1, &actuators_bebop.i2c_trans, actuators_bebop.i2c_trans.slave_addr, 1, 13);
// Update status
electrical.vsupply = (actuators_bebop.i2c_trans.buf[9] + (actuators_bebop.i2c_trans.buf[8] << 8)) / 100;
actuators_bebop.rpm_obs[0] = (actuators_bebop.i2c_trans.buf[1] + (actuators_bebop.i2c_trans.buf[0] << 8));
actuators_bebop.rpm_obs[1] = (actuators_bebop.i2c_trans.buf[3] + (actuators_bebop.i2c_trans.buf[2] << 8));
actuators_bebop.rpm_obs[2] = (actuators_bebop.i2c_trans.buf[5] + (actuators_bebop.i2c_trans.buf[4] << 8));
actuators_bebop.rpm_obs[3] = (actuators_bebop.i2c_trans.buf[7] + (actuators_bebop.i2c_trans.buf[6] << 8));
// Saturate the bebop motors
//actuators_bebop_saturate();
// When detected a suicide
actuators_bebop.i2c_trans.buf[10] = actuators_bebop.i2c_trans.buf[10] & 0x7;
if (actuators_bebop.i2c_trans.buf[11] == 2 && actuators_bebop.i2c_trans.buf[10] != 1) {
autopilot_set_motors_on(FALSE);
}
// Start the motors
if (actuators_bebop.i2c_trans.buf[10] != 4 && actuators_bebop.i2c_trans.buf[10] != 2 && autopilot_motors_on) {
// Reset the error
actuators_bebop.i2c_trans.buf[0] = ACTUATORS_BEBOP_CLEAR_ERROR;
i2c_transmit(&i2c1, &actuators_bebop.i2c_trans, actuators_bebop.i2c_trans.slave_addr, 1);
// Start the motors
actuators_bebop.i2c_trans.buf[0] = ACTUATORS_BEBOP_START_PROP;
i2c_transmit(&i2c1, &actuators_bebop.i2c_trans, actuators_bebop.i2c_trans.slave_addr, 1);
}
// Stop the motors
else if (actuators_bebop.i2c_trans.buf[10] == 4 && !autopilot_motors_on) {
actuators_bebop.i2c_trans.buf[0] = ACTUATORS_BEBOP_STOP_PROP;
i2c_transmit(&i2c1, &actuators_bebop.i2c_trans, actuators_bebop.i2c_trans.slave_addr, 1);
} else if (actuators_bebop.i2c_trans.buf[10] == 4 && autopilot_motors_on) {
// Send the commands
actuators_bebop.i2c_trans.buf[0] = ACTUATORS_BEBOP_SET_REF_SPEED;
actuators_bebop.i2c_trans.buf[1] = actuators_bebop.rpm_ref[0] >> 8;
actuators_bebop.i2c_trans.buf[2] = actuators_bebop.rpm_ref[0] & 0xFF;
actuators_bebop.i2c_trans.buf[3] = actuators_bebop.rpm_ref[1] >> 8;
actuators_bebop.i2c_trans.buf[4] = actuators_bebop.rpm_ref[1] & 0xFF;
actuators_bebop.i2c_trans.buf[5] = actuators_bebop.rpm_ref[2] >> 8;
actuators_bebop.i2c_trans.buf[6] = actuators_bebop.rpm_ref[2] & 0xFF;
actuators_bebop.i2c_trans.buf[7] = actuators_bebop.rpm_ref[3] >> 8;
actuators_bebop.i2c_trans.buf[8] = actuators_bebop.rpm_ref[3] & 0xFF;
actuators_bebop.i2c_trans.buf[9] = 0x00; //UNK?
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wcast-qual"
actuators_bebop.i2c_trans.buf[10] = actuators_bebop_checksum((uint8_t *)actuators_bebop.i2c_trans.buf, 9);
#pragma GCC diagnostic pop
i2c_transmit(&i2c1, &actuators_bebop.i2c_trans, actuators_bebop.i2c_trans.slave_addr, 11);
}
