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); }