/* flash a LED. R, Y, B selects the appropriate LED. time is in 10ms intervals */
void flash_LED(uint8_t ontime, uint8_t offtime, char LED)
{
if(LED_tick <= ontime)
{
switch_LED(1,LED);
}else{
switch_LED(0,LED);
}
if(LED_tick >= offtime)
{
LED_tick = 0;
}
}
/* check 12V supply voltage and warn user if it is too low */
void check_supply_voltage(void)
{
/*
* Voltage threshold: Device is powered by a lead battery.
* Warn user if voltage drops below 11.5V
* Increase warning intensity if voltage drops below 11V
*
* The input voltage divider is formed by a 100k and a 22k resistor.
*
* | V_in | V_div | ADC |
* | 13.8V | 2.49V | 249 |
* | 11.5V | 2.07V | 207 |
* | 11.0V | 1.98V | 198 |
*
*/
uint8_t ADC_result = read_ADC();
int8_t corr = 7; //correction for non-ideal resistors in the divider
if(ADC_result > (207+corr))
{
switch_LED(0,'R'); //Everything is fine, no need to warn
}
else if(ADC_result > (198+corr))
{
flash_LED(2,100,'R'); //Supply voltage is low, check battery charger
}else
{
flash_LED(2,30,'R'); //Supply voltage is dangerously low, user must act to prevent battery damage
}
}
uint8_t GSM_get_str(char start, char end, uint8_t value)
{
GSM.begin = start;
GSM.end = end;
GSM.status = WAITING;
set_time_out(value);
while(GSM.status != DONE
&& GSM.status != BUFFER_FULL
&& !timer1.time_out);
if(timer1.time_out)
{
return FALSE;
}
else if(GSM.status == BUFFER_FULL)
{
switch_LED(LED2, ON);
return FALSE;
}
return TRUE;
}
void initialise_board(void) {
int i;
central_data_t *central_data=central_data_get_pointer_to_struct();
irq_initialize_vectors();
cpu_irq_enable();
Disable_global_interrupt();
// Initialize the sleep manager
sleepmgr_init();
sysclk_init();
INTC_init_interrupts();
delay_init(sysclk_get_cpu_hz());
time_keeper_init();
// time_keeper_init_synchronisation();
init_LEDs();
switch_LED(1, LED_ON);
switch_LED(0, LED_ON);
pwm_servos_init(true);
// set up UART for main telemetry
uart_int_set_usart_conf(0, asv_xbee_uart_conf());
uart_int_init(0);
uart_int_register_write_stream(uart_int_get_uart_handle(0), ¢ral_data->xbee_out_stream);
//init_UART_DMA(0);
//register_write_stream_dma(get_UART_handle(0), ¢ral_data->xbee_out_stream);
buffer_make_buffered_stream(&(central_data->xbee_in_buffer), &(central_data->xbee_in_stream));
uart_int_register_read_stream(uart_int_get_uart_handle(0), &(central_data->xbee_in_stream));
uart_int_set_usart_conf(4, asv_debug_uart_conf());
uart_int_init(4);
uart_int_register_write_stream(uart_int_get_uart_handle(4), ¢ral_data->wired_out_stream);
//init_UART_DMA(0);
//register_write_stream_dma(get_UART_handle(0), ¢ral_data->wired_out_stream);
buffer_make_buffered_stream(&(central_data->wired_in_buffer), &(central_data->wired_in_stream));
uart_int_register_read_stream(uart_int_get_uart_handle(4), &(central_data->wired_in_stream));
// set up UART for GPS
uart_int_set_usart_conf(3, asv_gps_uart_conf());
uart_int_init(3);
uart_int_register_write_stream(uart_int_get_uart_handle(3), ¢ral_data->gps_out_stream);
buffer_make_buffered_stream(&(central_data->gps_in_buffer), &(central_data->gps_in_stream));
uart_int_register_read_stream(uart_int_get_uart_handle(3), &(central_data->gps_in_stream));
multistream_init(¢ral_data->multi_telemetry_down_stream);
multistream_init(¢ral_data->multi_telemetry_up_stream);
multistream_add_stream(¢ral_data->multi_telemetry_down_stream, ¢ral_data->wired_out_stream);
multistream_add_stream(¢ral_data->multi_telemetry_down_stream, ¢ral_data->xbee_out_stream);
multistream_add_stream(¢ral_data->multi_telemetry_up_stream, ¢ral_data->wired_in_stream);
multistream_add_stream(¢ral_data->multi_telemetry_up_stream, ¢ral_data->xbee_in_stream);
/*
can_bus_init(1, asv_can1_conf());
can_bus_register_write_stream(¢ral_data->ext_can_out_stream_data, ¢ral_data->ext_can_out_stream, 1, MAVLINK_COMPONENT_ID, 0x3ff);
can_bus_register_read_stream(¢ral_data->ext_can_in_stream_data, ¢ral_data->ext_can_in_stream, 1, MAVLINK_COMPONENT_ID, 0x00);
*/
// set telemetry stream for mavlink
//central_data->telemetry_down_stream=&(central_data->xbee_out_stream);
//central_data->telemetry_up_stream =&(central_data->xbee_in_stream);
//central_data->telemetry_down_stream=&(central_data->wired_out_stream);
//central_data->telemetry_up_stream =&(central_data->wired_in_stream);
central_data->telemetry_down_stream= multistream_get_stream(¢ral_data->multi_telemetry_down_stream);
central_data->telemetry_up_stream = multistream_get_stream(¢ral_data->multi_telemetry_up_stream);
central_data->debug_out_stream=¢ral_data->wired_out_stream;
central_data->debug_in_stream=¢ral_data->wired_in_stream;
//central_data->debug_out_stream = ¢ral_data->xbee_out_stream;
//central_data->debug_in_stream = ¢ral_data->xbee_in_stream;
print_util_dbg_print_init(central_data->debug_out_stream);
// init error handler as plain text until mavlink is configured
error_handler_init_plaintext(central_data->debug_out_stream);
// init mavlink
// Init mavlink communication
mavlink_communication_conf_t mavlink_config =
{
.scheduler_config =
{
.max_task_count = 30,
.schedule_strategy = ROUND_ROBIN,
.debug = true
},
.mavlink_stream_config =
{
.rx_stream = central_data->telemetry_up_stream,
.tx_stream = central_data->telemetry_down_stream,
.sysid = MAVLINK_SYS_ID,
.compid = MAVLINK_COMPONENT_ID,
.use_dma = false
},
.message_handler_config =
int main(void)
{
setup();
uint16_t increment = 0;
//DDRD &= ~(1<<PD4); no longer needed
//PORTD |= (1<<PD4);
while(1)
{
/* measure supply voltage and warn user if voltage drops too low */
if(ISR_flag.check_supply) //measure supply voltage every 10ms
{
check_supply_voltage();
ISR_flag.check_supply = 0; //clear flag
}
/* poll the inputs and look for any falling edges indicating button presses or other input signals */
if(ISR_flag.check_inputs)
{
poll_inputs();
ISR_flag.check_inputs = 0; //clear flag
/* Detect a long button press by incrementing a variable every ms the button is pressed
* This is necessary to avoid false triggering due to EMI from the motor lines.
*/
if(!(DOOR_BUTTON_PINREG & (1<<DOOR_BUTTON_PIN)))
{
increment++;
}else{
increment = 0; // reset the count if the button is released.
}
}
/* check for a 1000ms long button press
* Again, this is necessary to avoid false triggering due to motor interference
*/
if(increment >= 1000)
{
increment = 0;
command.delayed_lock = 1; //activate the delayed lock sequence
delayed_lock_tick = 0; //reset the timer for the delayed lock sequence
}
/* check, if status is unknown or a reference command has been issued
* If that is the case, ignore any other commands and find the reference point first!
*
* Note, that command.reference is not implemented yet. There was no need for this until now (10/Aug/15)
*/
if( ((!status.locked) && (!status.unlocked)) || (command.reference) )
{
if((command.lock)||(command.unlock)||(command.reference))
{
if(input_status_current[6]) //find out, if gear magnet is already near the reed contact
{
find_reference(0,500,2000);
}else{
find_reference(1,500,2000);
}
command.lock = 0;
command.unlock = 0;
command.reference = 0;
command.delayed_lock = 0;
}
}else
{
/* open the door */
if (command.unlock && !command.block_unlock && !command.reference)
{
command.block_lock = 1; //prevent any other lock commands while this is active
enable_stepper(1);
if(!status.open) //Door is not open yet, move to "open" position
{
if(move_to_position(OPEN)==1) //set status.open once at "open" position
{
status.open = 1;
}
ms_100_tick = 0; //reset the 100ms tick until at "open" position
}else if(ms_100_tick<=35)
{
//wait 3.5s at "open" position before turning back to "neutral". This lets the user open the door by pushing it
}
if((ms_100_tick>35)&&(status.open)) //Door is "open" and we waited for 3.5s
{
if(move_to_position(NEUTRAL)==1) //move back to "neutral" position and reset all status and command bits.
{
command.unlock = 0;
command.block_lock = 0;
status.open = 0;
status.locked = 0;
status.unlocked = 1;
status.error = 0;
status.reached_endstop = 0;
command.delayed_lock = 0;
enable_stepper(0);
}
}
} else
/* lock the door */
if (command.lock && !command.block_lock && !command.reference)
{
/* locking is the same as a reference turn, so we use every lock command to re-reference our position
* to account for stupid users turning the gear by hand
*/
enable_stepper(1);
command.block_unlock = 1; //block any other actions while locking the door
if(!status.reached_endstop && (position > (-2000))) //turn CCW until either endstop is reached or 2000 steps have passed
{
move_to_position(-2000);
ms_100_tick = 0;
}
else if (position <= (-2000)) //2000 steps have passed without reaching the endstop -> something went wrong
{
command.lock = 0;
command.block_unlock = 0;
status.locked = 0;
status.unlocked = 0;
status.error = 1;
command.delayed_lock = 0;
enable_stepper(0);
}
else if(ms_100_tick<=1){ //endstop reached, reset the position to 0 while waiting a bit
position = 0;
}
if((ms_100_tick>3)&&(status.reached_endstop)) //return to "neutral" position after 300ms
{
if(move_to_position(NEUTRAL)==1) //reset status and command bits after coming back to the "neutral" position
{
command.lock = 0;
command.block_unlock = 0;
status.reached_endstop = 0;
status.locked = 1;
status.unlocked = 0;
status.error = 0;
enable_stepper(0);
command.delayed_lock = 0;
}
}
}
else
/* someone locked the door by hand and triggered the endstop. Set status to "locked" */
if (!command.lock && !command.unlock && !command.reference)
{
if(status.reached_endstop)
{
status.open = 0;
status.unlocked = 0;
status.locked = 1;
}
}
}
/* delayed lock has been triggered. Start the countdown */
if (command.delayed_lock && status.unlocked && !status.open && !command.block_unlock && !command.block_lock && !command.lock && !command.unlock)
{
if(delayed_lock_tick < 150) //wait for 15s and set "status.delayed_lock". This triggers the flashing yellow LED
{
status.delayed_lock = 1;
switch_LED(0,'B');
}else
{
status.delayed_lock = 0;
command.delayed_lock = 0;
command.lock = 1; //15s have passed, give the "lock" command to lock the door
}
}
/* Switch LEDs according to the status bits.
*
* This needs to be streamlined, it is way to complicated
*/
if(status.delayed_lock) //Override any other LED configurations while "status.delayed_lock" is active
{
flash_LED(25,(180-delayed_lock_tick),'Y');
switch_LED(0,'B');
}else
{
if(status.error || ( (!status.unlocked)&&(!status.locked)))
{
flash_LED(10,20,'Y');
flash_LED(10,20,'B');
set_status('E');
}
if(status.open)
{
flash_LED(25,40,'B');
switch_LED(0,'Y');
set_status('U');
}
if(status.unlocked && !status.open)
{
switch_LED(0,'Y');
switch_LED(1,'B');
set_status('U');
}
if(status.locked && !status.open)
{
switch_LED(0,'B');
switch_LED(1,'Y');
set_status('L');
}
}
}
}
