static void set_status_led_pin(bool is_on)
{
if (is_on)
debug_P(PSTR("STATUS LED ON @ %lu\n"), get_current_time());
else
debug_P(PSTR("STATUS LED OFF @ %lu\n"), get_current_time());
status_led_state.current_pin_state = is_on;
SET_STATUS_LED(is_on);
}
static void set_motor_direction(MotorDirection_t direction)
{
motor_direction = direction;
motor_stopped_count = 0;
motor_forward_count = 0;
motor_reverse_count = 0;
#if _DEBUG
debug_P(PSTR("MOT DIR SET "));
print_motor_direction(direction);
debug_P(PSTR(" @ %lu\n"), get_current_time());
#endif
}
void sram_bulk_set(uint32_t addr, uint32_t len, uint8_t value)
{
uint32_t i;
debug_P(DEBUG_SRAM, PSTR("sram_bulk_set: addr=0x%08lx len=%li\n\r"), addr,
len);
sram_bulk_write_start(addr);
for (i = addr; i < (addr + len); i++) {
if (0 == i % 0xfff)
debug_P(DEBUG_SRAM, PSTR("sram_bulk_set: addr=0x%08lx\n\r"), i);
sram_bulk_write(value);
sram_bulk_write_next();
}
sram_bulk_write_end();
}
void sram_bulk_write_end(void)
{
debug_P(DEBUG_SRAM, PSTR("sram_bulk_write_end:"));
AVR_WR_PORT |= (1 << AVR_WR_PIN);
AVR_CS_PORT |= (1 << AVR_CS_PIN);
avr_data_in();
}
void sram_write(uint32_t addr, uint8_t data)
{
debug_P(DEBUG_SRAM_RAW, PSTR("sram_write: addr=0x%08lx data=%x\n\r"), addr,
data);
avr_data_out();
AVR_CS_PORT &= ~(1 << AVR_CS_PIN);
AVR_WR_PORT |= (1 << AVR_WR_PIN);
AVR_RD_PORT |= (1 << AVR_RD_PIN);
sreg_set(addr);
AVR_WR_PORT &= ~(1 << AVR_WR_PIN);
AVR_DATA_PORT = data;
AVR_WR_PORT |= (1 << AVR_WR_PIN);
asm volatile ("nop");
asm volatile ("nop");
asm volatile ("nop");
asm volatile ("nop");
asm volatile ("nop");
asm volatile ("nop");
AVR_CS_PORT |= (1 << AVR_CS_PIN);
avr_data_in();
}
uint8_t usbFunctionWrite(uint8_t * data, uint8_t len)
{
uint8_t *ptr;
uint8_t i;
if (len > usb_trans.rx_remaining) {
info_P(PSTR
("ERROR:usbFunctionWrite more data than expected remain: %i len: %i\n"),
usb_trans.rx_remaining, len);
len = usb_trans.rx_remaining;
}
if (usb_trans.req_state == REQ_STATUS_BULK_UPLOAD) {
usb_trans.rx_remaining -= len;
debug_P(DEBUG_USB_TRANS,
PSTR
("usbFunctionWrite REQ_STATUS_BULK_UPLOAD addr: 0x%08lx len: %i rx_remaining=%i\n"),
usb_trans.req_addr, len, usb_trans.rx_remaining);
ptr = data;
i = len;
while (i--) {
sram_bulk_write(*ptr++);
sram_bulk_write_next();
}
}
return len;
}
static void start_brake_coasting(void)
{
motor_coast_timestamp = get_current_time();
set_speed_motor_pwm_level(0);
debug_P(PSTR("Start Brake Coasting @ %lu\n"), get_current_time());
motor_state = MOTOR_STATE_BRAKE_COAST;
}
static void start_braking(void)
{
motor_brake_timestamp = get_current_time();
set_speed_motor_pwm_level(MOTOR_LEVEL_BRAKE);
debug_P(PSTR("Start Braking @ %lu\n"), get_current_time());
motor_state = MOTOR_STATE_BRAKING;
}
uint8_t sram_read(uint32_t addr)
{
uint8_t byte;
debug_P(DEBUG_SRAM_RAW, PSTR("sram_read: addr=0x%08lx\n\r"), addr);
avr_data_in();
AVR_CS_PORT &= ~(1 << AVR_CS_PIN);
AVR_WR_PORT |= (1 << AVR_WR_PIN);
AVR_RD_PORT |= (1 << AVR_RD_PIN);
sreg_set(addr);
AVR_RD_PORT &= ~(1 << AVR_RD_PIN);
asm volatile ("nop");
asm volatile ("nop");
asm volatile ("nop");
asm volatile ("nop");
asm volatile ("nop");
asm volatile ("nop");
byte = AVR_DATA_PIN;
AVR_RD_PORT |= (1 << AVR_RD_PIN);
AVR_CS_PORT |= (1 << AVR_CS_PIN);
avr_data_in();
return byte;
}
void sram_bulk_read_end(void)
{
debug_P(DEBUG_SRAM, PSTR("sram_bulk_read_end:\n"));
AVR_RD_PORT |= (1 << AVR_RD_PIN);
AVR_CS_PORT |= (1 << AVR_CS_PIN);
avr_data_in();
}
static void print_motor_direction(MotorDirection_t direction)
{
switch (direction)
{
case MOTOR_DIRECTION_FORWARD:
debug_P(PSTR("FWD"));
break;
case MOTOR_DIRECTION_REVERSE:
debug_P(PSTR("REV"));
break;
case MOTOR_DIRECTION_STOPPED:
debug_P(PSTR("STOP"));
break;
case MOTOR_DIRECTION_UNKNOWN:
debug_P(PSTR("???"));
break;
}
}
void sreg_set(uint32_t addr)
{
uint8_t i = 24;
debug_P(DEBUG_SREG, PSTR("sreg_set: addr=0x%08lx"), addr);
while (i--) {
if ((addr & (1L << i))) {
debug_P(DEBUG_SREG, PSTR("1"));
AVR_ADDR_SER_PORT |= (1 << AVR_ADDR_SER_PIN);
} else {
AVR_ADDR_SER_PORT &= ~(1 << AVR_ADDR_SER_PIN);
debug_P(DEBUG_SREG, PSTR("0"));
}
AVR_ADDR_SCK_PORT |= (1 << AVR_ADDR_SCK_PIN);
AVR_ADDR_SCK_PORT &= ~(1 << AVR_ADDR_SCK_PIN);
}
debug_P(DEBUG_SREG, PSTR("\n"));
AVR_ADDR_LATCH_PORT |= (1 << AVR_ADDR_LATCH_PIN);
AVR_ADDR_LATCH_PORT &= ~(1 << AVR_ADDR_LATCH_PIN);
counter_load();
}
void sram_bulk_write_start(uint32_t addr)
{
debug_P(DEBUG_SRAM, PSTR("sram_bulk_write_start: addr=0x%08lx\n\r"), addr);
addr_current = addr;
avr_data_out();
AVR_CS_PORT &= ~(1 << AVR_CS_PIN);
AVR_WR_PORT |= (1 << AVR_WR_PIN);
AVR_RD_PORT |= (1 << AVR_RD_PIN);
sreg_set(addr);
}
uint8_t usbFunctionRead(uint8_t * data, uint8_t len)
{
uint8_t i;
if (len > usb_trans.tx_remaining)
len = usb_trans.tx_remaining;
usb_trans.tx_remaining -= len;
debug_P(DEBUG_USB_TRANS, PSTR("usbFunctionRead len=%i tx_remaining=%i \n"),
len, usb_trans.tx_remaining);
for (i = 0; i < len; i++) {
*data = usb_trans.tx_buffer[len];
data++;
}
return len;
}
void sram_bulk_copy_into_buffer(uint32_t addr, uint8_t * dst, uint32_t len)
{
uint32_t i;
// uint8_t *ptr = dst;
debug_P(DEBUG_SRAM,
PSTR
("sram_bulk_copy_into_buffer: addr=0x%08lx dst=0x%p len=%li\n\r"),
addr, dst, len);
sram_bulk_read_start(addr);
for (i = addr; i < (addr + len); i++) {
dst[i] = sram_bulk_read();
sram_bulk_read_next();
}
sram_bulk_read_end();
}
void sram_bulk_copy_from_buffer(uint32_t addr, uint8_t * src, uint32_t len)
{
uint32_t i;
uint8_t *ptr = src;
debug_P(DEBUG_SRAM,
PSTR
("sram_bulk_copy_from_buffer: addr=0x%08lx src=0x%p len=%li\n\r"),
addr, src, len);
sram_bulk_write_start(addr);
for (i = addr; i < (addr + len); i++) {
sram_bulk_write(*ptr);
// hack
if ((i + 1) < (addr + len))
sram_bulk_write_next();
ptr++;
}
sram_bulk_write_end();
}
void sram_bulk_read_start(uint32_t addr)
{
debug_P(DEBUG_SRAM, PSTR("sram_bulk_read_start: addr=0x%08lx\n\r"), addr);
addr_current = addr;
avr_data_in();
AVR_CS_PORT &= ~(1 << AVR_CS_PIN);
AVR_WR_PORT |= (1 << AVR_WR_PIN);
AVR_RD_PORT |= (1 << AVR_RD_PIN);
sreg_set(addr);
AVR_RD_PORT &= ~(1 << AVR_RD_PIN);
asm volatile ("nop");
asm volatile ("nop");
asm volatile ("nop");
asm volatile ("nop");
asm volatile ("nop");
asm volatile ("nop");
}
usbMsgLen_t usbFunctionSetup(uchar data[8])
{
usbRequest_t *rq = (void *) data;
uint8_t ret_len = 0;
if (rq->bRequest == USB_BULK_UPLOAD_INIT) {
usb_trans.req_bank = 0;
usb_trans.rx_remaining = 0;
debug_P(DEBUG_USB, PSTR("USB_BULK_UPLOAD_INIT: %i %i\n"),
rq->wValue.word, rq->wIndex.word);
usb_trans.req_bank_size = (uint32_t) (1L << rq->wValue.word);
usb_trans.req_bank_cnt = rq->wIndex.word;
usb_trans.req_addr_end =
(uint32_t) usb_trans.req_bank_size * usb_trans.req_bank_cnt;
usb_trans.req_percent = 0;
usb_trans.req_percent_last = 0;
usb_trans.sync_errors = 0;
debug_P(DEBUG_USB,
PSTR
("USB_BULK_UPLOAD_INIT: bank_size=0x%08lx bank_cnt=0x%x end_addr=0x%08lx\n"),
usb_trans.req_bank_size, usb_trans.req_bank_cnt,
usb_trans.req_addr_end);
shared_memory_write(SHARED_MEM_TX_CMD_UPLOAD_START, 0);
shared_memory_write(SHARED_MEM_TX_CMD_BANK_COUNT,
usb_trans.req_bank_cnt);
#if DO_TIMER
if (usb_trans.req_addr == 0x000000) {
#ifndef NO_DEBUG
timer_start();
#endif
}
#endif
/*
* -------------------------------------------------------------------------
*/
} else if (rq->bRequest == USB_BULK_UPLOAD_ADDR) {
usb_trans.req_state = REQ_STATUS_BULK_UPLOAD;
usb_trans.req_addr = rq->wValue.word;
usb_trans.req_addr = usb_trans.req_addr << 16;
usb_trans.req_addr = usb_trans.req_addr | rq->wIndex.word;
usb_trans.rx_remaining = rq->wLength.word;
if (usb_trans.req_addr
&& usb_trans.req_addr % usb_trans.req_bank_size == 0) {
#if DO_TIMER
#ifndef NO_DEBUG
#ifdef FLT_DEBUG
debug_P(DEBUG_USB,
PSTR
("USB_BULK_UPLOAD_ADDR: req_bank=0x%02x addr=0x%08lx time=%.4f\n"),
usb_trans.req_bank, usb_trans.req_addr, timer_stop());
#else
debug_P(DEBUG_USB,
PSTR
("USB_BULK_UPLOAD_ADDR: req_bank=0x%02x addr=0x%08lx time=%i\n"),
usb_trans.req_bank, usb_trans.req_addr, timer_stop_int());
#endif
timer_start();
#endif
#endif
usb_trans.req_bank++;
} else {
sram_bulk_write_start(usb_trans.req_addr);
}
ret_len = USB_MAX_TRANS;
/*
* -------------------------------------------------------------------------
*/
} else if (rq->bRequest == USB_BULK_UPLOAD_NEXT) {
usb_trans.req_state = REQ_STATUS_BULK_UPLOAD;
usb_trans.req_addr = rq->wValue.word;
usb_trans.req_addr = usb_trans.req_addr << 16;
usb_trans.req_addr = usb_trans.req_addr | rq->wIndex.word;
usb_trans.rx_remaining = rq->wLength.word;
#if DO_SHM
usb_trans.req_percent =
(uint32_t) (100 * usb_trans.req_addr) / usb_trans.req_addr_end;
if (usb_trans.req_percent != usb_trans.req_percent_last) {
shared_memory_write(SHARED_MEM_TX_CMD_UPLOAD_PROGESS,
usb_trans.req_percent);
}
usb_trans.req_percent_last = usb_trans.req_percent;
shared_memory_scratchpad_region_save_helper(usb_trans.req_addr);
#endif
if (usb_trans.req_addr
&& (usb_trans.req_addr % usb_trans.req_bank_size) == 0) {
#if DO_TIMER
#ifndef NO_DEBUG
#ifdef FLT_DEBUG
debug_P(DEBUG_USB,
PSTR
("USB_BULK_UPLOAD_NEXT: req_bank=0x%02x addr=0x%08lx time=%.4f\n"),
usb_trans.req_bank, usb_trans.req_addr, timer_stop());
#else
debug_P(DEBUG_USB,
PSTR
("USB_BULK_UPLOAD_NEXT: req_bank=0x%02x addr=0x%08lx time=%i\n"),
usb_trans.req_bank, usb_trans.req_addr, timer_stop_int());
#endif
timer_start();
#endif
#endif
usb_trans.req_bank++;
#if DO_SHM
shared_memory_write(SHARED_MEM_TX_CMD_BANK_CURRENT,
usb_trans.req_bank);
#endif
}
ret_len = USB_MAX_TRANS;
/*
* -------------------------------------------------------------------------
*/
} else if (rq->bRequest == USB_BULK_UPLOAD_END) {
debug_P(DEBUG_USB, PSTR("USB_BULK_UPLOAD_END:\n"));
usb_trans.req_state = REQ_STATUS_IDLE;
sram_bulk_write_end();
#if DO_SHM
shared_memory_write(SHARED_MEM_TX_CMD_UPLOAD_END, 0);
#endif
ret_len = 0;
/*
* -------------------------------------------------------------------------
*/
} else if (rq->bRequest == USB_CRC) {
usb_trans.req_addr = rq->wValue.word;
usb_trans.req_addr = usb_trans.req_addr << 16;
usb_trans.req_addr = usb_trans.req_addr | rq->wIndex.word;
debug_P(DEBUG_USB, PSTR("USB_CRC: addr=0x%08lx \n"),
usb_trans.req_addr);
crc_check_bulk_memory(0x000000, usb_trans.req_addr,
usb_trans.req_bank_size);
ret_len = 0;
/*
* -------------------------------------------------------------------------
*/
} else if (rq->bRequest == USB_MODE_SNES) {
usb_trans.req_state = REQ_STATUS_SNES;
debug_P(DEBUG_USB, PSTR("USB_MODE_SNES:\n"));
ret_len = 0;
/*
* -------------------------------------------------------------------------
*/
} else if (rq->bRequest == USB_MODE_AVR) {
usb_trans.req_state = REQ_STATUS_AVR;
debug_P(DEBUG_USB, PSTR("USB_MODE_AVR:\n"));
ret_len = 0;
/*
* -------------------------------------------------------------------------
*/
} else if (rq->bRequest == USB_AVR_RESET) {
debug_P(DEBUG_USB, PSTR("USB_AVR_RESET:\n"));
soft_reset();
ret_len = 0;
/*
* -------------------------------------------------------------------------
*/
} else if (rq->bRequest == USB_SET_LAODER) {
debug_P(DEBUG_USB, PSTR("USB_SET_LAODER:\n"));
usb_trans.loader_enabled = rq->wValue.word;
ret_len = 0;
}
usbMsgPtr = usb_trans.rx_buffer;
return ret_len;
}
static void handle_back_emf_measurement()
{
uint16_t emf_fwd = get_sensor_adc_counts(SENSOR_MOTOR_EMF_FWD);
uint16_t emf_rev = get_sensor_adc_counts(SENSOR_MOTOR_EMF_REV);
#if _DEBUG
MotorDirection_t last_motor_direction = motor_direction;
#endif
if (emf_fwd < EMF_THRESHOLD)
{
if (motor_forward_count < EMF_DEMOD_COUNT)
motor_forward_count++;
}
else
{
motor_forward_count = 0;
if (motor_direction == MOTOR_DIRECTION_REVERSE)
{
#if _DEBUG
if (last_motor_direction != motor_direction)
debug_P(PSTR("MOTOR READ ??? @ %lu\n"), get_current_time());
#endif
motor_direction = MOTOR_DIRECTION_UNKNOWN;
}
}
if (emf_rev < EMF_THRESHOLD)
{
if (motor_reverse_count < EMF_DEMOD_COUNT)
motor_reverse_count++;
}
else
{
motor_reverse_count = 0;
if (motor_direction == MOTOR_DIRECTION_FORWARD)
{
#if _DEBUG
if (last_motor_direction != motor_direction)
debug_P(PSTR("MOTOR READ ??? @ %lu\n"), get_current_time());
#endif
motor_direction = MOTOR_DIRECTION_UNKNOWN;
}
}
if (motor_forward_count == EMF_DEMOD_COUNT)
{
if (motor_reverse_count == EMF_DEMOD_COUNT)
{
motor_direction = MOTOR_DIRECTION_STOPPED;
#if _DEBUG
if (last_motor_direction != motor_direction)
debug_P(PSTR("MOTOR READ STOP @ %lu\n"), get_current_time());
#endif
}
else
{
motor_direction = MOTOR_DIRECTION_REVERSE;
#if _DEBUG
if (last_motor_direction != motor_direction)
debug_P(PSTR("MOTOR READ REVERSE @ %lu\n"), get_current_time());
#endif
}
}
else
{
if (motor_reverse_count == EMF_DEMOD_COUNT)
{
motor_direction = MOTOR_DIRECTION_FORWARD;
#if _DEBUG
if (last_motor_direction != motor_direction)
debug_P(PSTR("MOTOR READ FWD @ %lu\n"), get_current_time());
#endif
}
}
}
void motor_run_handler(uint32_t current_time)
{
if (get_nvm_error_flag() != ERR_NONE)
return;
switch (motor_state)
{
case MOTOR_STATE_IDLE:
if (!get_sensor_waiting_for_measurement())
{
handle_back_emf_measurement();
set_sensor_waiting_for_measurement(true);
if (motor_direction == MOTOR_DIRECTION_STOPPED)
{
if (get_last_error(NULL) == ERR_MOTOR_FAULT)
clear_last_error();
}
else
{
post_error(ERR_MOTOR_FAULT);
}
}
break;
case MOTOR_STATE_BRAKING:
if (check_for_timeout(current_time, motor_brake_timestamp,
MOTOR_BRAKE_TIME))
{
start_brake_coasting();
}
break;
case MOTOR_STATE_BRAKE_COAST:
if (check_for_timeout(current_time, motor_coast_timestamp,
MOTOR_COAST_TIME))
{
debug_P(PSTR("Stop brake coasting @ %lu\n"), current_time);
set_sensor_waiting_for_measurement(true);
motor_state = MOTOR_STATE_BRAKE_READ_BACK_EMF;
motor_read_back_emf_count = 0;
}
break;
case MOTOR_STATE_BRAKE_READ_BACK_EMF:
if (!get_sensor_waiting_for_measurement())
{
handle_back_emf_measurement();
// if we have receive a brake command we just keep braking
if (motor_levels.speed_channel_level == MOTOR_LEVEL_BRAKE)
{
if (motor_direction == MOTOR_DIRECTION_STOPPED)
{
if (get_last_error(NULL) == ERR_MOTOR_FAULT)
clear_last_error();
}
else
{
post_error(ERR_MOTOR_FAULT);
}
}
else if (motor_levels.speed_channel_level > 0)
{
if ((motor_direction == MOTOR_DIRECTION_STOPPED)
|| (motor_direction == MOTOR_DIRECTION_FORWARD))
{
motor_state = MOTOR_STATE_ACTIVE;
set_motor_direction(MOTOR_DIRECTION_FORWARD);
}
}
else if (motor_levels.speed_channel_level < 0)
{
if ((motor_direction == MOTOR_DIRECTION_STOPPED)
|| (motor_direction == MOTOR_DIRECTION_REVERSE))
{
motor_state = MOTOR_STATE_ACTIVE;
set_motor_direction(MOTOR_DIRECTION_REVERSE);
}
}
else if (motor_direction == MOTOR_DIRECTION_STOPPED)
{
motor_state = MOTOR_STATE_IDLE;
set_motor_direction(MOTOR_DIRECTION_STOPPED);
}
// motor fault is done
if (motor_state != MOTOR_STATE_BRAKE_READ_BACK_EMF)
{
if (get_last_error(NULL) == ERR_MOTOR_FAULT)
clear_last_error();
}
else if (++motor_read_back_emf_count == BACK_EMF_READ_COUNT)
{
start_braking();
}
else
{
set_sensor_waiting_for_measurement(true);
}
}
break;
case MOTOR_STATE_COASTING:
if (check_for_timeout(current_time, motor_coast_timestamp,
MOTOR_COAST_TIME))
{
set_sensor_waiting_for_measurement(true);
motor_state = MOTOR_STATE_IDLE;
}
break;
case MOTOR_STATE_ACTIVE:
if (check_for_timeout(current_time, motor_timestamp, motor_timeout))
{
motor_levels.speed_channel_level = 0;
start_braking();
debug_P(PSTR("MOTOR Timeout @ %lu\n"), get_current_time());
post_error(ERR_MOTOR_TIMEOUT);
}
else
{
set_speed_motor_pwm_level(motor_levels.speed_channel_level);
set_direction_motor_pwm_level(motor_levels.direction_channel_level);
}
break;
}
}
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_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 sram_bulk_addr_save()
{
addr_stash = addr_current;
debug_P(DEBUG_SRAM, PSTR("sram_bulk_addr_save: addr=0x%08lx\n\r"),
addr_stash);
}
inline void sram_bulk_addr_restore()
{
debug_P(DEBUG_SRAM, PSTR("sram_bulk_addr_restore: addr=0x%08lx\n\r"),
addr_stash);
sram_bulk_write_start(addr_stash);
}
