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 GCS_MAVLINK::send_sensor_offsets(const AP_InertialSensor &ins, const Compass &compass, AP_Baro &barometer)
{
// run this message at a much lower rate - otherwise it
// pointlessly wastes quite a lot of bandwidth
static uint8_t counter;
if (counter++ < 10) {
return;
}
counter = 0;
const Vector3f &mag_offsets = compass.get_offsets(0);
const Vector3f &accel_offsets = ins.get_accel_offsets(0);
const Vector3f &gyro_offsets = ins.get_gyro_offsets(0);
mavlink_msg_sensor_offsets_send(chan,
mag_offsets.x,
mag_offsets.y,
mag_offsets.z,
compass.get_declination(),
barometer.get_pressure(),
barometer.get_temperature()*100,
gyro_offsets.x,
gyro_offsets.y,
gyro_offsets.z,
accel_offsets.x,
accel_offsets.y,
accel_offsets.z);
}
/*
send LOCAL_POSITION_NED message
*/
void GCS_MAVLINK::send_vibration(const AP_InertialSensor &ins) const
{
Vector3f vibration = ins.get_vibration_levels();
mavlink_msg_vibration_send(
chan,
AP_HAL::micros64(),
vibration.x,
vibration.y,
vibration.z,
ins.get_accel_clip_count(0),
ins.get_accel_clip_count(1),
ins.get_accel_clip_count(2));
}
void ReplayVehicle::setup(void)
{
load_parameters();
// we pass zero log structures, as we will be outputting the log
// structures we need manually, to prevent FMT duplicates
dataflash.Init(log_structure, 0);
dataflash.StartNewLog();
ahrs.set_compass(&compass);
ahrs.set_fly_forward(true);
ahrs.set_wind_estimation(true);
ahrs.set_correct_centrifugal(true);
ahrs.set_ekf_use(true);
EKF2.set_enable(true);
printf("Starting disarmed\n");
hal.util->set_soft_armed(false);
barometer.init();
barometer.setHIL(0);
barometer.update();
compass.init();
ins.set_hil_mode();
}
/*
send LOCAL_POSITION_NED message
*/
void GCS_MAVLINK::send_vibration(const AP_InertialSensor &ins) const
{
#if INS_VIBRATION_CHECK
Vector3f vibration = ins.get_vibration_levels();
mavlink_msg_vibration_send(
chan,
hal.scheduler->micros64(),
vibration.x,
vibration.y,
vibration.z,
ins.get_accel_clip_count(0),
ins.get_accel_clip_count(1),
ins.get_accel_clip_count(2));
#endif
}
void SchedTest::setup(void)
{
// we
ins.init(AP_InertialSensor::COLD_START,
AP_InertialSensor::RATE_50HZ);
// initialise the scheduler
scheduler.init(&scheduler_tasks[0], ARRAY_SIZE(scheduler_tasks));
}
void setup(void)
{
serial_manager.init();
ins.init(100);
baro.init();
ahrs.init();
gps.init(NULL, serial_manager);
}
/* MPU6000 implementation of the HMC5843 */
AP_HMC5843_SerialBus_MPU6000::AP_HMC5843_SerialBus_MPU6000(AP_InertialSensor &ins,
uint8_t addr)
{
// Only initialize members. Fails are handled by configure or while
// getting the semaphore
_bus = ins.get_auxiliar_bus(HAL_INS_MPU60XX_SPI);
if (!_bus)
return;
_slave = _bus->request_next_slave(addr);
}
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");
}
static void display_offsets_and_scaling()
{
const Vector3f &accel_offsets = ins.get_accel_offsets();
const Vector3f &accel_scale = ins.get_accel_scale();
const Vector3f &gyro_offsets = ins.get_gyro_offsets();
// display results
hal.console->printf("\nAccel Offsets X:%10.8f \t Y:%10.8f \t Z:%10.8f\n",
(double)accel_offsets.x,
(double)accel_offsets.y,
(double)accel_offsets.z);
hal.console->printf("Accel Scale X:%10.8f \t Y:%10.8f \t Z:%10.8f\n",
(double)accel_scale.x,
(double)accel_scale.y,
(double)accel_scale.z);
hal.console->printf("Gyro Offsets X:%10.8f \t Y:%10.8f \t Z:%10.8f\n",
(double)gyro_offsets.x,
(double)gyro_offsets.y,
(double)gyro_offsets.z);
}
void SchedTest::loop(void)
{
// wait for an INS sample
ins.wait_for_sample();
// tell the scheduler one tick has passed
scheduler.tick();
// run all tasks that fit in 20ms
scheduler.run(20000);
}
static void run_calibration()
{
float roll_trim, pitch_trim;
// clear off any other characters (like line feeds,etc)
while( hal.console->available() ) {
hal.console->read();
}
AP_InertialSensor_UserInteractStream interact(hal.console);
ins.calibrate_accel(&interact, roll_trim, pitch_trim);
}
bool AP_Arming::ins_accels_consistent(const AP_InertialSensor &ins)
{
const uint8_t accel_count = ins.get_accel_count();
if (accel_count <= 1) {
return true;
}
const Vector3f &prime_accel_vec = ins.get_accel();
const uint32_t now = AP_HAL::millis();
for(uint8_t i=0; i<accel_count; i++) {
if (!ins.use_accel(i)) {
continue;
}
// get next accel vector
const Vector3f &accel_vec = ins.get_accel(i);
Vector3f vec_diff = accel_vec - prime_accel_vec;
// allow for user-defined difference, typically 0.75 m/s/s. Has to pass in last 10 seconds
float threshold = accel_error_threshold;
if (i >= 2) {
/*
we allow for a higher threshold for IMU3 as it
runs at a different temperature to IMU1/IMU2,
and is not used for accel data in the EKF
*/
threshold *= 3;
}
// EKF is less sensitive to Z-axis error
vec_diff.z *= 0.5f;
if (vec_diff.length() <= threshold) {
last_accel_pass_ms[i] = now;
}
if (now - last_accel_pass_ms[i] > 10000) {
return false;
}
}
return true;
}
static void run_test()
{
Vector3f accel;
Vector3f gyro;
float length;
uint8_t counter = 0;
// 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();
accel = ins.get_accel();
gyro = ins.get_gyro();
length = accel.length();
if (counter++ % 50 == 0) {
// display results
hal.console->printf_P(PSTR("Accel X:%4.2f \t Y:%4.2f \t Z:%4.2f \t len:%4.2f \t Gyro X:%4.2f \t Y:%4.2f \t Z:%4.2f\n"),
accel.x, accel.y, accel.z, length, gyro.x, gyro.y, gyro.z);
}
}
// clear user input
while( hal.console->available() ) {
hal.console->read();
}
}
/* HMC5843 on an auxiliary bus of IMU driver */
AP_HMC5843_BusDriver_Auxiliary::AP_HMC5843_BusDriver_Auxiliary(AP_InertialSensor &ins, uint8_t backend_id,
uint8_t addr)
{
/*
* Only initialize members. Fails are handled by configure or while
* getting the semaphore
*/
_bus = ins.get_auxiliary_bus(backend_id);
if (!_bus) {
return;
}
_slave = _bus->request_next_slave(addr);
}
void setup(void)
{
ins.init(100);
ahrs.init();
serial_manager.init();
if( compass.init() ) {
hal.console->printf("Enabling compass\n");
ahrs.set_compass(&compass);
} else {
hal.console->printf("No compass detected\n");
}
gps.init(NULL, serial_manager);
}
static void display_offsets_and_scaling()
{
Vector3f accel_offsets = ins.get_accel_offsets();
Vector3f accel_scale = ins.get_accel_scale();
Vector3f gyro_offsets = ins.get_gyro_offsets();
// display results
hal.console->printf_P(
PSTR("\nAccel Offsets X:%10.8f \t Y:%10.8f \t Z:%10.8f\n"),
accel_offsets.x,
accel_offsets.y,
accel_offsets.z);
hal.console->printf_P(
PSTR("Accel Scale X:%10.8f \t Y:%10.8f \t Z:%10.8f\n"),
accel_scale.x,
accel_scale.y,
accel_scale.z);
hal.console->printf_P(
PSTR("Gyro Offsets X:%10.8f \t Y:%10.8f \t Z:%10.8f\n"),
gyro_offsets.x,
gyro_offsets.y,
gyro_offsets.z);
}
void setup(void)
{
hal.console->println("AP_InertialSensor startup...");
#if CONFIG_HAL_BOARD == HAL_BOARD_APM2 || CONFIG_HAL_BOARD == HAL_BOARD_MEGA2560
// we need to stop the barometer from holding the SPI bus
hal.gpio->pinMode(40, HAL_GPIO_OUTPUT);
hal.gpio->write(40, 1);
#endif
ins.init(AP_InertialSensor::COLD_START,
AP_InertialSensor::RATE_100HZ);
// display initial values
display_offsets_and_scaling();
hal.console->println("Complete. Reading:");
}
void ReplayVehicle::setup(void) {
dataflash.Init(log_structure, sizeof(log_structure)/sizeof(log_structure[0]));
dataflash.StartNewLog();
ahrs.set_compass(&compass);
ahrs.set_fly_forward(true);
ahrs.set_wind_estimation(true);
ahrs.set_correct_centrifugal(true);
ahrs.set_ekf_use(true);
printf("Starting disarmed\n");
hal.util->set_soft_armed(false);
barometer.init();
barometer.setHIL(0);
barometer.update();
compass.init();
ins.set_hil_mode();
}
void setup(void)
{
#ifdef APM2_HARDWARE
// we need to stop the barometer from holding the SPI bus
hal.gpio->pinMode(40, HAL_HAL_GPIO_OUTPUT);
hal.gpio->write(40, HIGH);
#endif
ins.init(AP_InertialSensor::RATE_100HZ);
ahrs.init();
serial_manager.init();
if( compass.init() ) {
hal.console->printf("Enabling compass\n");
ahrs.set_compass(&compass);
} else {
hal.console->printf("No compass detected\n");
}
gps.init(NULL, serial_manager);
}
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
}
/*
update inertial sensor, reading data
*/
void SchedTest::ins_update(void)
{
ins_counter++;
ins.update();
}
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();
}
}
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);
}
