void main(void) {
//configuring
P1OUT |= 0x04; // set P1.2 for debug
P4DIR |= 0x20; // P4.5 as output (for debug)
gina_init();
scheduler_init();
leds_init();
if (*(&eui64+3)==0x09) { // this is a GINA board (not a basestation)
gyro_init();
large_range_accel_init();
magnetometer_init();
sensitive_accel_temperature_init();
}
radio_init();
timer_init();
P1OUT &= ~0x04; // clear P1.2 for debug
//check sensor configuration is right
gyro_get_config();
large_range_accel_get_config();
magnetometer_get_config();
sensitive_accel_temperature_get_config();
//scheduler_push_task(ID_TASK_APPLICATION);
scheduler_register_application_task(&task_application_imu_radio, 0, FALSE);
scheduler_start();
}
void magnetometer(void)
{
/*
The magnetometer values must be read consecutively
in order to move the magnetometer pointer. Therefore the x, y, and z
outputs need to be kept in this function. To read the magnetometer
values, call the function magnetometer(), then global vars
x_mag, y_mag, z_mag.
*/
magnetometer_init();
uint8_t xh, xl, yh, yl, zh, zl;
//must read all six registers plus one to move the pointer back to 0x03
i2cSendStart();
i2cWaitForComplete();
i2cSendByte(0x3D); //write to HMC
i2cWaitForComplete();
i2cReceiveByte(TRUE);
i2cWaitForComplete();
xh = i2cGetReceivedByte(); //x high byte
i2cWaitForComplete();
i2cReceiveByte(TRUE);
i2cWaitForComplete();
xl = i2cGetReceivedByte(); //x low byte
i2cWaitForComplete();
x_mag = xl|(xh << 8);
i2cReceiveByte(TRUE);
i2cWaitForComplete();
zh = i2cGetReceivedByte();
i2cWaitForComplete(); //z high byte
i2cReceiveByte(TRUE);
i2cWaitForComplete();
zl = i2cGetReceivedByte(); //z low byte
i2cWaitForComplete();
z_mag = zl|(zh << 8);
i2cReceiveByte(TRUE);
i2cWaitForComplete();
yh = i2cGetReceivedByte(); //y high byte
i2cWaitForComplete();
i2cReceiveByte(TRUE);
i2cWaitForComplete();
yl = i2cGetReceivedByte(); //y low byte
i2cWaitForComplete();
y_mag = yl|(yh << 8);
i2cSendByte(0x3D); //must reach 0x09 to go back to 0x03
i2cWaitForComplete();
i2cSendStop();
}
void main(void) {
//configuring
P1OUT |= 0x04; // set P1.2 for debug
P4DIR |= 0x20; // P4.5 as output (for debug)
gina_init();
scheduler_init();
leds_init();
if (*(&eui64+3)==0x09) { // this is a GINA board (not a basestation)
magnetometer_init();
}
radio_init();
timer_init();
P1OUT &= ~0x04; // clear P1.2 for debug
//check sensor configuration is right
magnetometer_get_config();
//scheduler_push_task(ID_TASK_APPLICATION);
//initialize variables
timer_period = 0x033333; //set the timer frequency to 80Hz
for (int c=0;c<9;c++)
{
delay[c] = 0;
out[c] = 0;
}
alpha[0]= 0.423466145992279;
alpha[1]= 0.359764546155930;
alpha[2]= 0.134587764739990;
alpha[3]= 0.445259362459183;
alpha[4]= 0.134587764739990;
alpha[5]= 0.400678455829620;
alpha[6]= 0.134587764739990;
alpha[7]= 0.160087645053864;
alpha[8]= 0.134587764739990;
//FSM variable initialization
threshold = 0.1096;
state = NOCAR; //initial state
FSMcounter = 0;
maxCount = 10; //change?
minCount = 2;
seenCar=0;
scheduler_register_application_task(&task_application_intersection, 410, TRUE);
scheduler_start();
}
void init (void)
{
//1 = output, 0 = input
DDRB = 0b01100000; //PORTB4, B5 output for stat LED
DDRC = 0b00010000; //PORTC4 (SDA), PORTC5 (SCL), PORTC all others are inputs
DDRD = 0b00000010; //PORTD (TX output on PD1)
PORTC = 0b00110000; //pullups on the I2C bus
cbi(PORTB, 5);
i2cInit();
accelerometer_init();
magnetometer_init();
gyro_init();
check_baud();
}
void config_menu(void)
{
i2cInit();
accelerometer_init();
magnetometer_init();
printf_P(wlcm_str);
printf_P(accel);
printf_P(mag);
printf_P(gyro);
printf_P(raw_out);
printf_P(baud_change);
printf("%ldbps\n\r", baud);
printf_P(autorun);
printf_P(help_);
config_read();
}
void main(void)
{
WDTCTL = WDTPW + WDTHOLD; // disable watchdog timer
BCSCTL1 = CALBC1_16MHZ; // MCLK at 16MHz
DCOCTL = CALDCO_16MHZ;
P1DIR |= 0x1E; // P1.1-4 as outputs (for debug)
P4DIR |= 0x20; // P4.5 as output (for debug)
__enable_interrupt(); // global enable interrupts
leds_init();
//configuring the magnetometer
if (*(&eui64+3)==0x09) { // this is a GINA board (not a basestation)
i2c_init():
magnetometer_init();
}
int main(){
DDRB |= _BV(PORTB1);
serial_init_b(9600);
magnetometer_init();
//Main program loop
while (1){
double heading = magnetometer_read_heading();
//int16_t data[3];
//magnetometer_read_raw(data);
char temp[100];
sprintf(temp, "Heading: %2.3f\n\r", heading);
//sprintf(temp, "%d\t%d\t%d\n\r", data[0], data[1], data[2]);
serial_write_s(temp);
PORTB ^= _BV(PORTB1);
_delay_ms(100);
}
}
void self_test(void)
{
//MAGNETOMETER
uint8_t xh, xl, yh, yl, zh, zl, hmc_flag = 0;
x_mag = 0;
y_mag = 0;
z_mag = 0;
magnetometer_init();
//must read all six registers plus one to move the pointer back to 0x03
i2cSendStart();
i2cWaitForComplete();
i2cSendByte(0x3D); //write to HMC
i2cWaitForComplete();
i2cReceiveByte(TRUE);
i2cWaitForComplete();
xh = i2cGetReceivedByte(); //x high byte
i2cWaitForComplete();
i2cReceiveByte(TRUE);
i2cWaitForComplete();
xl = i2cGetReceivedByte(); //x low byte
i2cWaitForComplete();
x_mag = xl|(xh << 8);
i2cReceiveByte(TRUE);
i2cWaitForComplete();
zh = i2cGetReceivedByte();
i2cWaitForComplete(); //z high byte
i2cReceiveByte(TRUE);
i2cWaitForComplete();
zl = i2cGetReceivedByte(); //z low byte
i2cWaitForComplete();
z_mag = zl|(zh << 8);
i2cReceiveByte(TRUE);
i2cWaitForComplete();
yh = i2cGetReceivedByte(); //y high byte
i2cWaitForComplete();
i2cReceiveByte(TRUE);
i2cWaitForComplete();
yl = i2cGetReceivedByte(); //y low byte
i2cWaitForComplete();
y_mag = yl|(yh << 8);
i2cSendByte(0x3D); //must reach 0x09 to go back to 0x03
i2cWaitForComplete();
i2cSendStop();
//if it gets to this point and there are values in x,y,z_mag, we can assume part is responding correctly
if((x_mag == y_mag) && (y_mag == z_mag)) hmc_flag = 0xFF;
else hmc_flag = 0;
//ACCELEROMETER
uint8_t x, dummy;
//0x32 data registers
i2cSendStart();
i2cWaitForComplete();
i2cSendByte(0xA6); //write to ADXL
i2cWaitForComplete();
i2cSendByte(0x00); //X0 data register
i2cWaitForComplete();
i2cSendStop(); //repeat start
i2cSendStart();
i2cWaitForComplete();
i2cSendByte(0xA7); //read from ADXL
i2cWaitForComplete();
i2cReceiveByte(TRUE);
i2cWaitForComplete();
x = i2cGetReceivedByte();
i2cWaitForComplete();
i2cReceiveByte(FALSE);
i2cWaitForComplete();
dummy = i2cGetReceivedByte();
i2cWaitForComplete();
i2cSendStop();
///////////////////////////////////
// Gyro////////////////////////////
///////////////////////////////////
char data;
cbi(TWCR, TWEN); // Disable TWI
sbi(TWCR, TWEN); // Enable TWI
i2cSendStart();
i2cWaitForComplete();
i2cSendByte(ITG3200_W); // write 0xD2
i2cWaitForComplete();
i2cSendByte(0x00); // who am i
i2cWaitForComplete();
i2cSendStart();
i2cSendByte(ITG3200_R); // write 0xD3
i2cWaitForComplete();
i2cReceiveByte(FALSE);
i2cWaitForComplete();
data = i2cGetReceivedByte(); // Get MSB result
i2cWaitForComplete();
i2cSendStop();
cbi(TWCR, TWEN); // Disable TWI
sbi(TWCR, TWEN); // Enable TWI
int gyro_flag = 0;
int mag_flag = 0;
int accel_flag = 0;
if(data == 0x69)
{
printf("ITG: GOOD\n\r");
gyro_flag = 1;
}else printf("ITG: BAD\n\r");
if(hmc_flag == 0)
{
printf("HMC: GOOD\n\r");
mag_flag = 1;
}else printf("HMC: BAD\n\r");
if(x == 0xE5)
{
printf("ADXL: GOOD\n\r");
accel_flag = 1;
}else printf("ADXL: BAD\n\r");
if(gyro_flag ==1 && mag_flag == 1 && accel_flag == 1)
{
sbi(PORTB, 5);
delay_ms(1000);
cbi(PORTB, 5);
delay_ms(1000);
sbi(PORTB, 5);
delay_ms(1000);
cbi(PORTB, 5);
delay_ms(1000);
sbi(PORTB, 5);
delay_ms(1000);
cbi(PORTB, 5);
delay_ms(1000);
sbi(PORTB, 5);
delay_ms(1000);
cbi(PORTB, 5);
delay_ms(1000);
sbi(PORTB, 5);
delay_ms(1000);
cbi(PORTB, 5);
gyro_flag = 0;
mag_flag = 0;
accel_flag = 0;
}else sbi(PORTB, 5);
//while(!(UCSR0A & (1 << RXC0)));
config_menu();
}
int threads_linux_init(void)
{
int status = 0;
int n = 0;
int return_value = THREADS_LINUX_SUCCESS;
// Init data
status = sensors_init(&battery_data, &gps_data, &imu_data, &pitot_data, &pwm_read_data, &pwm_write_data, &scp1000_data, &sonar_data);
if(status != SENSORS_SUCCESS)
{
return_value = THREADS_LINUX_FAILURE;
}
if(return_value != THREADS_LINUX_FAILURE)
{
status = calibration_init(&calibration_local_coordinate_system_data, &calibration_local_fields_data, &calibration_altimeter_data);
if(status != CALIBRATION_SUCCESS)
{
return_value = THREADS_LINUX_FAILURE;
}
}
if(return_value != THREADS_LINUX_FAILURE)
{
status = gps_init(&gps_measure);
if(status != 1)
{
return_value = THREADS_LINUX_FAILURE;
}
}
if(return_value != THREADS_LINUX_FAILURE)
{
status = imu_init(&imu_measure);
if(status != 1)
{
return_value = THREADS_LINUX_FAILURE;
}
}
if(return_value != THREADS_LINUX_FAILURE)
{
status = magnetometer_init(&magnetometer_measure);
if(status != 1)
{
return_value = THREADS_LINUX_FAILURE;
}
}
if(return_value != THREADS_LINUX_FAILURE)
{
status = sonar_init(&sonar_measure);
if(status != 1)
{
return_value = THREADS_LINUX_FAILURE;
}
}
if(return_value != THREADS_LINUX_FAILURE)
{
status = estimation_init(ESTIMATION_DO_NOT_ESTIMATE_ACCEL_BIAS, &estimation_data);
if(status != ESTIMATION_SUCCESS)
{
return_value = THREADS_LINUX_FAILURE;
}
}
if(return_value != THREADS_LINUX_FAILURE)
{
status = control_init(&control_data);
if(status != CONTROL_SUCCESS)
{
return_value = THREADS_LINUX_FAILURE;
}
}
if(return_value != THREADS_LINUX_FAILURE)
{
status = protocol_init();
if(status != PROTOCOL_SUCCESS)
{
return_value = THREADS_LINUX_FAILURE;
}
}
if(return_value != THREADS_LINUX_FAILURE)
{
status = ui_init();
if(status != UI_SUCCESS)
{
return_value = THREADS_LINUX_FAILURE;
}
}
if(return_value != THREADS_LINUX_FAILURE)
{
status = datalogger_init();
if(status != DATALOGGER_SUCCESS)
{
return_value = THREADS_LINUX_FAILURE;
}
}
// Main Loop
if(return_value != THREADS_LINUX_FAILURE)
{
threads_linux_timer_start_task_1();
usleep(0.5*task_1_period_us);
threads_linux_timer_start_task_2();
usleep(5*task_1_period_us);
while(quittask == 0)
{
#if ANU_COMPILE_FOR_OVERO
#else
if(datalogger_status() == DATALOGGER_RUNNING) datalogger_update_IPC();
#endif
if(++n>100)
{
if(datalogger_status() == DATALOGGER_RUNNING) datalogger_write_file();
n = 0;
}
usleep(5*task_1_period_us);
}
threads_linux_timer_stop_task_1();
usleep(TASK1_PERIOD_US);
threads_linux_timer_stop_task_2();
usleep(TASK2_PERIOD_US);
}
status = datalogger_close();
if(status != DATALOGGER_SUCCESS)
{
return_value = THREADS_LINUX_FAILURE;
}
status = ui_close();
if(status != UI_SUCCESS)
{
return_value = THREADS_LINUX_FAILURE;
}
status = protocol_close();
if(status != PROTOCOL_SUCCESS)
{
return_value = THREADS_LINUX_FAILURE;
}
status = control_close();
if(status != CONTROL_SUCCESS)
{
return_value = THREADS_LINUX_FAILURE;
}
status = estimation_close(&estimation_data);
if(status != ESTIMATION_SUCCESS)
{
return_value = THREADS_LINUX_FAILURE;
}
status = magnetometer_close(&magnetometer_measure);
if(status != 1)
{
return_value = THREADS_LINUX_FAILURE;
}
status = imu_close(&imu_measure);
if(status != 1)
{
return_value = THREADS_LINUX_FAILURE;
}
status = gps_close(&gps_measure);
if(status != 1)
{
return_value = THREADS_LINUX_FAILURE;
}
status = calibration_close(&calibration_local_coordinate_system_data, &calibration_local_fields_data, &calibration_altimeter_data);
if(status != CALIBRATION_SUCCESS)
{
return_value = THREADS_LINUX_FAILURE;
}
status = sensors_close();
if(status != SENSORS_SUCCESS)
{
return_value = THREADS_LINUX_FAILURE;
}
return return_value;
}
