void
AP_IMU_INS::_init_gyro(void (*delay_cb)(unsigned long t), void (*flash_leds_cb)(bool on))
{
Vector3f last_average, best_avg;
float ins_gyro[3];
float best_diff = 0;
// cold start
delay_cb(100);
debug_P(PSTR("Init Gyro"));
for(int c = 0; c < 75; c++)
{
// Mostly we are just flashing the LED's here
// to tell the user to keep the IMU still
FLASH_LEDS(true);
delay_cb(20);
_ins->update();
//_ins->get_gyros(ins_gyro);
FLASH_LEDS(false);
delay_cb(20);
}
// the strategy is to average 200 points over 1 second, then do it
// again and see if the 2nd average is within a small margin of
// the first
last_average.zero();
// we try to get a good calibration estimate for up to 10 seconds
// if the gyros are stable, we should get it in 2 seconds
for (int j = 0; j <= 10; j++)
{
Vector3f gyro_sum, gyro_avg, gyro_diff;
float diff_norm;
uint8_t i;
debug_P(PSTR("*"));
gyro_sum.zero();
for (i = 0; i < 200; i++)
{
_ins->update();
//_ins->get_gyros(ins_gyro);
gyro_sum.add(_ins->gyro[0], _ins->gyro[1], _ins->gyro[2]);
if (i % 40 == 20)
{
FLASH_LEDS(true);
}
else if (i % 40 == 0)
{
FLASH_LEDS(false);
}
delay_cb(5);
}
gyro_avg = gyro_sum / i;
gyro_diff = last_average - gyro_avg;
diff_norm = gyro_diff.length();
if (j == 0)
{
best_diff = diff_norm;
best_avg = gyro_avg;
}
else if (gyro_diff.length() < ToRad(0.02))
{
// we want the average to be within 0.1 bit, which is 0.04 degrees/s
last_average = (gyro_avg * 0.5) + (last_average * 0.5);
_sensor_cal[0] = last_average.x;
_sensor_cal[1] = last_average.y;
_sensor_cal[2] = last_average.z;
// all done
return;
}
else if (diff_norm < best_diff)
{
best_diff = diff_norm;
best_avg = (gyro_avg * 0.5) + (last_average * 0.5);
}
last_average = gyro_avg;
}
// we've kept the user waiting long enough - use the best pair we
// found so far
debug_P(PSTR("\ngyro did not converge: diff=%f dps\n"), ToDeg(best_diff));
_sensor_cal[0] = best_avg.x;
_sensor_cal[1] = best_avg.y;
_sensor_cal[2] = best_avg.z;
}
void
AP_IMU_INS::_init_accel(void (*delay_cb)(unsigned long t), void (*flash_leds_cb)(bool on))
{
int flashcount = 0;
float adc_in;
float prev[6] = {0,0,0,0,0,0};
float total_change;
float max_offset;
//float* ins_accel = ins->accel;
// cold start
delay_cb(500);
debug_P(PSTR("Init Accel"));
for (int j=3; j<=5; j++) _sensor_cal[j] = 500; // Just a large value to load prev[j] the first time
do
{
_ins->update();
//_ins->get_accels(ins_accel);
for (int j = 3; j <= 5; j++)
{
prev[j] = _sensor_cal[j];
adc_in = _ins->accel[j-3];
_sensor_cal[j] = adc_in;
}
for(int i = 0; i < 50; i++) // We take some readings...
{
delay_cb(20);
_ins->update();
//_ins->get_accels(ins_accel);
for (int j = 3; j < 6; j++)
{
adc_in = _ins->accel[j-3];
_sensor_cal[j] = _sensor_cal[j] * 0.9 + adc_in * 0.1;
}
if(flashcount == 5)
{
debug_P(PSTR("*"));
FLASH_LEDS(true);
}
if(flashcount >= 10)
{
flashcount = 0;
FLASH_LEDS(false);
}
flashcount++;
}
// null gravity from the Z accel
_sensor_cal[5] += 9.805;
//_sensor_cal[5] -= 9.805;
total_change = fabs(prev[3] - _sensor_cal[3]) + fabs(prev[4] - _sensor_cal[4]) +fabs(prev[5] - _sensor_cal[5]);
max_offset = (_sensor_cal[3] > _sensor_cal[4]) ? _sensor_cal[3] : _sensor_cal[4];
max_offset = (max_offset > _sensor_cal[5]) ? max_offset : _sensor_cal[5];
delay_cb(500);
}
while ( total_change > _accel_total_cal_change || max_offset > _accel_max_cal_offset);
debug_P(PSTR(" "));
}
void
AP_InertialSensor::_init_accel(void (*delay_cb)(unsigned long t), void (*flash_leds_cb)(bool on))
{
int8_t flashcount = 0;
Vector3f ins_accel;
Vector3f prev;
Vector3f accel_offset;
float total_change;
float max_offset;
// cold start
delay_cb(100);
if (_serial)
_serial->printf_P(PSTR("Init Accel"));
// clear accelerometer offsets and scaling
_accel_offset = Vector3f(0,0,0);
_accel_scale = Vector3f(1,1,1);
// initialise accel offsets to a large value the first time
// this will force us to calibrate accels at least twice
accel_offset = Vector3f(500, 500, 500);
// loop until we calculate acceptable offsets
do {
// get latest accelerometer values
read();
update();
ins_accel = get_accel();
// store old offsets
prev = accel_offset;
// get new offsets
accel_offset = ins_accel;
// We take some readings...
for(int8_t i = 0; i < 50; i++) {
delay_cb(20);
read();
update();
ins_accel = get_accel();
// low pass filter the offsets
accel_offset = accel_offset * 0.9 + ins_accel * 0.1;
// display some output to the user
if(flashcount == 5) {
_serial->printf_P(PSTR("*"));
FLASH_LEDS(true);
}
if(flashcount >= 10) {
flashcount = 0;
FLASH_LEDS(false);
}
flashcount++;
}
// null gravity from the Z accel
// TO-DO: replace with gravity #define form location.cpp
accel_offset.z += GRAVITY;
total_change = fabsf(prev.x - accel_offset.x) + fabsf(prev.y - accel_offset.y) + fabsf(prev.z - accel_offset.z);
max_offset = (accel_offset.x > accel_offset.y) ? accel_offset.x : accel_offset.y;
max_offset = (max_offset > accel_offset.z) ? max_offset : accel_offset.z;
delay_cb(500);
} while ( total_change > AP_INERTIAL_SENSOR_ACCEL_TOT_MAX_OFFSET_CHANGE || max_offset > AP_INERTIAL_SENSOR_ACCEL_MAX_OFFSET);
// set the global accel offsets
_accel_offset = accel_offset;
if (_serial)
_serial->printf_P(PSTR(" "));
}
void
AP_InertialSensor::_init_gyro(void (*delay_cb)(unsigned long t), void (*flash_leds_cb)(bool on))
{
Vector3f last_average, best_avg;
Vector3f ins_gyro;
float best_diff = 0;
// cold start
delay_cb(100);
if (_serial)
_serial->printf_P(PSTR("Init Gyro"));
// remove existing gyro offsets
_gyro_offset = Vector3f(0,0,0);
for(int16_t c = 0; c < 25; c++) {
// Mostly we are just flashing the LED's here
// to tell the user to keep the IMU still
FLASH_LEDS(true);
delay_cb(20);
read();
update();
ins_gyro = get_gyro();
FLASH_LEDS(false);
delay_cb(20);
}
// the strategy is to average 200 points over 1 second, then do it
// again and see if the 2nd average is within a small margin of
// the first
last_average.zero();
// we try to get a good calibration estimate for up to 10 seconds
// if the gyros are stable, we should get it in 2 seconds
int num_attempts = 10;
float target_average = 0.04;
#ifdef INS_VRIMUFULL
num_attempts = 5;
target_average = 0.1
#endif
for (int16_t j = 0; j <= num_attempts; j++) {
Vector3f gyro_sum, gyro_avg, gyro_diff;
float diff_norm;
uint8_t i;
if (_serial)
_serial->printf_P(PSTR("*"));
gyro_sum.zero();
for (i=0; i<200; i++) {
//for (i=0; i<10; i++) {
for(int z = 0; z < 10; z++){
read();
if (_serial)
_serial->printf_P(PSTR("."));
}
update();
ins_gyro = get_gyro();
// if (_serial) {
// _serial->println();
// _serial->print(ins_gyro.x, 5); _serial->print(" ");
// _serial->print(ins_gyro.y, 5); _serial->print(" ");
// _serial->print(ins_gyro.z, 5); _serial->print(" ");
//
// }
gyro_sum += ins_gyro;
if (i % 40 == 20) {
FLASH_LEDS(true);
} else if (i % 40 == 0) {
FLASH_LEDS(false);
}
delay_cb(5);
}
gyro_avg = gyro_sum / i;
gyro_diff = last_average - gyro_avg;
diff_norm = gyro_diff.length();
if (j == 0) {
best_diff = diff_norm;
best_avg = gyro_avg;
} else if (gyro_diff.length() < ToRad(target_average)) {
// we want the average to be within 0.1 bit, which is 0.04 degrees/s
last_average = (gyro_avg * 0.5) + (last_average * 0.5);
_gyro_offset = last_average;
// all done
if (_serial) {
//_serial->printf_P(PSTR("\ngyro converged: diff=%f dps\n"), ToDeg(diff_norm));
_serial->printf_P(PSTR("\ngyro converged: diff="));
_serial->print(ToDeg(diff_norm), 5);
_serial->printf_P(PSTR(" dps\n"));
}
return;
} else if (diff_norm < best_diff) {
best_diff = diff_norm;
best_avg = (gyro_avg * 0.5) + (last_average * 0.5);
}
last_average = gyro_avg;
}
// we've kept the user waiting long enough - use the best pair we
// found so far
if (_serial)
_serial->printf_P(PSTR("\ngyro did not converge: diff=%f dps\n"), ToDeg(best_diff));
_gyro_offset = best_avg;
}
static void sh4_frontend_translate_code(struct jit_frontend *base,
uint32_t begin_addr, int size,
struct ir *ir) {
struct sh4_frontend *frontend = (struct sh4_frontend *)base;
struct sh4_guest *guest = (struct sh4_guest *)frontend->guest;
struct sh4_context *ctx = (struct sh4_context *)guest->ctx;
int offset = 0;
int use_fpscr = 0;
int was_delay = 0;
/* append inital block */
struct ir_block *block = ir_append_block(ir);
/* generate code specialized for the current fpscr state */
int flags = 0;
if (ctx->fpscr & PR_MASK) {
flags |= SH4_DOUBLE_PR;
}
if (ctx->fpscr & SZ_MASK) {
flags |= SH4_DOUBLE_SZ;
}
/* cheap idle skip. in an idle loop, the block is just spinning, waiting for
an interrupt such as vblank before it'll exit. scale the block's number of
cycles in order to yield execution faster, enabling the interrupt to
actually be generated */
int idle_loop = sh4_frontend_is_idle_loop(frontend, begin_addr);
int cycle_scale = idle_loop ? 8 : 1;
while (offset < size) {
/* if a branch instruction / delay slot was just emitted, rewind and emit
the delay slot as its own instruction */
if (was_delay) {
offset -= 2;
was_delay = 0;
}
uint32_t addr = begin_addr + offset;
uint16_t data = guest->r16(guest->mem, addr);
union sh4_instr instr = {data};
struct jit_opdef *def = sh4_get_opdef(data);
use_fpscr |= (def->flags & SH4_FLAG_USE_FPSCR) == SH4_FLAG_USE_FPSCR;
/* emit meta information for the current guest instruction. this info is
essential to the jit, and is used to map guest instructions to host
addresses for branching and fastmem access */
ir_source_info(ir, addr, def->cycles * cycle_scale);
/* the pc is normally only written to the context at the end of the block,
sync now for any instruction which needs to read the correct pc */
if (def->flags & SH4_FLAG_LOAD_PC) {
ir_store_context(ir, offsetof(struct sh4_context, pc),
ir_alloc_i32(ir, addr));
}
/* emit the instruction's translation if available */
sh4_translate_cb cb = sh4_get_translator(data);
if (cb) {
/* if the instruction has a delay slot, delay_point is assigned where the
slot's translation should be emitted */
struct ir_insert_point delay_point;
cb(guest, ir, addr, instr, flags, &delay_point);
offset += 2;
if (def->flags & SH4_FLAG_DELAYED) {
uint32_t delay_addr = begin_addr + offset;
uint32_t delay_data = guest->r16(guest->mem, delay_addr);
union sh4_instr delay_instr = {delay_data};
struct jit_opdef *delay_def = sh4_get_opdef(delay_data);
use_fpscr |=
(delay_def->flags & SH4_FLAG_USE_FPSCR) == SH4_FLAG_USE_FPSCR;
/* move insert point back to the middle of the preceding instruction */
struct ir_insert_point original = ir_get_insert_point(ir);
ir_set_insert_point(ir, &delay_point);
if (delay_def->flags & SH4_FLAG_LOAD_PC) {
ir_store_context(ir, offsetof(struct sh4_context, pc),
ir_alloc_i32(ir, delay_addr));
}
/* emit the delay slot's translation if available */
sh4_translate_cb delay_cb = sh4_get_translator(delay_data);
if (delay_cb) {
delay_cb(guest, ir, delay_addr, delay_instr, flags, NULL);
} else {
ir_fallback(ir, delay_def->fallback, delay_addr, delay_data);
}
/* restore insert point */
ir_set_insert_point(ir, &original);
offset += 2;
was_delay = 1;
}
} else {
ir_fallback(ir, def->fallback, addr, data);
offset += 2;
/* don't emit a fallback for the delay slot, the original fallback will
execute it */
if (def->flags & SH4_FLAG_DELAYED) {
offset += 2;
was_delay = 1;
}
}
/* there are 3 possible block endings:
1.) the block terminates due to an unconditional branch; nothing needs to
be done
2.) the block terminates due to an instruction which doesn't set the pc;
an unconditional branch to the next address needs to be added
3.) the block terminates due to an instruction which sets the pc but is
not a branch (e.g. an invalid instruction trap); nothing needs to be
done dispatch will always implicitly branch to the next pc */
int store_pc = (def->flags & SH4_FLAG_STORE_PC) == SH4_FLAG_STORE_PC;
int end_of_block = sh4_frontend_is_terminator(def) || offset >= size;
if (end_of_block) {
if (!store_pc) {
struct ir_block *tail_block =
list_last_entry(&ir->blocks, struct ir_block, it);
struct ir_instr *tail_instr =
list_last_entry(&tail_block->instrs, struct ir_instr, it);
ir_set_current_instr(ir, tail_instr);
uint32_t next_addr = begin_addr + offset;
ir_branch(ir, ir_alloc_i32(ir, next_addr));
}
}
}
/* if the block makes optimizations based on the fpscr state, assert that the
run-time fpscr state matches the compile-time state */
if (use_fpscr) {
/* insert after the first guest marker */
struct ir_instr *after = NULL;
list_for_each_entry(instr, &block->instrs, struct ir_instr, it) {
if (instr->op == OP_SOURCE_INFO) {
after = instr;
break;
}
}
ir_set_current_instr(ir, after);
struct ir_value *actual =
ir_load_context(ir, offsetof(struct sh4_context, fpscr), VALUE_I32);
actual = ir_and(ir, actual, ir_alloc_i32(ir, PR_MASK | SZ_MASK));
struct ir_value *expected =
ir_alloc_i32(ir, ctx->fpscr & (PR_MASK | SZ_MASK));
ir_assert_eq(ir, actual, expected);
}
}
// perform accelerometer calibration including providing user instructions and feedback
bool AP_InertialSensor::calibrate_accel(void (*delay_cb)(unsigned long t), void (*flash_leds_cb)(bool on), void (*send_msg)(const prog_char_t *, ...))
{
Vector3f samples[6];
Vector3f new_offsets;
Vector3f new_scaling;
Vector3f orig_offset;
Vector3f orig_scale;
// backup original offsets and scaling
orig_offset = _accel_offset.get();
orig_scale = _accel_scale.get();
// clear accelerometer offsets and scaling
_accel_offset = Vector3f(0,0,0);
_accel_scale = Vector3f(1,1,1);
// capture data from 6 positions
for (int8_t i=0; i<6; i++) {
const prog_char_t *msg;
// display message to user
switch ( i ) {
case 0:
msg = PSTR("level");
break;
case 1:
msg = PSTR("on it's left side");
break;
case 2:
msg = PSTR("on it's right side");
break;
case 3:
msg = PSTR("nose down");
break;
case 4:
msg = PSTR("nose up");
break;
case 5:
msg = PSTR("on it's back");
break;
}
if (send_msg == NULL) {
Serial.printf_P(PSTR("USER: Place APM %S and press any key.\n"), msg);
}else{
send_msg(PSTR("USER: Place APM %S and press any key.\n"), msg);
}
// wait for user input
while( !Serial.available() ) {
delay_cb(20);
}
// clear unput buffer
while( Serial.available() ) {
Serial.read();
}
// clear out any existing samples from ins
update();
// wait until we have 32 samples
while( num_samples_available() < 32 * SAMPLE_UNIT ) {
delay_cb(1);
}
// read samples from ins
update();
// capture sample
samples[i] = get_accel();
}
// run the calibration routine
if( _calibrate_accel(samples, new_offsets, new_scaling) ) {
if (send_msg == NULL) {
Serial.printf_P(PSTR("Calibration successful\n"));
}else{
send_msg(PSTR("Calibration successful\n"));
}
// set and save calibration
_accel_offset.set(new_offsets);
_accel_scale.set(new_scaling);
_save_parameters();
return true;
}
if (send_msg == NULL) {
Serial.printf_P(PSTR("Calibration failed (%.1f %.1f %.1f %.1f %.1f %.1f)\n"),
new_offsets.x, new_offsets.y, new_offsets.z,
new_scaling.x, new_scaling.y, new_scaling.z);
} else {
send_msg(PSTR("Calibration failed (%.1f %.1f %.1f %.1f %.1f %.1f)\n"),
new_offsets.x, new_offsets.y, new_offsets.z,
new_scaling.x, new_scaling.y, new_scaling.z);
}
// restore original scaling and offsets
_accel_offset.set(orig_offset);
_accel_scale.set(orig_scale);
return false;
}
