bool AP_Arming::ins_gyros_consistent(const AP_InertialSensor &ins)
{
const uint8_t gyro_count = ins.get_gyro_count();
if (gyro_count <= 1) {
return true;
}
const Vector3f &prime_gyro_vec = ins.get_gyro();
const uint32_t now = AP_HAL::millis();
for(uint8_t i=0; i<gyro_count; i++) {
if (!ins.use_gyro(i)) {
continue;
}
// get next gyro vector
const Vector3f &gyro_vec = ins.get_gyro(i);
Vector3f vec_diff = gyro_vec - prime_gyro_vec;
// allow for up to 5 degrees/s difference. Pass if it has
// been OK in last 10 seconds
if (vec_diff.length() <= radians(5)) {
last_gyro_pass_ms[i] = now;
}
if (now - last_gyro_pass_ms[i] > 10000) {
return false;
}
}
return true;
}
void setup(void)
{
// setup any board specific drivers
BoardConfig.init();
hal.console->printf("AP_InertialSensor startup...\n");
ins.init(100);
// display initial values
display_offsets_and_scaling();
// display number of detected accels/gyros
hal.console->printf("\n");
hal.console->printf("Number of detected accels : %u\n", ins.get_accel_count());
hal.console->printf("Number of detected gyros : %u\n\n", ins.get_gyro_count());
hal.console->printf("Complete. Reading:\n");
}
void GCS_MAVLINK::send_raw_imu(const AP_InertialSensor &ins, const Compass &compass)
{
const Vector3f &accel = ins.get_accel(0);
const Vector3f &gyro = ins.get_gyro(0);
const Vector3f &mag = compass.get_field(0);
mavlink_msg_raw_imu_send(
chan,
hal.scheduler->micros(),
accel.x * 1000.0f / GRAVITY_MSS,
accel.y * 1000.0f / GRAVITY_MSS,
accel.z * 1000.0f / GRAVITY_MSS,
gyro.x * 1000.0f,
gyro.y * 1000.0f,
gyro.z * 1000.0f,
mag.x,
mag.y,
mag.z);
#if INS_MAX_INSTANCES > 1
if (ins.get_gyro_count() <= 1 &&
ins.get_accel_count() <= 1 &&
compass.get_count() <= 1) {
return;
}
const Vector3f &accel2 = ins.get_accel(1);
const Vector3f &gyro2 = ins.get_gyro(1);
const Vector3f &mag2 = compass.get_field(1);
mavlink_msg_scaled_imu2_send(
chan,
hal.scheduler->millis(),
accel2.x * 1000.0f / GRAVITY_MSS,
accel2.y * 1000.0f / GRAVITY_MSS,
accel2.z * 1000.0f / GRAVITY_MSS,
gyro2.x * 1000.0f,
gyro2.y * 1000.0f,
gyro2.z * 1000.0f,
mag2.x,
mag2.y,
mag2.z);
#endif
}
void GCS_MAVLINK::send_raw_imu(const AP_InertialSensor &ins, const Compass &compass)
{
const Vector3f &accel = ins.get_accel(0);
const Vector3f &gyro = ins.get_gyro(0);
Vector3f mag;
if (compass.get_count() >= 1) {
mag = compass.get_field(0);
} else {
mag.zero();
}
mavlink_msg_raw_imu_send(
chan,
AP_HAL::micros(),
accel.x * 1000.0f / GRAVITY_MSS,
accel.y * 1000.0f / GRAVITY_MSS,
accel.z * 1000.0f / GRAVITY_MSS,
gyro.x * 1000.0f,
gyro.y * 1000.0f,
gyro.z * 1000.0f,
mag.x,
mag.y,
mag.z);
if (ins.get_gyro_count() <= 1 &&
ins.get_accel_count() <= 1 &&
compass.get_count() <= 1) {
return;
}
const Vector3f &accel2 = ins.get_accel(1);
const Vector3f &gyro2 = ins.get_gyro(1);
if (compass.get_count() >= 2) {
mag = compass.get_field(1);
} else {
mag.zero();
}
mavlink_msg_scaled_imu2_send(
chan,
AP_HAL::millis(),
accel2.x * 1000.0f / GRAVITY_MSS,
accel2.y * 1000.0f / GRAVITY_MSS,
accel2.z * 1000.0f / GRAVITY_MSS,
gyro2.x * 1000.0f,
gyro2.y * 1000.0f,
gyro2.z * 1000.0f,
mag.x,
mag.y,
mag.z);
if (ins.get_gyro_count() <= 2 &&
ins.get_accel_count() <= 2 &&
compass.get_count() <= 2) {
return;
}
const Vector3f &accel3 = ins.get_accel(2);
const Vector3f &gyro3 = ins.get_gyro(2);
if (compass.get_count() >= 3) {
mag = compass.get_field(2);
} else {
mag.zero();
}
mavlink_msg_scaled_imu3_send(
chan,
AP_HAL::millis(),
accel3.x * 1000.0f / GRAVITY_MSS,
accel3.y * 1000.0f / GRAVITY_MSS,
accel3.z * 1000.0f / GRAVITY_MSS,
gyro3.x * 1000.0f,
gyro3.y * 1000.0f,
gyro3.z * 1000.0f,
mag.x,
mag.y,
mag.z);
}
static void run_test()
{
Vector3f accel;
Vector3f gyro;
uint8_t counter = 0;
static uint8_t accel_count = ins.get_accel_count();
static uint8_t gyro_count = ins.get_gyro_count();
static uint8_t ins_count = MAX(accel_count, gyro_count);
// flush any user input
while (hal.console->available()) {
hal.console->read();
}
// clear out any existing samples from ins
ins.update();
// loop as long as user does not press a key
while (!hal.console->available()) {
// wait until we have a sample
ins.wait_for_sample();
// read samples from ins
ins.update();
// print each accel/gyro result every 50 cycles
if (counter++ % 50 != 0) {
continue;
}
// loop and print each sensor
for (uint8_t ii = 0; ii < ins_count; ii++) {
char state;
if (ii > accel_count - 1) {
// No accel present
state = '-';
} else if (ins.get_accel_health(ii)) {
// Healthy accel
state = 'h';
} else {
// Accel present but not healthy
state = 'u';
}
accel = ins.get_accel(ii);
hal.console->printf("%u - Accel (%c) : X:%6.2f Y:%6.2f Z:%6.2f norm:%5.2f",
ii, state, (double)accel.x, (double)accel.y, (double)accel.z,
(double)accel.length());
gyro = ins.get_gyro(ii);
if (ii > gyro_count - 1) {
// No gyro present
state = '-';
} else if (ins.get_gyro_health(ii)) {
// Healthy gyro
state = 'h';
} else {
// Gyro present but not healthy
state = 'u';
}
hal.console->printf(" Gyro (%c) : X:%6.2f Y:%6.2f Z:%6.2f\n",
state, (double)gyro.x, (double)gyro.y, (double)gyro.z);
auto temp = ins.get_temperature(ii);
hal.console->printf(" t:%6.2f\n", (double)temp);
}
}
// clear user input
while (hal.console->available()) {
hal.console->read();
}
}