/****************************************************************
CALL: Pressure_Init()
INTRO: This routine initializes the Bosch BMP183 pressure
sensor.
INPUT: nothing
OUTPUT: nothing
****************************************************************/
void Pressure_Init( void )
{
int iErr;
short tempC;
BYTE b;
// If set to 0xB6, will perform the same sequence as power on reset.
//b = 0xB6;
//I2C_WriteData( SLAVE_ADDR_BMP180, BMPREG_SOFT_RESET, 1, &b);
/*---- verify that pressure sensor can be read ----*/
iErr = i2c_read( SLAVE_ADDR_BMP180, BMPREG_ID, 1, &b );
if( iErr || (b != 0x55) )
{
/*---- indicate an error in the status ----*/
if( status.Error[ERROR_PRESSURE] < 255 ) {
status.Error[ERROR_PRESSURE]++;
}
}
#ifdef CJC_HACK
/*---- configure oversampling rate ----*/
s_oversampling_setting = Oversample_Off; // 0x00;
/*---- read all calibration parameters ----*/
iErr = read_calibration();
if( iErr == 0x0)
{
/*---- make sure to read temp first to initialize temp variable used in pressure calibration ----*/
GetTemperature(&tempC);
tempC = (tempC + 5)/ 10; // Round
}
#endif
}
END_TEST
START_TEST(test_point_position)
{
/* Generate target information based on a known point. In the simple case,
calculated point must equal known point and average ray distance == 0.
Then jigg the cameras symmetrically to get the same points again but
with an analytically derivable ray distance. */
int cam, num_cams = 4;
vec2d targs_plain[4], targs_jigged[4];
Calibration *calib[4];
char ori_tmpl[] = "cal/sym_cam%d.tif.ori";
char ori_name[25];
mm_np media_par = {1, 1., {1., 0., 0.}, {1., 0., 0.}, 1.};
vec3d point = {17, 42, 0}; /* Something in the FOV, non-symmetric. */
vec3d res, jigged;
double jigg_amp = 0.5, skew_dist, jigged_correct;
/* Target generation requires an existing calibration. */
chdir("testing_fodder/");
/* Four cameras on 4 quadrants. */
for (cam = 0; cam < num_cams; cam++) {
sprintf(ori_name, ori_tmpl, cam + 1);
calib[cam] = read_calibration(ori_name, "cal/cam1.tif.addpar", NULL);
img_coord(point, calib[cam], &media_par,
&(targs_plain[cam][0]), &(targs_plain[cam][1]));
vec_copy(jigged, point);
jigged[1] += ((cam % 2) ? jigg_amp : -jigg_amp);
img_coord(jigged, calib[cam], &media_par,
&(targs_jigged[cam][0]), &(targs_jigged[cam][1]));
}
skew_dist = point_position(targs_plain, num_cams, &media_par, calib, res);
fail_unless(skew_dist < 1e-10);
vec_subt(point, res, res);
fail_unless(vec_norm(res) < 1e-10);
skew_dist = point_position(targs_jigged, num_cams, &media_par, calib, res);
jigged_correct = 4*(2*jigg_amp)/6;
/* two thirds, but because of details: each disagreeing pair (4 out of 6)
has a 2*jigg_amp error because the cameras are moved in opposite
directions. */
fail_unless(fabs(skew_dist - jigged_correct) < 0.05);
vec_subt(point, res, res);
fail_unless(vec_norm(res) < 0.01);
}
END_TEST
/* Unit test for writing orientation files. Writes a sample calibration,
reads it back and compares.
*/
START_TEST(test_write_ori)
{
Calibration correct_cal, *cal;
correct_cal = test_cal();
char ori_file[] = "testing_fodder/test.ori";
char add_file[] = "testing_fodder/test.addpar";
write_ori(correct_cal.ext_par, correct_cal.int_par,
correct_cal.glass_par, correct_cal.added_par, ori_file, add_file);
fail_if((cal = read_calibration(ori_file, add_file, NULL)) == NULL);
fail_unless(compare_calib(cal, &correct_cal));
remove(ori_file);
remove(add_file);
}
END_TEST
START_TEST(test_convergence_measure)
{
/* Generate target information based on known points. In the simple case,
convergence will be spot-on. Then generate other targets based on a
per-camera jigging of 3D points and get convergence measure compatible
with the jig amplitude. */
vec3d known[16], jigged;
vec2d* targets[16];
Calibration *calib[4];
int num_cams = 4, num_pts = 16;
int cam, cpt_vert, cpt_horz, cpt_ix;
double jigg_amp = 0.5, jigged_skew_dist, jigged_correct;
char ori_tmpl[] = "cal/sym_cam%d.tif.ori";
char ori_name[25];
/* Using a neutral medium, this isn't what's tested. */
mm_np media_par = {1, 1., {1., 0., 0.}, {1., 0., 0.}, 1.};
control_par *cpar;
/* Target generation requires an existing calibration. */
chdir("testing_fodder/");
cpar = read_control_par("parameters/ptv.par");
/* Four cameras on 4 quadrants looking down into a calibration target.
Calibration taken from an actual experimental setup */
for (cam = 0; cam < num_cams; cam++) {
sprintf(ori_name, ori_tmpl, cam + 1);
calib[cam] = read_calibration(ori_name, "cal/cam1.tif.addpar", NULL);
}
/* Reference points before jigging: */
for (cpt_horz = 0; cpt_horz < 4; cpt_horz++) {
for (cpt_vert = 0; cpt_vert < 4; cpt_vert++) {
cpt_ix = cpt_horz*4 + cpt_vert;
vec_set(known[cpt_ix], cpt_vert * 10, cpt_horz * 10, 0);
}
}
/* Plain case: */
for (cpt_ix = 0; cpt_ix < num_pts; cpt_ix++) {
targets[cpt_ix] = (vec2d *) calloc(num_cams, sizeof(vec2d));
for (cam = 0; cam < num_cams; cam++) {
img_coord(known[cpt_ix], calib[cam], &media_par,
&(targets[cpt_ix][cam][0]),
&(targets[cpt_ix][cam][1]));
}
}
fail_unless(fabs(weighted_dumbbell_precision(
targets, 16, num_cams, &media_par, calib, 1, 0)) < 1e-10);
/* With dumbbell length: */
fail_unless(fabs(weighted_dumbbell_precision(
targets, 16, num_cams, &media_par, calib, 10, 10)) < 1e-10);
/* Jigged case (reusing target memory), moving the points and cameras
in parallel to create a known shift between parallel rays. */
for (cam = 0; cam < num_cams; cam++) {
calib[cam]->ext_par.y0 += ((cam % 2) ? jigg_amp : -jigg_amp);
for (cpt_ix = 0; cpt_ix < num_pts; cpt_ix++) {
vec_copy(jigged, known[cpt_ix]);
jigged[1] += ((cam % 2) ? jigg_amp : -jigg_amp);
img_coord(jigged, calib[cam], &media_par,
&(targets[cpt_ix][cam][0]),
&(targets[cpt_ix][cam][1]));
}
}
jigged_skew_dist = weighted_dumbbell_precision(
targets, 16, num_cams, &media_par, calib, 1, 0);
jigged_correct = 16*4*(2*jigg_amp)/(16*6);
/* two thirds, but because of details: each disagreeing pair (4 out of 6)
has a 2*jigg_amp error because the cameras are moved in opposite
directions. */
fail_unless(fabs(jigged_skew_dist - jigged_correct) < 0.05 );
}
int main(void){
clock_switch32M();
uart_init();
bus_init(&bus);
PORTC.DIR = 0xFF;
//Setup a timer, just for counting. Used for bus message and timeouts.
TCC1_CTRLA = TC_CLKSEL_DIV1024_gc;
//Set the interrupts we want to listen to.
PMIC.CTRL = PMIC_MEDLVLEN_bm |PMIC_HILVLEN_bm | PMIC_RREN_bm;
//Make sure to move the reset vectors to application flash.
CCP = CCP_IOREG_gc;
PMIC.CTRL &= ~PMIC_IVSEL_bm;
//Enable interrupts.
sei();
//Read temperature calibration:
read_calibration();
uint8_t cnt_tm = 0; //Used by TCC1
uint8_t poll_cnt = 0; //Used for polling the display.
//uint16_t pwm_lim = 0; //PWM limit may change at higher speed.
while(1){
continue;
get_measurements();
if (get_throttle()){
motor.throttle = throttle;
}else{
motor.throttle = 0;
}
if (display.online == false){
motor.mode = 0;
}
//Set display data
display.voltage = motor.voltage;
display.current = motor.current;
//display.power = power_av;
display.throttle = throttle;
display.error = status | motor.status;
/*if (motor.status){
display.error = motor.status;
}*/
//display.speed = speed% 1000; //999 max
if (TCC1.CNT > 500){
cnt_tm++;
TCC1.CNT = 0;
//Last character in message
if (wait_for_last_char){
wait_for_last_char = false;
}else{
bus_endmessage(&bus);
}
//Send timer tick
bus_tick(&bus);
if (bus_check_for_message()){
green_led_on();
}else{
green_led_off();
//Increase offline count.
display.offline_cnt++;
if (display.offline_cnt > 10){
uart_rate_find();
PORTC.OUTSET = PIN6_bm;
}else{
PORTC.OUTCLR = PIN6_bm;
}
//If offline too long:
if (display.offline_cnt > 50){
//You need to make it unsafe again.
display.road_legal = true;
display.online = false;
throttle_cruise = false;
throttle = 0;
use_brake = false;
brake = 0;
}
if (motor.offline_cnt > 20){
motor.online = false;
}
poll_cnt++;
if (poll_cnt == 2){
bus_display_poll(&bus);
}else if (poll_cnt == 4){
bus_display_update(&bus);
}else if (poll_cnt == 6){
bus_motor_poll(&bus);
motor.offline_cnt++;
}else if (poll_cnt == 8){
poll_cnt = 0;
}
}
}
}
}
