static void jump_to_app(void) {
const uint32_t *app_base = (const uint32_t *)APP_LOAD_ADDRESS;
// Check that flash has been programmed
if (app_base[0] == 0xffffffff)
goto fail;
// Check that reset vector is somewhere in the app's boundaries
if (app_base[1] < APP_LOAD_ADDRESS)
goto fail;
if (app_base[1] >= (APP_LOAD_ADDRESS + (1024 * 1024)))
goto fail;
// LEDs to known state
green_led(1);
red_led(0);
// Set vector table address
SCB->VTOR = APP_LOAD_ADDRESS;
// Jump to the application
do_jump(app_base[0], app_base[1]);
// LEDs indicate error
fail:
green_led(0);
red_led(0);
for(;;);
}
int main() // run over and over again
{
while(1)
{
// note that the following line could also be accomplished with:
// int pot = analogRead(7);
int pot = read_trimpot(); // determine the trimpot position
// avoid clearing the LCD to reduce flicker
lcd_goto_xy(0, 0);
print("pot=");
print_long(pot); // print the trim pot position (0 - 1023)
print(" "); // overwrite any left over digits
int motorSpeed = (512 - pot) / 2;
lcd_goto_xy(0, 1);
print("spd=");
print_long(motorSpeed); // print the resulting motor speed (-255 - 255)
print(" ");
set_motors(motorSpeed, motorSpeed); // set speeds of motors 1 and 2
// all LEDs off
red_led(0);
green_led(0);
// turn green LED on when motors are spinning forward
if (motorSpeed > 0)
green_led(1);
// turn red LED on when motors are spinning in reverse
if (motorSpeed < 0)
red_led(1);
delay_ms(100);
}
}
void pot_test()
{
long start = get_ms();
char elapsed_ms;
int value;
set_analog_mode(MODE_10_BIT);
print_long(read_trimpot());
print(" "); // to clear the display
while((elapsed_ms = get_ms() - start) < 100)
{
value = read_trimpot();
play_frequency(value, 200, 15);
if(value < elapsed_ms*10)
{
red_led(0);
green_led(1);
}
else
{
red_led(1);
green_led(0);
}
}
}
int main()
{
set_analog_mode(MODE_8_BIT); // 8-bit analog-to-digital conversions
sum = 0;
samples = 0;
avg = 0;
start_analog_conversion(TRIMPOT); // start initial conversion
while(1)
{
if (!analog_is_converting()) // if conversion is done...
{
sum += analog_conversion_result(); // get result
start_analog_conversion(TRIMPOT); // start next conversion
if (++samples == 20) // if 20 samples have been taken...
{
avg = sum / 20; // compute 20-sample average of ADC result
samples = 0;
sum = 0;
}
}
// when avg == 0, the red LED is almost totally off.
// when avg == 255, the red LED is almost totally on.
// brightness should scale approximately linearly in between.
red_led(0); // red LED off
delay_us(256 - avg);
red_led(1); // red LED on
delay_us(avg+1);
}
}
int main(void) {
led_setup();
for(int i = 0; i < 4; i++) {
red_led(1);
green_led(1);
sleep(500);
red_led(0);
green_led(0);
sleep(500);
red_led(1);
sleep(500);
red_led(0);
sleep(500);
}
jump_to_app();
}
int main()
{
TCCR0A = 0; // configure timer0 to run at 78 kHz
TCCR0B = 0x04; // and overflow when TCNT0 = 256 (~3 ms)
play_from_program_space(rhapsody);
while(1)
{
// allow the sequence to keep playing automatically through the following delays
#ifndef ALWAYS_CHECK
play_mode(PLAY_AUTOMATIC);
#else
play_mode(PLAY_CHECK);
#endif
lcd_goto_xy(0, 0);
print("blink!");
int i;
for (i = 0; i < 8; i++)
{
#ifdef ALWAYS_CHECK
play_check();
#endif
red_led(1);
delay_ms(500);
red_led(0);
delay_ms(500);
}
lcd_goto_xy(0, 0);
print("timing");
lcd_goto_xy(0, 1);
print(" "); // clear bottom LCD line
// turn off automatic playing so that our time-critical code won't be interrupted by
// the buzzer's long timer1 interrupt. Otherwise, this interrupt could throw off our
// timing measurements. Instead, we will now use playCheck() to keep the sequence
// playing in a way that won't throw off our measurements.
#ifndef ALWAYS_AUTOMATIC
play_mode(PLAY_CHECK);
#endif
unsigned char maxTime = 0;
for (i = 0; i < 8000; i++)
{
TCNT0 = 0;
while (TCNT0 < 20) // time for ~250 us
;
if (TCNT0 > maxTime)
maxTime = TCNT0; // if the elapsed time is greater than the previous max, save it
#ifndef ALWAYS_AUTOMATIC
play_check(); // check if it's time to play the next note and play it if so
#endif
}
lcd_goto_xy(0, 1);
print("max=");
print_long((unsigned int)maxTime);
print(" "); // overwrite any left over characters
}
}
// Blinks the LEDs
void led_test()
{
play("c32");
print("Red 1 ");
red_led(1);
if(wait_for_250_ms_or_button_b())
return;
red_led(0);
if(wait_for_250_ms_or_button_b())
return;
play(">c32");
lcd_goto_xy(0,0);
print("Green 1 ");
green_led(1);
if(wait_for_250_ms_or_button_b())
return;
green_led(0);
if(wait_for_250_ms_or_button_b())
return;
play("d32");
lcd_goto_xy(0,0);
print("Red 2 ");
red_led2(1);
if(wait_for_250_ms_or_button_b())
return;
red_led2(0);
if(wait_for_250_ms_or_button_b())
return;
play(">d32");
lcd_goto_xy(0,0);
print("Green 2 ");
green_led2(1);
if(wait_for_250_ms_or_button_b())
return;
green_led2(0);
if(wait_for_250_ms_or_button_b())
return;
play("e32>e32");
lcd_goto_xy(0,0);
print("Yellow ");
yellow_led(1);
if(wait_for_250_ms_or_button_b())
return;
yellow_led(0);
if(wait_for_250_ms_or_button_b())
return;
}
static void panic(void) {
led_setup();
green_led(0);
for(;;){
red_led(1);
spin_sleep(200);
red_led(0);
spin_sleep(200);
}
}
int main()
{
while(1)
{
red_led(1); // Turn on the red LED.
delay_ms(200); // Wait for 200 ms.
red_led(0); // Turn off the red LED.
delay_ms(200); // Wait for 200 ms.
}
}
int main()
{
while(1)
{
red_led(1); // red LED on
delay_ms(1000); // waits for a second
red_led(0); // red LED off
delay_ms(1000); // waits for a second
green_led(1); // green LED on (will not work on the Baby Orangutan)
delay_ms(500); // waits for 0.5 seconds
green_led(0); // green LED off (will not work on the Baby Orangutan)
delay_ms(500); // waits for 0.5 seconds
}
}
void *dmr_sms_arrive_hook(void *pkt){
/* This hooks the SMS arrival routine, but as best I can tell,
dmr_sms_arrive() only handles the header and not the actual
data payload, which is managed by dmr_handle_data() in each
fragment chunk.
*pkt points to a twelve byte header with two bytes of C5000
overhead. The body packets will arrive at dmr_handle_data_hook()
in chunks of up to twelve bytes, varying by data rate.
A full transaction from 3147092 to 99 looks like this:
header
| / /flg\ /--dst-\ /--src-\ /flg\ /crc\
SMS header: 08 6a 02 40 00 00 63 30 05 54 88 00 83 0c
Data: 08 7a 45 00 00 5c 00 03 00 00 40 11 5c a8
Data: 08 7a 0c 30 05 54 0c 00 00 63 0f a7 0f a7
Data: 08 72 00 48 d1 dc 00 3e e0 00 92 04 0d 00
Data: 08 72 0a 00 54 00 68 00 69 00 73 00 20 00
Data: 08 72 69 00 73 00 20 00 61 00 20 00 74 00
Data: 08 7a 65 00 73 00 74 00 20 00 66 00 72 00
Data: 08 7a 6f 00 6d 00 20 00 6b 00 6b 00 34 00
Data: 08 7a 76 00 63 00 7a 00 21 00 9e 21 5a 5c
*/
//Turn on the red LED to know that we're here.
red_led(1);
printf("SMS header: ");
printhex((char*) pkt, 12+2);
printf("\n");
//Forward to the original function.
return dmr_sms_arrive(pkt);
}
static portTASK_FUNCTION( vBlockingQueueProducer, pvParameters )
{
unsigned short usValue = 0;
xBlockingQueueParameters *pxQueueParameters;
short sErrorEverOccurred = pdFALSE;
pxQueueParameters = ( xBlockingQueueParameters * ) pvParameters;
for( ;; )
{
if( xQueueSend( pxQueueParameters->xQueue, ( void * ) &usValue, pxQueueParameters->xBlockTime ) != pdPASS )
{
sErrorEverOccurred = pdTRUE;
}
else
{
/* We have successfully posted a message, so increment the variable
used to check we are still running. */
if( sErrorEverOccurred == pdFALSE )
{
( *pxQueueParameters->psCheckVariable )++;
}
/* Increment the variable we are going to post next time round. The
consumer will expect the numbers to follow in numerical order. */
++usValue;
play_from_program_space(PSTR("d"));
while (is_playing()){};
red_led(1); // Turn on the red LED.
delay_ms(20); // Wait for 20 ms.
}
}
}
// This is the main function, where the code starts. All C programs
// must have a main() function defined somewhere.
int main()
{
// Set all five user LEDs as outputs driven low.
// Otherwise, the LEDs are driven by the LCD and can appear
// on or partially on.
red_led(0);
green_led(0);
red_led2(0);
green_led2(0);
yellow_led(0);
clear();
delay(10);
// if any button is pressed, go into the old version of the test code
if(button_is_pressed(ANY_BUTTON))
{
print("Simple Test");
wait_for_button_release(button_is_pressed(ANY_BUTTON));
test(); // activate the simpler test code
}
// set up the robot
initialize();
// This is the "main loop" - it will run forever.
while(1)
{
menu_select();
}
}
int main()
{
play_from_program_space(PSTR(">g32>>c32")); // Play welcoming notes.
while(1)
{
// Print battery voltage (in mV) on LCD.
clear();
print_long(read_battery_millivolts_sv());
red_led(1); // Turn on the red LED.
delay_ms(200); // Wait for 200 ms.
red_led(0); // Turn off the red LED.
delay_ms(200); // Wait for 200 ms.
}
}
void error()
{
Led red_led(LED1, OFF);
while (1) {
red_led.toggle();
delay();
}
}
int main()
{
play_from_program_space(PSTR(">g32>>c32")); // Play welcoming notes.
while(1)
{
// Get battery voltage (in mV) from the auxiliary processor
// and print it on the the LCD.
clear();
print_long(read_battery_millivolts_svp());
red_led(1); // Turn on the red LED.
delay_ms(200); // Wait for 200 ms.
red_led(0); // Turn off the red LED.
delay_ms(200); // Wait for 200 ms.
}
}
int main()
{
print("Hello!");
play("L16 ceg>c");
while(1)
{
red_led(0);
green_led(1);
delay_ms(100);
red_led(1);
green_led(0);
delay_ms(100);
}
return 0;
}
int main (void) {
int on=0, ID, calb;
int batt_voltage = 0;
//init();
//ID = readID();
//calb = readCalibration();
// GPIO_SetValue(RED_LED_PORT, RED_LED_BIT,1);
int i = 0; /* Used in main loop */
uint32_t value = 0xaa;
setup_ports();
sc_time_t one_sec_timer = sc_get_timer(); /* Initialise the timer variable */
sc_time_t test_in_timer = sc_get_timer(); /* Initialise the timer variable */
/* Set LEDs to known states, i.e. on */
red_led(ON);
yellow_led(ON);
GPIO_SetValue(CAN_EN_PORT, CAN_EN_BIT, ON);
scandal_register_in_channel_handler(0, &in_channel_0_handler);
/* This is the main loop, go for ever! */
while (1) {
/* This checks whether there are pending requests from CAN, and sends a heartbeat message.
* The heartbeat message encodes some data in the first 4 bytes of the CAN message, such as
* the number of errors and the version of scandal */
handle_scandal();
/*
* scandal_send_channel(TELEM_LOW, ID_VOLT, readVoltage());
* scandal_send_channel(TELEM_LOW, ID_TEMP, readTemperature());
* scandal_send_channel(TELEM_LOW, ID_VOLT, readVoltage());
*/
if(sc_get_timer() >= one_sec_timer + 1000) {
toggle_red_led();
toggle_yellow_led();
one_sec_timer = sc_get_timer();
batt_voltage = readVoltage();
scandal_send_channel(TELEM_LOW, // priority
0, // channel num
batt_voltage // value
);
}
}
}
void test_leds()
{
while(!button_is_pressed(ALL_BUTTONS))
{
clear();
printf("Check\n");
printf("Red");
red_led(1);
play_frequency(440,50,15);
delay_ms(250);
if(button_is_pressed(ALL_BUTTONS))
break;
red_led(0);
delay_ms(250);
if(button_is_pressed(ALL_BUTTONS))
break;
clear();
printf("Check\n");
printf("Green");
green_led(1);
play_frequency(880,50,15);
delay_ms(250);
if(button_is_pressed(ALL_BUTTONS))
break;
green_led(0);
delay_ms(250);
}
while(button_is_pressed(ALL_BUTTONS));
delay_ms(100);
}
// Blinks the LEDs
void led_test()
{
play("c32");
print("Red ");
red_led(1);
if(wait_for_250_ms_or_button_b())
return;
red_led(0);
if(wait_for_250_ms_or_button_b())
return;
play(">c32");
lcd_goto_xy(0,0);
print("Green");
green_led(1);
if(wait_for_250_ms_or_button_b())
return;
green_led(0);
if(wait_for_250_ms_or_button_b())
return;
}
// *** triggered by middle button ***
// This function tests the motors by first ramping motor1 speed from zero
// to full speed "forward", to full speed "reverse", and finally back to zero.
// It then does the same for motor2 before repeating all over again.
// While motor1 is running, the red user LED is on, otherwise it is off.
// While the currently active motor is moving "forward", the green user LED
// is on, otherwise it is off. The LCD gives you feedback as to which motor
// is currently moving in which direction (F = "forward", R = "reverse", and
// - = inactive).
unsigned char motorTest()
{
unsigned char button;
int speed;
unsigned char motor = 0;
clear(); // clear the LCD, go to the start of the first LCD line
print("motor2"); // print to the first line of the LCD
lcd_goto_xy(0, 1); // go to the start of the second LCD line
print("motor1"); // print to the second line of the LCD
while (1)
{
red_led(!motor); // turn red LED on when m1 is active, off for m2
lcd_goto_xy(7, !motor); // go to end of LCD line for active motor
print("F"); // print "F" for "forward"
lcd_goto_xy(7, motor); // go to end of LCD line for inactive motor
print("-"); // print "-" for "inactive"
green_led(1); // turn green LED on when motor is moving "forward"
for (speed = 0; speed < 255; speed++)
{
button = motorUpdate(motor, speed); // ramp up motor speed
if (button != 0) // from 0 to 255
return button;
}
for (speed = 255; speed > -255; speed--)
{
if (speed == -1) // motor starts moving in "reverse"
{
green_led(0); // green LED off when motor going "reverse"
lcd_goto_xy(7, !motor); // go to end of active motor's LCD line
print("R"); // print "R" for "reverse"
}
button = motorUpdate(motor, speed); // ramp down motor speed
if (button != 0) // from 255 to -255
return button;
}
for (speed = -255; speed <= 0; speed++)
{
button = motorUpdate(motor, speed); // ramp up motor speed
if (button != 0) // from -255 to 0
return button;
}
motor = !motor; // alternate between m1 and m2
}
}
void servo() {
clear();
const unsigned char demuxPins[] = { };
servos_start(demuxPins, sizeof(demuxPins));
set_servo_target(0, 1300);
set_servo_speed(0, 0);
while (1) {
if (button_is_pressed(TOP_BUTTON)) {
red_led(1);
green_led(0);
clear();
print("Moving Left...");
set_servo_target(0, 0);
_delay_ms(400);
set_servo_target(0, 200);
}
if (button_is_pressed(BOTTOM_BUTTON)) {
red_led(0);
green_led(1);
clear();
print("Moving Right...");
set_servo_target(0, 0);
_delay_ms(400);
set_servo_target(0, 1800);
}
}
}
// process_received_byte: Responds to a byte that has been received on
// USB_COMM. If you are writing your own serial program, you can
// replace all the code in this function with your own custom behaviors.
void process_received_byte(char byte)
{
clear(); // clear LCD
print("RX: ");
print_character(byte);
lcd_goto_xy(0, 1); // go to start of second LCD row
switch(byte)
{
// If the character 'G' or 'g' is received, toggle the green LED.
case 'G':
case 'g':
green_led(TOGGLE);
print("green LED");
break;
// If the character 'R' or 'r' is received, toggle the red LED.
case 'R':
case 'r':
red_led(TOGGLE);
print("red LED");
break;
// If the character 'C' or 'c' is received, play the note C.
case 'C':
case 'c':
play_from_program_space(PSTR("c16"));
print("play note C");
break;
// If the character 'D' or 'd' is received, play the note D.
case 'D':
case 'd':
play_from_program_space(PSTR("d16"));
print("play note D");
break;
// If any other character is received, change its capitalization and
// send it back.
default:
wait_for_sending_to_finish();
send_buffer[0] = byte ^ 0x20;
serial_send(USB_COMM, send_buffer, 1);
print("TX: ");
print_character(send_buffer[0]);
break;
}
}
int main()
{
// Set mode to CTC
// WGM2 = 0
// WGM1 = 1
// WGM0 = 0
TCCR0A &= ~(0 << WGM00);
TCCR0A |= (1 << WGM01);
TCCR0B &= ~(0 << WGM02);
//Set the scaler to 256
// CS00 = 0
// CS01 = 0
// CS02 = 1
TCCR0B &= ~(0 << CS00);
TCCR0B &= ~(0 << CS01);
TCCR0B |= (1 << CS02);
//Set the top
OCR0A = 78;
// Turn on global interupts
sei();
// Turn on interupt for
TIMSK0 |= (1 << OCIE0A);
clear();
while(1)
{
//lcd_goto_xy(0,0);
//print_long(msTicks);
if(redReleased)
{
redOn ^= 1;
red_led(redOn);
green_led(!redOn);
redReleased = 0;
}
}
return 0;
}
int main()
{
Led red_led(LED1, ON);
Led green_led(LED2, OFF);
Switch button(S2);
while (1) {
while (!button.is_pressed())
;
red_led.toggle();
green_led.toggle();
while (button.is_pressed())
;
}
}
void *dmr_handle_data_hook(char *pkt, int len){
/* This hook handles the dmr_contact_check() function, calling
back to the original function where appropriate.
Packes are up to twelve bytes, but they are always preceeded by
two bytes of C5000 overhead.
*/
//Turn on the red LED to know that we're here.
red_led(1);
printf("Data: ");
printhex(pkt,len+2);
printf("\n");
//Forward to the original function.
return dmr_handle_data(pkt,len);
}
// This function comprises the main part of the motor speed update loop.
// If a button press is detected, both the red and green user LEDs are
// turned off, as are the motor outputs. This function also includes
// a brief delay that ensures the entire loop takes the desired amount
// of time.
unsigned char motorUpdate(unsigned char motor, int speed)
{
unsigned char button;
if (motor == 0) // set the desired motor to the desired speed
set_m1_speed(speed);
else
set_m2_speed(speed);
delay_ms(2); // delay here for 2 milliseconds
// check if top or bottom buttons have been pressed
button = button_is_pressed(TOP_BUTTON | MIDDLE_BUTTON);
if (button != 0) // if so, turn off motors and LEDs, return button ID
{
red_led(0);
green_led(0);
set_motors(0, 0);
}
return button;
}
static portTASK_FUNCTION( vBlockingQueueConsumer, pvParameters )
{
unsigned short usData, usExpectedValue = 0;
xBlockingQueueParameters *pxQueueParameters;
short sErrorEverOccurred = pdFALSE;
pxQueueParameters = ( xBlockingQueueParameters * ) pvParameters;
for( ;; )
{
if( xQueueReceive( pxQueueParameters->xQueue, &usData, pxQueueParameters->xBlockTime ) == pdPASS )
{
if( usData != usExpectedValue )
{
/* Catch-up. */
usExpectedValue = usData;
sErrorEverOccurred = pdTRUE;
}
else
{
/* We have successfully received a message, so increment the
variable used to check we are still running. */
if( sErrorEverOccurred == pdFALSE )
{
( *pxQueueParameters->psCheckVariable )++;
}
/* Increment the value we expect to remove from the queue next time
round. */
++usExpectedValue;
play_from_program_space(PSTR("c"));
while (is_playing()){};
red_led(0); // Turn off the red LED.
delay_ms(20); // Wait for 20 ms.
}
}
}
}
/* This is your main function! You should have an infinite loop in here that
* does all the important stuff your node was designed for */
int main(void) {
/* variables for double buffer example */
char buf_1[BUFFER_SIZE];
char buf_2[BUFFER_SIZE];
char *current_buf;
// struct //UART_buffer_descriptor buf_desc_1, buf_desc_2;
setup();
scandal_init();
//UART_Init(115200);
sc_time_t one_sec_timer = sc_get_timer(); /* Initialise the timer variable */
sc_time_t test_in_timer = sc_get_timer(); /* Initialise the timer variable */
/* Set LEDs to known states */
red_led(0);
yellow_led(1);
scandal_delay(100); /* wait for the UART clocks to settle */
/* Display welcome header over UART */
//UART_printf("Welcome to the template project! This is coming out over UART1\n\r");
//UART_printf("The 2 debug LEDs should blink at a rate of 1HZ\n\r");
//UART_printf("If you configure the in channel 0, I should print a message upon receipt of such a channel message\n\r");
//UART_printf("This also shows an example of double buffer reading from the UART. Enter the text 'time' and press enter\n\r> ");
scandal_register_in_channel_handler(0, &in_channel_0_handler);
/*UART_init_double_buffer(&buf_desc_1, buf_1, BUFFER_SIZE,
&buf_desc_2, buf_2, BUFFER_SIZE);
*/
/* This is the main loop, go for ever! */
while (1) {
/* This checks whether there are pending requests from CAN, and sends a heartbeat message.
* The heartbeat message encodes some data in the first 4 bytes of the CAN message, such as
* the number of errors and the version of scandal */
handle_scandal();
// current_buf = //UART_readline_double_buffer(&buf_desc_1, &buf_desc_2);
/* //UART_readline_double_buffer will return a pointer to the current buffer. */
// if (current_buf != NULL) {
// if (strncmp("time", current_buf, 4) == 0) {
//UART_printf("The time is: %d\r\n> ", (int)sc_get_timer());
// }
// }
/* Send a UART and CAN message and flash an LED every second */
if(sc_get_timer() >= one_sec_timer + 1000) {
/* Send the message */
/* Send a channel message with a blerg value at low priority on channel 0 */
scandal_send_channel(TELEM_LOW, /* priority */
0, /* channel num */
0xaa /* value */
);
/* Twiddle the LEDs */
toggle_yellow_led();
toggle_red_led();
/* Update the timer */
one_sec_timer = sc_get_timer();
}
/* The old way of checking for an incoming message that you've registered for.
* This is a silly way to do this. A better way is to use the scandal_register_in_channel_handler
* feature. Your function will get called when a new message comes in */
if(scandal_get_in_channel_rcvd_time(TEMPLATE_TEST_IN) > test_in_timer) {
/*UART_printf("I received a channel message in the main loop on in_channel 0, value %u at time %d\n\r",
(unsigned int)scandal_get_in_channel_value(TEMPLATE_TEST_IN),
(int)scandal_get_in_channel_rcvd_time(TEMPLATE_TEST_IN)
);*/
if(scandal_get_in_channel_value(TEMPLATE_TEST_IN) == 1) {
toggle_red_led();
} else {
toggle_yellow_led();
}
test_in_timer = scandal_get_in_channel_rcvd_time(TEMPLATE_TEST_IN);
}
}
}
/* This is your main function! You should have an infinite loop in here that
* does all the important stuff your node was designed for */
int main(void) {
int initialGPSLock = 1;
char nmea_buf_1[NMEA_BUFFER_SIZE];
char nmea_buf_2[NMEA_BUFFER_SIZE];
char *nmea_current_buf;
struct UART_buffer_descriptor nmea_buf_desc_1, nmea_buf_desc_2;
gps_point cur_point;
gps_speed cur_speed;
sc_time_t cur_point_stamp = 0;
sc_time_t last_timesync_time = 0;
uint32_t gga_parse_errors = 0;
uint32_t next_baro_read=0;
uint32_t next_rtc_time=0;
/* We allow some time for the GPS to come up before we continue here */
scandal_naive_delay(100000);
setup();
scandal_naive_delay(100000);
scandal_init();
scandal_delay(1000);
/* Initialise the UART to the correct GPS baud rate */
#if defined(LOCOSYS)
UART_Init(57600);
#else
#if defined(SANJOSE)
UART_Init(38400);
#endif
#endif
scandal_delay(1000); /* wait for the UART clocks to settle */
sc_time_t one_sec_timer = sc_get_timer(); /* Initialise the timer variable */
/* Set LEDs to known states */
red_led(1);
yellow_led(0);
// Set up the barometer
long b5, pres, temp, alt, up, ut;
readCalibrationValues(); //read calibration values
/* Some GPS config stuff that isn't really necessary */
/* We can send a reset command if need be
mtk_send_command("$PMTK103"); */
/* Set which messages to send out
* This sets us to receive GPRMC and GPGGA on every position fix */
mtk_send_command("$PMTK314,0,1,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0");
/* put in 5Hz mode
mtk_send_command("$PMTK220,200"); */
/* Change the rate at which we fix the position. Presently every 1000ms */
mtk_send_command("$PMTK300,1000,0,0,0,0*");
/* We need a double buffer for reading the GPS messages while we parse them */
UART_init_double_buffer(&nmea_buf_desc_1, nmea_buf_1, NMEA_BUFFER_SIZE,
&nmea_buf_desc_2, nmea_buf_2, NMEA_BUFFER_SIZE);
/* This is the main loop, go for ever! */
while (1) {
/* This checks whether there are pending requests from CAN, and sends a heartbeat message.
* The heartbeat message encodes some data in the first 4 bytes of the CAN message, such as
* the number of errors and the version of scandal */
handle_scandal();
if (sc_get_timer() > next_rtc_time){
ReadTime();
UART_printf("Scandal Get Timer: %d\r\n", sc_get_timer_1());
next_rtc_time+=250;
}
/* Read a line from the UART into one of the buffers */
nmea_current_buf = UART_readline_double_buffer(&nmea_buf_desc_1, &nmea_buf_desc_2);
/* UART_readline_double_buffer will return a pointer to the current buffer. */
if (nmea_current_buf != NULL) {
/* If we didn't get a valid NMEA line */
if(validate_nmea(nmea_current_buf) != 0)
continue;
/* check to see if we have a GGA message */
if(strncmp(nmea_current_buf, "$GPGGA", 6) == 0){
int res = parse_msg_gga(nmea_current_buf, &cur_point);
if(res == 0){
if(initialGPSLock == 1) {
// Syncs GPS clock with RTC
SetTime(get_gga_time_array());
SetDate(get_rmc_date_array());
initialGPSLock = 0;
}
/*If the GPS is locked (res==0), send GPS data and set the RTC*/
toggle_yellow_led();
cur_point_stamp = scandal_get_realtime32();
scandal_send_channel_with_timestamp(CRITICAL_PRIORITY, GPSBAROMETER_FIX,
1, cur_point_stamp);
scandal_send_channel_with_timestamp(CRITICAL_PRIORITY, GPSBAROMETER_TIME,
cur_point.time, cur_point_stamp);
scandal_send_channel_with_timestamp(CRITICAL_PRIORITY, GPSBAROMETER_LATITUDE,
cur_point.lat, cur_point_stamp);
scandal_send_channel_with_timestamp(CRITICAL_PRIORITY, GPSBAROMETER_LONGITUDE,
cur_point.lng, cur_point_stamp);
scandal_send_channel_with_timestamp(CRITICAL_PRIORITY, GPSBAROMETER_ALTITUDE,
cur_point.alt, cur_point_stamp);
/* Only sets the RTC if the seconds value is under 58
* This is done to make sure nothing ticks over in the
* middle of a write process as strange values may result
*/
/* an actual parse error */
} else if (res == -1) {
scandal_send_channel_with_timestamp(CRITICAL_PRIORITY, GPSBAROMETER_GGA_PARSE_ERROR_COUNT,
gga_parse_errors++, cur_point_stamp);
/* no fix yet */
} else if (res == -2) {
scandal_send_channel_with_timestamp(CRITICAL_PRIORITY, GPSBAROMETER_FIX,
0, cur_point_stamp);
//gps_lock=0;
}
}
/* check to see if we have an RMC message */
if(strncmp(nmea_current_buf, "$GPRMC", 6) == 0) {
if(parse_msg_rmc(nmea_current_buf, &cur_speed) == 0) {
toggle_red_led();
/* Milliseconds since the epoch */
/* Converts date to milliseconds, adds time */
uint64_t timestamp = days_since_epoch(getRTCDay(), getRTCMonth(), getRTCYear());
timestamp *= 3600;
timestamp *= 24;
timestamp *= 1000;
timestamp += getRTCTimeSecond() * 1000;
timestamp += sc_get_timer_1();
UART_printf("Day:%d Month:%d Year:%d\r\n", getRTCDay(), getRTCMonth(), getRTCYear());
scandal_send_timesync(CRITICAL_PRIORITY, scandal_get_addr(), timestamp);
scandal_set_realtime(timestamp);
scandal_send_channel(CRITICAL_PRIORITY, GPSBAROMETER_SPEED, cur_speed.speed * 1000);
scandal_send_channel(CRITICAL_PRIORITY, GPSBAROMETER_MILLISECONDS_TODAY, cur_speed.time);
scandal_send_channel(CRITICAL_PRIORITY, GPSBAROMETER_DAYS_SINCE_EPOCH, cur_speed.date);
/* This is an evil hack to make sure that we get fairly consistent timestamps
Sometimes there seems to be a really long dela of ~0.3s on some timestamp
messages. To get rid of this, we don't accept any time differences that are
more than 20ms later than we expect them to be.
This is pure evil, and the problem should really be fixed rather than
hacking around it like this */
/*
if(last_timesync_time == 0)
last_timesync_time = sc_get_timer();
timediff = (sc_get_timer() - last_timesync_time) % 1000;
if((timediff < 50) || (timediff > 600)) {
last_timesync_time = sc_get_timer();
scandal_send_timesync(CRITICAL_PRIORITY, scandal_get_addr(), timestamp);
}
scandal_set_realtime(timestamp);
scandal_send_channel(CRITICAL_PRIORITY, GPSBAROMETER_SPEED, cur_speed.speed * 1000);
scandal_send_channel(CRITICAL_PRIORITY, GPSBAROMETER_MILLISECONDS_TODAY, cur_speed.time);
scandal_send_channel(CRITICAL_PRIORITY, GPSBAROMETER_DAYS_SINCE_EPOCH, cur_speed.date);
*/
/* if(cur_point_stamp != 0){
scandal_send_channel_with_timestamp(CRITICAL_PRIORITY, GPSBAROMETER_TIME,
cur_point.time, cur_point_stamp);
scandal_send_channel_with_timestamp(CRITICAL_PRIORITY, GPSBAROMETER_LATITUDE,
cur_point.lat, cur_point_stamp);
scandal_send_channel_with_timestamp(CRITICAL_PRIORITY, GPSBAROMETER_LONGITUDE,
cur_point.lng, cur_point_stamp);
scandal_send_channel_with_timestamp(CRITICAL_PRIORITY, GPSBAROMETER_ALTITUDE,
cur_point.alt, cur_point_stamp);
cur_point_stamp = 0;
}
*/
}
}
}
#if 1
if (sc_get_timer() > next_baro_read){
ut = bmp085ReadUT(); //read uncompensated temperature
scandal_delay(5); //delay 4.5ms
up = bmp085ReadUP(); //read uncompensated pressure
scandal_delay(5); //delay 4.5ms
b5 = bmp085Getb5(ut); //calculate temperature constant
temp = bmp085GetTemperature(ut, b5); //calculate true temperature
pres = bmp085GetPressure(up, b5); //calculate true pressure
alt = bmp085GetAltitude(pres); //estimate the altitude
next_baro_read=(sc_get_timer()+1000);
}
#endif
//UART_printf("B5: %d,Temp: %d:%d, Pres: %d:%d, Alt: %d\r\n", (int) b5,(int) ut, (int) temp, (int)up,(int) pres, (int) alt);
// UART_printf("%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d\r\n",(int) barometerCal -> AC1,(int) barometerCal -> AC2,(int) barometerCal -> AC3,(int) barometerCal -> AC4,(int) barometerCal -> AC5,(int) barometerCal -> AC6,(int) barometerCal -> B1,(int) barometerCal -> B2,(int) barometerCal -> MB,(int) barometerCal -> MC,(int) barometerCal -> MD);
#if 0
/* Flash an LED every second */
if(sc_get_timer() >= one_sec_timer + 1000) {
toggle_red_led();
one_sec_timer = sc_get_timer();
}
#endif
}
}
