void init_accel(int cs2)
{
int i, tmpx, tmpy, tmpz;
float avrx, avry, avrz;
avrx = avry = avrz = 0;
//Initiate an SPI communication instance.
//Configure the SPI connection for the ADXL345.
digitalWrite(cs2, HIGH);
pinMode(cs2, OUTPUT);
//Before communication starts, the Chip Select pin needs to be set high.
CS2 = cs2;
//Put the ADXL345 into +/- 4G range by writing the value 0x01 to the DATA_FORMAT register.
writeRegister(DATA_FORMAT, 0x01);
//Put the ADXL345 into Measurement Mode by writing 0x08 to the POWER_CTL register.
writeRegister(POWER_CTL, 0x08); //Measurement mode
for (i = 0; i < OFFSET_NUM; i++) {
get_accel(&tmpx, &tmpy, &tmpz);
avrx += tmpx;
avry += tmpy;
avrz += tmpz;
}
offsetx = avrx / OFFSET_NUM;
offsety = avry / OFFSET_NUM;
offsetz = avrz / OFFSET_NUM;
}
// Once you're done, just type make and it should produce an executable
// called <name of source file>.out
int main()
{
// Initialize by connecting to the FPGA
init_fpga();
// Power on the system
power_on();
/* YOUR CODE GOES HERE */
sleep(2); // Pause for 2 seconds
int x, y, z, d; // Get acceleration vector and depth
get_accel(&x, &y, &z);
d = get_depth();
printf("Acceleration is: (%d, %d, %d)\n", x, y, z);
printf("Current depth is: %d\n", d);
int p = get_power(); // Check if power is still on
printf("Power system %s\n", p ? "is good": "has failed");
puts("Exiting...");
/* END */
// Power off and release resources
exit_safe();
return 0;
}
void measure_accel(float *x, float *y, float *z)
{
int tmpx, tmpy, tmpz;
get_accel(&tmpx, &tmpy, &tmpz);
*x = (float)(tmpx - offsetx) * MODE_4G; //-4g ～ +4gモードの場合
*y = (float)(tmpy - offsety) * MODE_4G; //-4g ～ +4gモードの場合
*z = (float)(tmpz - offsetz) * MODE_4G; //-4g ～ +4gモードの場合
}
/*
get delta velocity if available
*/
bool AP_InertialSensor::get_delta_velocity(uint8_t i, Vector3f &delta_velocity) const
{
if (_delta_velocity_valid[i]) {
delta_velocity = _delta_velocity[i];
return true;
} else if (get_accel_health(i)) {
delta_velocity = get_accel(i) * get_delta_time();
return true;
}
return false;
}
/*
calculate the trim_roll and trim_pitch. This is used for redoing the
trim without needing a full accel cal
*/
bool AP_InertialSensor::calibrate_trim(float &trim_roll, float &trim_pitch)
{
Vector3f level_sample;
// exit immediately if calibration is already in progress
if (_calibrating) {
return false;
}
_calibrating = true;
const uint8_t update_dt_milliseconds = (uint8_t)(1000.0f/get_sample_rate()+0.5f);
// wait 100ms for ins filter to rise
for (uint8_t k=0; k<100/update_dt_milliseconds; k++) {
wait_for_sample();
update();
hal.scheduler->delay(update_dt_milliseconds);
}
uint32_t num_samples = 0;
while (num_samples < 400/update_dt_milliseconds) {
wait_for_sample();
// read samples from ins
update();
// capture sample
Vector3f samp;
samp = get_accel(0);
level_sample += samp;
if (!get_accel_health(0)) {
goto failed;
}
hal.scheduler->delay(update_dt_milliseconds);
num_samples++;
}
level_sample /= num_samples;
if (!_calculate_trim(level_sample, trim_roll, trim_pitch)) {
goto failed;
}
_calibrating = false;
return true;
failed:
_calibrating = false;
return false;
}
CarControl
Stuck::drive(CarState &cs) {
++elapsed_ticks;
track_initial_pos = getInitialPos(cs);
if(notStuckAnymore(cs) || hasBeenStuckLongEnough()) {
elapsed_ticks = 0;
slow_speed_ticks = 0;
track_initial_pos = 0;
}
int gear = get_gear(cs);
float accel = get_accel(cs);
float brake = get_brake(cs);
float clutch = get_clutch(cs);
const int focus = 0, meta = 0;
float steer = auxSteer(track_initial_pos, cs);
return CarControl(accel, brake, gear, steer, clutch, focus, meta);
}
int main(int argc, char * argv[]) {
uint32_t feature_set;
fprintf(stdout,"Initialising librpip...\n");
feature_set = librpipInit(LIBRPIP_BOARD_DETECT, LIBRPIP_FLAG_DEBUG_ON, 0);
fprintf(stdout,"\n");
if(feature_set == 0) {
fprintf(stdout,"librpip failed to initialise!\n");
} else {
if(feature_set & LIBRPIP_FEATURE_I2C1) {
uint8_t client=0x68;
uint8_t i2c_bus=0;
float x,y,z;
setup_transactions();
librpipTransactionSend(init, i2c_bus, client);
while (1) {
get_accel(i2c_bus, client, &x, &y, &z);
fprintf(stdout,"x: %0.2f, y: %0.2f, z: %0.2f \n", x,y,z );
sleep(1);
}
librpipTransactionDestroy(init); //kind of pointless after an infinite loop but its a very good habit to destroy what you create
librpipTransactionDestroy(readAccel); //ditto
librpipTransactionDestroy(readGyro); //ditto
}
}
librpipClose();
return 0;
}
void
AP_InertialSensor::_init_gyro()
{
uint8_t num_gyros = MIN(get_gyro_count(), INS_MAX_INSTANCES);
Vector3f last_average[INS_MAX_INSTANCES], best_avg[INS_MAX_INSTANCES];
Vector3f new_gyro_offset[INS_MAX_INSTANCES];
float best_diff[INS_MAX_INSTANCES];
bool converged[INS_MAX_INSTANCES];
// exit immediately if calibration is already in progress
if (_calibrating) {
return;
}
// record we are calibrating
_calibrating = true;
// flash leds to tell user to keep the IMU still
AP_Notify::flags.initialising = true;
// cold start
hal.console->print("Init Gyro");
/*
we do the gyro calibration with no board rotation. This avoids
having to rotate readings during the calibration
*/
enum Rotation saved_orientation = _board_orientation;
_board_orientation = ROTATION_NONE;
// remove existing gyro offsets
for (uint8_t k=0; k<num_gyros; k++) {
_gyro_offset[k].set(Vector3f());
new_gyro_offset[k].zero();
best_diff[k] = 0;
last_average[k].zero();
converged[k] = false;
}
for(int8_t c = 0; c < 5; c++) {
hal.scheduler->delay(5);
update();
}
// the strategy is to average 50 points over 0.5 seconds, then do it
// again and see if the 2nd average is within a small margin of
// the first
uint8_t num_converged = 0;
// we try to get a good calibration estimate for up to 30 seconds
// if the gyros are stable, we should get it in 1 second
for (int16_t j = 0; j <= 30*4 && num_converged < num_gyros; j++) {
Vector3f gyro_sum[INS_MAX_INSTANCES], gyro_avg[INS_MAX_INSTANCES], gyro_diff[INS_MAX_INSTANCES];
Vector3f accel_start;
float diff_norm[INS_MAX_INSTANCES];
uint8_t i;
memset(diff_norm, 0, sizeof(diff_norm));
hal.console->print("*");
for (uint8_t k=0; k<num_gyros; k++) {
gyro_sum[k].zero();
}
accel_start = get_accel(0);
for (i=0; i<50; i++) {
update();
for (uint8_t k=0; k<num_gyros; k++) {
gyro_sum[k] += get_gyro(k);
}
hal.scheduler->delay(5);
}
Vector3f accel_diff = get_accel(0) - accel_start;
if (accel_diff.length() > 0.2f) {
// the accelerometers changed during the gyro sum. Skip
// this sample. This copes with doing gyro cal on a
// steadily moving platform. The value 0.2 corresponds
// with around 5 degrees/second of rotation.
continue;
}
for (uint8_t k=0; k<num_gyros; k++) {
gyro_avg[k] = gyro_sum[k] / i;
gyro_diff[k] = last_average[k] - gyro_avg[k];
diff_norm[k] = gyro_diff[k].length();
warnx("gyro[%d]: avg=%5.5f last_avg=%5.5f diff=%5.5f diff_norm=%5.5f\n", k, gyro_avg[k].length(), last_average[k].length(), gyro_diff[k].length(), diff_norm[k]);
}
for (uint8_t k=0; k<num_gyros; k++) {
if (j == 0) {
best_diff[k] = diff_norm[k];
best_avg[k] = gyro_avg[k];
} else if (gyro_diff[k].length() < ToRad(0.1f)) {
// we want the average to be within 0.1 bit, which is 0.04 degrees/s
last_average[k] = (gyro_avg[k] * 0.5f) + (last_average[k] * 0.5f);
if (!converged[k] || last_average[k].length() < new_gyro_offset[k].length()) {
new_gyro_offset[k] = last_average[k];
}
if (!converged[k]) {
converged[k] = true;
num_converged++;
}
} else if (diff_norm[k] < best_diff[k]) {
best_diff[k] = diff_norm[k];
best_avg[k] = (gyro_avg[k] * 0.5f) + (last_average[k] * 0.5f);
}
last_average[k] = gyro_avg[k];
}
}
// we've kept the user waiting long enough - use the best pair we
// found so far
hal.console->println();
for (uint8_t k=0; k<num_gyros; k++) {
if (!converged[k]) {
hal.console->printf("gyro[%u] did not converge: diff=%f dps\n",
(unsigned)k, (double)ToDeg(best_diff[k]));
_gyro_offset[k] = best_avg[k];
// flag calibration as failed for this gyro
_gyro_cal_ok[k] = false;
} else {
_gyro_cal_ok[k] = true;
_gyro_offset[k] = new_gyro_offset[k];
}
}
// restore orientation
_board_orientation = saved_orientation;
// record calibration complete
_calibrating = false;
// stop flashing leds
AP_Notify::flags.initialising = false;
}
void
AP_InertialSensor::_init_accel()
{
uint8_t num_accels = min(get_accel_count(), INS_MAX_INSTANCES);
uint8_t flashcount = 0;
Vector3f prev[INS_MAX_INSTANCES];
Vector3f accel_offset[INS_MAX_INSTANCES];
float total_change[INS_MAX_INSTANCES];
float max_offset[INS_MAX_INSTANCES];
memset(max_offset, 0, sizeof(max_offset));
memset(total_change, 0, sizeof(total_change));
// cold start
hal.scheduler->delay(100);
hal.console->print_P(PSTR("Init Accel"));
// flash leds to tell user to keep the IMU still
AP_Notify::flags.initialising = true;
// clear accelerometer offsets and scaling
for (uint8_t k=0; k<num_accels; k++) {
_accel_offset[k] = Vector3f(0,0,0);
_accel_scale[k] = 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[k] = Vector3f(500, 500, 500);
}
// loop until we calculate acceptable offsets
while (true) {
// get latest accelerometer values
update();
for (uint8_t k=0; k<num_accels; k++) {
// store old offsets
prev[k] = accel_offset[k];
// get new offsets
accel_offset[k] = get_accel(k);
}
// We take some readings...
for(int8_t i = 0; i < 50; i++) {
hal.scheduler->delay(20);
update();
// low pass filter the offsets
for (uint8_t k=0; k<num_accels; k++) {
accel_offset[k] = accel_offset[k] * 0.9f + get_accel(k) * 0.1f;
}
// display some output to the user
if(flashcount >= 10) {
hal.console->print_P(PSTR("*"));
flashcount = 0;
}
flashcount++;
}
for (uint8_t k=0; k<num_accels; k++) {
// null gravity from the Z accel
accel_offset[k].z += GRAVITY_MSS;
total_change[k] =
fabsf(prev[k].x - accel_offset[k].x) +
fabsf(prev[k].y - accel_offset[k].y) +
fabsf(prev[k].z - accel_offset[k].z);
max_offset[k] = (accel_offset[k].x > accel_offset[k].y) ? accel_offset[k].x : accel_offset[k].y;
max_offset[k] = (max_offset[k] > accel_offset[k].z) ? max_offset[k] : accel_offset[k].z;
}
uint8_t num_converged = 0;
for (uint8_t k=0; k<num_accels; k++) {
if (total_change[k] <= AP_INERTIAL_SENSOR_ACCEL_TOT_MAX_OFFSET_CHANGE &&
max_offset[k] <= AP_INERTIAL_SENSOR_ACCEL_MAX_OFFSET) {
num_converged++;
}
}
if (num_converged == num_accels) break;
hal.scheduler->delay(500);
}
// set the global accel offsets
for (uint8_t k=0; k<num_accels; k++) {
_accel_offset[k] = accel_offset[k];
}
// stop flashing the leds
AP_Notify::flags.initialising = false;
hal.console->print_P(PSTR(" "));
}
void
AP_InertialSensor::_init_accel()
{
int8_t flashcount = 0;
Vector3f ins_accel;
Vector3f prev;
Vector3f accel_offset;
float total_change;
float max_offset;
// cold start
hal.scheduler->delay(100);
hal.console->printf_P(PSTR("Init Accel"));
// flash leds to tell user to keep the IMU still
AP_Notify::flags.initialising = true;
// 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
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++) {
hal.scheduler->delay(20);
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 >= 10) {
hal.console->printf_P(PSTR("*"));
flashcount = 0;
}
flashcount++;
}
// null gravity from the Z accel
accel_offset.z += GRAVITY_MSS;
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;
hal.scheduler->delay(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;
// stop flashing the leds
AP_Notify::flags.initialising = false;
hal.console->printf_P(PSTR(" "));
}
// calibrate_accel - perform accelerometer calibration including providing user
// instructions and feedback Gauss-Newton accel calibration routines borrowed
// from Rolfe Schmidt blog post describing the method:
// http://chionophilous.wordpress.com/2011/10/24/accelerometer-calibration-iv-1-implementing-gauss-newton-on-an-atmega/
// original sketch available at
// http://rolfeschmidt.com/mathtools/skimetrics/adxl_gn_calibration.pde
bool AP_InertialSensor::calibrate_accel(AP_InertialSensor_UserInteract* interact,
float &trim_roll,
float &trim_pitch)
{
uint8_t num_accels = min(get_accel_count(), INS_MAX_INSTANCES);
Vector3f samples[INS_MAX_INSTANCES][6];
Vector3f new_offsets[INS_MAX_INSTANCES];
Vector3f new_scaling[INS_MAX_INSTANCES];
Vector3f orig_offset[INS_MAX_INSTANCES];
Vector3f orig_scale[INS_MAX_INSTANCES];
uint8_t num_ok = 0;
// exit immediately if calibration is already in progress
if (_calibrating) {
return false;
}
_calibrating = true;
/*
we do the accel calibration with no board rotation. This avoids
having to rotate readings during the calibration
*/
enum Rotation saved_orientation = _board_orientation;
_board_orientation = ROTATION_NONE;
for (uint8_t k=0; k<num_accels; k++) {
// backup original offsets and scaling
orig_offset[k] = _accel_offset[k].get();
orig_scale[k] = _accel_scale[k].get();
// clear accelerometer offsets and scaling
_accel_offset[k] = Vector3f(0,0,0);
_accel_scale[k] = Vector3f(1,1,1);
}
memset(samples, 0, sizeof(samples));
// capture data from 6 positions
for (uint8_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 its LEFT side");
break;
case 2:
msg = PSTR("on its RIGHT side");
break;
case 3:
msg = PSTR("nose DOWN");
break;
case 4:
msg = PSTR("nose UP");
break;
default: // default added to avoid compiler warning
case 5:
msg = PSTR("on its BACK");
break;
}
interact->printf_P(
PSTR("Place vehicle %S and press any key.\n"), msg);
// wait for user input
if (!interact->blocking_read()) {
//No need to use interact->printf_P for an error, blocking_read does this when it fails
goto failed;
}
const uint8_t update_dt_milliseconds = (uint8_t)(1000.0f/get_sample_rate()+0.5f);
// wait 100ms for ins filter to rise
for (uint8_t k=0; k<100/update_dt_milliseconds; k++) {
wait_for_sample();
update();
hal.scheduler->delay(update_dt_milliseconds);
}
uint32_t num_samples = 0;
while (num_samples < 400/update_dt_milliseconds) {
wait_for_sample();
// read samples from ins
update();
// capture sample
for (uint8_t k=0; k<num_accels; k++) {
Vector3f samp;
if(get_delta_velocity(k,samp) && _delta_velocity_dt[k] > 0) {
samp /= _delta_velocity_dt[k];
} else {
samp = get_accel(k);
}
samples[k][i] += samp;
if (!get_accel_health(k)) {
interact->printf_P(PSTR("accel[%u] not healthy"), (unsigned)k);
goto failed;
}
}
hal.scheduler->delay(update_dt_milliseconds);
num_samples++;
}
for (uint8_t k=0; k<num_accels; k++) {
samples[k][i] /= num_samples;
}
}
// run the calibration routine
for (uint8_t k=0; k<num_accels; k++) {
if (!_check_sample_range(samples[k], saved_orientation, interact)) {
interact->printf_P(PSTR("Insufficient accel range"));
continue;
}
bool success = _calibrate_accel(samples[k], new_offsets[k], new_scaling[k], saved_orientation);
interact->printf_P(PSTR("Offsets[%u]: %.2f %.2f %.2f\n"),
(unsigned)k,
(double)new_offsets[k].x, (double)new_offsets[k].y, (double)new_offsets[k].z);
interact->printf_P(PSTR("Scaling[%u]: %.2f %.2f %.2f\n"),
(unsigned)k,
(double)new_scaling[k].x, (double)new_scaling[k].y, (double)new_scaling[k].z);
if (success) num_ok++;
}
if (num_ok == num_accels) {
interact->printf_P(PSTR("Calibration successful\n"));
for (uint8_t k=0; k<num_accels; k++) {
// set and save calibration
_accel_offset[k].set(new_offsets[k]);
_accel_scale[k].set(new_scaling[k]);
}
for (uint8_t k=num_accels; k<INS_MAX_INSTANCES; k++) {
// clear unused accelerometer's scaling and offsets
_accel_offset[k] = Vector3f(0,0,0);
_accel_scale[k] = Vector3f(0,0,0);
}
_save_parameters();
/*
calculate the trims as well from primary accels
We use the original board rotation for this sample
*/
Vector3f level_sample = samples[0][0];
level_sample.rotate(saved_orientation);
_calculate_trim(level_sample, trim_roll, trim_pitch);
_board_orientation = saved_orientation;
_calibrating = false;
return true;
}
failed:
interact->printf_P(PSTR("Calibration FAILED\n"));
// restore original scaling and offsets
for (uint8_t k=0; k<num_accels; k++) {
_accel_offset[k].set(orig_offset[k]);
_accel_scale[k].set(orig_scale[k]);
}
_board_orientation = saved_orientation;
_calibrating = false;
return false;
}
void add_to_patterns(char *pattern) {
char first[MAX_BUFF];
char second[MAX_BUFF];
char type[MAX_BUFF];
char accel[MAX_BUFF];
regex_t compiled;
struct REGEX_pattern rpattern;
int stored;
/* The regex_flags that we use are:
REG_EXTENDED
REG_NOSUB
REG_ICASE;
*/
int regex_flags=REG_NOSUB;
rpattern.type=NORMAL;
rpattern.case_sensitive=1;
stored=sscanf(pattern, "%s %s %s %s", type, first, second, accel);
if((stored < 2)||(stored > 4)) {
logit(log_file, "unable to get a pair of patterns in add_to_patterns() for [%s]\n", pattern);
bridge_mode=1;
return;
}
if(stored==2) strcpy(second, "");
if(strcmp(type, "redirect")==0) {
redirect_url=(char *)malloc(sizeof(char)*(strlen(first)+1));
strcpy(redirect_url, first);
return;
}
if(strcmp(type, "timeout")==0) {
timeout=atoi(first);
return;
}
if(strcmp(type, "force")==0) {
force=atoi(first);
return;
}
if(strcmp(type, "max_size")==0) {
max_size=atoi(first);
return;
}
if(strcmp(type, "logfile")==0) {
log_file=(char *)malloc(sizeof(char)*(strlen(first)+1));
strcpy(log_file, first);
return;
}
if(strcmp(type, "proxy")==0) {
proxy=(char *)malloc(sizeof(char)*(strlen(first)+1));
strcpy(proxy, first);
return;
}
if(strcmp(type, "clamd_ip")==0) {
clamd_ip=(char *)malloc(sizeof(char)*(strlen(first)+1));
strcpy(clamd_ip, first);
return;
}
if(strcmp(type, "clamd_port")==0) {
clamd_port=(char *)malloc(sizeof(char)*(strlen(first)+1));
strcpy(clamd_port, first);
return;
}
if(strcmp(type, "clamd_local")==0) {
clamd_local=(char *)malloc(sizeof(char)*(strlen(first)+1));
strcpy(clamd_local, first);
return;
}
if((strcmp(type, "abort")==0)||(strcmp(type, "aborti")==0)) {
rpattern.type=ABORT;
}
if((strcmp(type, "content")==0)||(strcmp(type, "contenti")==0)) {
rpattern.type=CONTENT;
check_content_type=1;
}
if((strcmp(type, "regexi")==0)||(strcmp(type, "aborti")==0)||(strcmp(type, "contenti")==0)) {
regex_flags |= REG_ICASE;
rpattern.case_sensitive=0;
}
if((strcmp(type, "regex")==0)||(strcmp(type, "regexi")==0)) {
has_regex=1;
}
if(regcomp(&compiled, first, regex_flags)) {
logit(log_file, "Invalid regex [%s] in pattern file\n", first);
bridge_mode=1;
return;
}
rpattern.cpattern=compiled;
rpattern.pattern=(char *)malloc(sizeof(char)*(strlen(first)+1));
if(rpattern.pattern==NULL) {
logit(log_file, "unable to allocate memory in add_to_patterns()\n");
bridge_mode=1;
return;
}
strcpy(rpattern.pattern, first);
rpattern.replacement=(char *)malloc(sizeof(char)*(strlen(second)+1));
if(rpattern.replacement==NULL) {
logit(log_file, "unable to allocate memory in add_to_patterns()\n");
bridge_mode=1;
return;
}
strcpy(rpattern.replacement, second);
/* use accelerator string if it exists */
if(stored==4) {
rpattern.has_accel=1;
rpattern.accel=get_accel(accel, &rpattern.accel_type,rpattern.case_sensitive);
if(rpattern.accel==NULL) {
logit(log_file, "unable to allocate memory from get_accel()\n");
bridge_mode=1;
return;
}
} else {
rpattern.has_accel=0;
rpattern.accel=NULL;
}
add_to_plist(rpattern);
}
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(" "));
}
// 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;
}
void add_to_patterns(char *pattern)
{
char first[MAX_BUFF];
char second[MAX_BUFF];
char type[MAX_BUFF];
char accel[MAX_BUFF];
regex_t compiled;
struct REGEX_pattern rpattern;
int abort_type = 0;
int parenthesis;
int stored;
/* The regex_flags that we use are:
REG_EXTENDED
REG_NOSUB
REG_ICASE; */
int regex_flags = REG_NOSUB;
rpattern.type = NORMAL;
rpattern.case_sensitive = 1;
stored = sscanf(pattern, "%s %s %s %s", type, first, second, accel);
if((stored < 2) || (stored > 4)) {
log(LOG_ERROR,
"unable to get a pair of patterns in add_to_patterns() "
"for [%s]\n", pattern);
dodo_mode = 1;
return;
}
if(stored == 2)
strcpy(second, "");
if(strcmp(type, "abort") == 0) {
rpattern.type = ABORT;
abort_type = 1;
}
if(strcmp(type, "regexi") == 0) {
regex_flags |= REG_ICASE;
rpattern.case_sensitive = 0;
}
if(!abort_type) {
parenthesis = count_parenthesis (first);
if (parenthesis < 0) {
/* The function returned an invalid result,
indicating an invalid string */
log (LOG_ERROR, "count_parenthesis() returned "
"left count did not match right count for line: [%s]\n",
pattern);
dodo_mode = 1;
return;
} else if (parenthesis > 0) {
regex_flags |= REG_EXTENDED;
rpattern.type = EXTENDED;
regex_flags ^= REG_NOSUB;
}
}
if(regcomp(&compiled, first, regex_flags)) {
log(LOG_ERROR, "Invalid regex [%s] in pattern file\n", first);
dodo_mode = 1;
return;
}
rpattern.cpattern = compiled;
rpattern.pattern = (char *)malloc(sizeof(char) * (strlen(first) +1));
if(rpattern.pattern == NULL) {
log(LOG_ERROR, "unable to allocate memory in add_to_patterns()\n");
dodo_mode = 1;
return;
}
strcpy(rpattern.pattern, first);
rpattern.replacement = (char *)malloc(sizeof(char) * (strlen(second) +1));
if(rpattern.replacement == NULL) {
log(LOG_ERROR, "unable to allocate memory in add_to_patterns()\n");
dodo_mode = 1;
return;
}
strcpy(rpattern.replacement, second);
/* use accelerator string if it exists */
if(stored == 4) {
rpattern.has_accel = 1;
rpattern.accel = get_accel(accel, &rpattern.accel_type,
rpattern.case_sensitive);
}
/* use accelerator string if it exists */
if(stored == 4) {
rpattern.has_accel = 1;
rpattern.accel = get_accel(accel, &rpattern.accel_type,
rpattern.case_sensitive);
if(rpattern.accel == NULL) {
log(LOG_ERROR, "unable to allocate memory from get_accel()\n");
dodo_mode = 1;
return;
}
} else {
rpattern.has_accel = 0;
rpattern.accel = NULL;
}
add_to_plist(rpattern);
}
/* actually compiles individual regex etc. */
void add_to_patterns(char *pattern, struct pattern_item **list)
{
char first[MAX_BUFF];
char second[MAX_BUFF];
char type[MAX_BUFF];
char accel[MAX_BUFF];
regex_t compiled;
struct REGEX_pattern rpattern;
int abort_type = 0;
int abort_regex_type = 0;
int parenthesis;
int stored;
/* The regex_flags that we use are:
REG_EXTENDED
REG_NOSUB
REG_ICASE; */
int regex_flags = REG_NOSUB;
rpattern.type = NORMAL;
rpattern.case_sensitive = 1;
stored = sscanf(pattern, "%s %s %s %s", type, first, second, accel);
if((stored < 2) || (stored > 4)) {
logprint(LOG_ERROR,
"unable to get a pair of patterns in add_to_patterns() "
"for [%s]\n", pattern);
dodo_mode = 1;
return;
}
if(stored == 2)
strcpy(second, "");
/* type to lower case so as to be case insensitive */
lower_case(type);
if(strcmp(type, "abort") == 0) {
rpattern.type = ABORT;
abort_type = 1;
}
if(strcmp(type, "abortregexi") == 0) {
abort_regex_type = 1;
regex_flags |= REG_ICASE;
rpattern.case_sensitive = 0;
rpattern.type = ABORT_NORMAL;
}
if(strcmp(type, "abortregex") == 0) {
abort_regex_type = 1;
rpattern.type = ABORT_NORMAL;
}
if (strcmp(type, "abort_on_match") == 0) {
rpattern.type = ABORT_ON_MATCH;
abort_type = 1;
/* make sure on or off is in the correct state */
lower_case (first);
}
if(strcmp(type, "regexi") == 0) {
regex_flags |= REG_ICASE;
rpattern.case_sensitive = 0;
}
if(!abort_type) {
parenthesis = count_parenthesis (first);
if (parenthesis < 0) {
/* The function returned an invalid result,
indicating an invalid string */
logprint(LOG_ERROR, "count_parenthesis() returned "
"left count did not match right count for line: [%s]\n",
pattern);
dodo_mode = 1;
return;
} else if (parenthesis > 0) {
regex_flags |= REG_EXTENDED;
regex_flags ^= REG_NOSUB;
if (!abort_regex_type) {
rpattern.type = EXTENDED;
} else {
rpattern.type = ABORT_EXTENDED;
}
}
}
/* compile the regex */
if(regcomp(&compiled, first, regex_flags)) {
logprint(LOG_ERROR, "Invalid regex [%s] in pattern file\n", first);
dodo_mode = 1;
return;
}
/* put the compiled regex into the structure, along with replacement etc. */
rpattern.cpattern = compiled;
rpattern.pattern = (char *)malloc(sizeof(char) * (strlen(first) +1));
if(rpattern.pattern == NULL) {
logprint(LOG_ERROR, "unable to allocate memory in add_to_patterns()\n");
dodo_mode = 1;
return;
}
strcpy(rpattern.pattern, first);
rpattern.replacement = (char *)malloc(sizeof(char) * (strlen(second) +1));
if(rpattern.replacement == NULL) {
logprint(LOG_ERROR, "unable to allocate memory in add_to_patterns()\n");
dodo_mode = 1;
return;
}
strcpy(rpattern.replacement, second);
/* make up an accel string if one was not supplied */
/* if we have a regex type */
if (!abort_type) {
/* if it is a abort regex, 3 fields means that we have accelerator */
if (abort_regex_type && (stored != 3)) {
makeup_accel(accel, first);
stored = 3;
}
/* if it normal replacement regex, 4 fields means we have an accelerator already */
if (!abort_regex_type && (stored != 4)) {
makeup_accel(accel, first);
stored = 4;
}
}
if (accel != NULL) {
rpattern.has_accel = 1;
rpattern.accel = get_accel(accel, &rpattern.accel_type,
rpattern.case_sensitive);
if(rpattern.accel == NULL) {
logprint(LOG_ERROR, "unable to allocate memory from get_accel()\n");
dodo_mode = 1;
return;
}
} else {
rpattern.has_accel = 0;
rpattern.accel = NULL;
}
/* finally, add it to the end of the list */
add_to_plist(rpattern, list);
}
// calibrate_accel - perform accelerometer calibration including providing user
// instructions and feedback Gauss-Newton accel calibration routines borrowed
// from Rolfe Schmidt blog post describing the method:
// http://chionophilous.wordpress.com/2011/10/24/accelerometer-calibration-iv-1-implementing-gauss-newton-on-an-atmega/
// original sketch available at
// http://rolfeschmidt.com/mathtools/skimetrics/adxl_gn_calibration.pde
bool AP_InertialSensor::calibrate_accel(void (*flash_leds_cb)(bool on),
AP_InertialSensor_UserInteract* interact)
{
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;
default: // default added to avoid compiler warning
case 5:
msg = PSTR("on it's back");
break;
}
interact->printf_P(
PSTR("Place APM %S and press any key.\n"), msg);
// wait for user input
interact->blocking_read();
// clear out any existing samples from ins
update();
// wait until we have 32 samples
while( num_samples_available() < 32 * SAMPLE_UNIT ) {
hal.scheduler->delay(10);
}
// read samples from ins
update();
// capture sample
samples[i] = get_accel();
}
// run the calibration routine
if( _calibrate_accel(samples, new_offsets, new_scaling) ) {
interact->printf_P(PSTR("Calibration successful\n"));
// set and save calibration
_accel_offset.set(new_offsets);
_accel_scale.set(new_scaling);
_save_parameters();
return true;
}
interact->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);
// restore original scaling and offsets
_accel_offset.set(orig_offset);
_accel_scale.set(orig_scale);
return false;
}
// calibrate_accel - perform accelerometer calibration including providing user
// instructions and feedback Gauss-Newton accel calibration routines borrowed
// from Rolfe Schmidt blog post describing the method:
// http://chionophilous.wordpress.com/2011/10/24/accelerometer-calibration-iv-1-implementing-gauss-newton-on-an-atmega/
// original sketch available at
// http://rolfeschmidt.com/mathtools/skimetrics/adxl_gn_calibration.pde
bool AP_InertialSensor::calibrate_accel(AP_InertialSensor_UserInteract* interact,
float &trim_roll,
float &trim_pitch)
{
uint8_t num_accels = min(get_accel_count(), INS_MAX_INSTANCES);
Vector3f samples[INS_MAX_INSTANCES][6];
Vector3f new_offsets[INS_MAX_INSTANCES];
Vector3f new_scaling[INS_MAX_INSTANCES];
Vector3f orig_offset[INS_MAX_INSTANCES];
Vector3f orig_scale[INS_MAX_INSTANCES];
uint8_t num_ok = 0;
for (uint8_t k=0; k<num_accels; k++) {
// backup original offsets and scaling
orig_offset[k] = _accel_offset[k].get();
orig_scale[k] = _accel_scale[k].get();
// clear accelerometer offsets and scaling
_accel_offset[k] = Vector3f(0,0,0);
_accel_scale[k] = Vector3f(1,1,1);
}
// capture data from 6 positions
for (uint8_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 its LEFT side");
break;
case 2:
msg = PSTR("on its RIGHT side");
break;
case 3:
msg = PSTR("nose DOWN");
break;
case 4:
msg = PSTR("nose UP");
break;
default: // default added to avoid compiler warning
case 5:
msg = PSTR("on its BACK");
break;
}
interact->printf_P(
PSTR("Place vehicle %S and press any key.\n"), msg);
// wait for user input
if (!interact->blocking_read()) {
//No need to use interact->printf_P for an error, blocking_read does this when it fails
goto failed;
}
// clear out any existing samples from ins
update();
// average 32 samples
for (uint8_t k=0; k<num_accels; k++) {
samples[k][i] = Vector3f();
}
uint8_t num_samples = 0;
while (num_samples < 32) {
wait_for_sample();
// read samples from ins
update();
// capture sample
for (uint8_t k=0; k<num_accels; k++) {
samples[k][i] += get_accel(k);
}
hal.scheduler->delay(10);
num_samples++;
}
for (uint8_t k=0; k<num_accels; k++) {
samples[k][i] /= num_samples;
}
}
// run the calibration routine
for (uint8_t k=0; k<num_accels; k++) {
bool success = _calibrate_accel(samples[k], new_offsets[k], new_scaling[k]);
interact->printf_P(PSTR("Offsets[%u]: %.2f %.2f %.2f\n"),
(unsigned)k,
new_offsets[k].x, new_offsets[k].y, new_offsets[k].z);
interact->printf_P(PSTR("Scaling[%u]: %.2f %.2f %.2f\n"),
(unsigned)k,
new_scaling[k].x, new_scaling[k].y, new_scaling[k].z);
if (success) num_ok++;
}
if (num_ok == num_accels) {
interact->printf_P(PSTR("Calibration successful\n"));
for (uint8_t k=0; k<num_accels; k++) {
// set and save calibration
_accel_offset[k].set(new_offsets[k]);
_accel_scale[k].set(new_scaling[k]);
}
_save_parameters();
check_3D_calibration();
// calculate the trims as well from primary accels and pass back to caller
_calculate_trim(samples[0][0], trim_roll, trim_pitch);
return true;
}
failed:
interact->printf_P(PSTR("Calibration FAILED\n"));
// restore original scaling and offsets
for (uint8_t k=0; k<num_accels; k++) {
_accel_offset[k].set(orig_offset[k]);
_accel_scale[k].set(orig_scale[k]);
}
return false;
}
// calibrate_accel - perform accelerometer calibration including providing user
// instructions and feedback Gauss-Newton accel calibration routines borrowed
// from Rolfe Schmidt blog post describing the method:
// http://chionophilous.wordpress.com/2011/10/24/accelerometer-calibration-iv-1-implementing-gauss-newton-on-an-atmega/
// original sketch available at
// http://rolfeschmidt.com/mathtools/skimetrics/adxl_gn_calibration.pde
bool AP_InertialSensor::calibrate_accel(AP_InertialSensor_UserInteract* interact,
float &trim_roll,
float &trim_pitch)
{
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 its LEFT side");
break;
case 2:
msg = PSTR("on its RIGHT side");
break;
case 3:
msg = PSTR("nose DOWN");
break;
case 4:
msg = PSTR("nose UP");
break;
default: // default added to avoid compiler warning
case 5:
msg = PSTR("on its BACK");
break;
}
interact->printf_P(
PSTR("Place APM %S and press any key.\n"), msg);
// wait for user input
interact->blocking_read();
// clear out any existing samples from ins
update();
// wait until we have 32 samples
while( num_samples_available() < 32 * SAMPLE_UNIT ) {
hal.scheduler->delay(10);
}
// read samples from ins
update();
// capture sample
samples[i] = get_accel();
}
// run the calibration routine
bool success = _calibrate_accel(samples, new_offsets, new_scaling);
interact->printf_P(PSTR("Offsets: %.2f %.2f %.2f\n"),
new_offsets.x, new_offsets.y, new_offsets.z);
interact->printf_P(PSTR("Scaling: %.2f %.2f %.2f\n"),
new_scaling.x, new_scaling.y, new_scaling.z);
if (success) {
interact->printf_P(PSTR("Calibration successful\n"));
// set and save calibration
_accel_offset.set(new_offsets);
_accel_scale.set(new_scaling);
_save_parameters();
// calculate the trims as well and pass back to caller
_calculate_trim(samples[0], trim_roll, trim_pitch);
return true;
}
interact->printf_P(PSTR("Calibration FAILED\n"));
// restore original scaling and offsets
_accel_offset.set(orig_offset);
_accel_scale.set(orig_scale);
return false;
}
int main(void)
{
setup();
uart_init();
TWI_Init();
input = getchar();
while((input != 's'))//wait for user input to begin program
{
input = getchar();
}
sei();//global interrupts on
IMU_setup();
printf("\nLet's Begin\n\nChoose an option:\n\n space bar - PID (loops forever)\n 'm' - manual control (loops forever)\n 'i' - check IMU\n 'x' - get x acceleration\n 'y' - get y acceleration\n 'z' - get z acceleration\n 'f' - bluetooth fast mode\n 't' - test IMU write\n 'a' - enter acceleration mode\n");
while((1))
{
input = getchar();
if ((input == ' '))//PID algorithm
{
output_timer_on();
while((1))
{
PID();
}
}
else if ((input == 'm'))//manual control
{
printf("\nManual Mode:\n\nInstructions:\n\n 'n' - forwards\n 'v' - reverse\n 'b' - stop\n '1-9' - use the number keys to select the power level\n");
while((1))
{
manual();
}
}
else if ((input == 'i'))//check WHO_AM_I register
{
check_imu();
}
else if ((input == 'x'))//get x direction acceleration
{
acceleration = get_accel('x');
print_float(acceleration);
printf("\n");
}
else if ((input == 'y'))//get y direction acceleration
{
acceleration = get_accel('y');
print_float(acceleration);
printf("\n");
}
else if ((input == 'z'))//get z direction acceleration
{
acceleration = get_accel('z');
print_float(acceleration);
printf("\n");
}
else if ((input == 'f'))//place RN-42 HID into fast mode
{
fast_mode();
}
else if ((input == 't'))//write a regsiter of the IMU and check it worked
{
TWI_WRITEBYTE(MPU6050_PWR_MGMT_1, 0x02);
if ((TWI_READBYTE(MPU6050_PWR_MGMT_1) == 0x02))
printf("\nSuccess\n");
else
printf("\nFailure\n");
}
else if ((input == 'a'))//acceleration mode for acquiring data
{
acceleration_mode();
}
}
}
