inline static
void main_event_loop(void)
{
/** Frame parser FSM state */
typedef enum {
STF_MAGIC,
STF_COMMAND,
STF_LENGTH,
STF_PARAM,
STF_CHECKSUM0,
STF_CHECKSUM1,
} frame_state_t;
frame_state_t fstate = STF_MAGIC;
/** Frame parser offset/index into magic/data */
uint8_t idx = 0;
/** Frame parser cached data for current frame */
uint8_t cmd = 0;
/** Frame parser cached data for current frame */
uint8_t len = 0;
/** The UART checksum initialization code in uart-comm.c runs
* "uart_recv_checksum_reset()", i.e. we do not need to run it here
* a second time for the initial state.
*/
/* Firmware FSM State */
firmware_state_t pstate = STP_READY;
/* Globally enable interrupts */
sei();
/* Firmware main event loop */
while (1) {
/* check for "measurement finished" event */
if (GF_ISSET(GF_MEASUREMENT_FINISHED)) {
pstate = firmware_handle_measurement_finished(pstate);
GF_CLEAR(GF_MEASUREMENT_FINISHED);
continue;
}
/* check whether a key event occured */
if (switch_trigger_measurement()) {
pstate = firmware_handle_switch_pressed(pstate);
continue;
}
/* check whether a byte has arrived via UART */
if (bit_is_set(UCSR0A, RXC0)) {
/* The loop condition from uart_getc() checking UCSR0A for RCX0
* has just been checked here, so we directly read UDR0 instead
* of calling uart_getc(). */
const uint8_t byte = (uint8_t) UDR0;
uart_recv_checksum_update(byte);
frame_state_t next_fstate = fstate;
switch (fstate) {
case STF_MAGIC:
if (byte == pgm_read_byte_near(&(magic_header[idx++]))) {
if (idx < 4) {
next_fstate = STF_MAGIC;
} else {
next_fstate = STF_COMMAND;
}
} else {
/* syncing, not an error */
goto restart;
}
break;
case STF_COMMAND:
cmd = byte;
next_fstate = STF_LENGTH;
break;
case STF_LENGTH:
len = byte;
if (pstate == STP_READY) {
/* We can only use the personality_param_sram buffer in the
* STP_READY state. By not writing to the buffer after
* transitioning from STP_READY, we keep the content of the
* buffer from the "start measurement" command for sending
* back later.
*/
pparam_sram.length = len;
}
if (len == 0) {
next_fstate = STF_CHECKSUM0;
} else if ((len >= personality_param_size) &&
(len < MAX_PARAM_LENGTH)) {
idx = 0;
next_fstate = STF_PARAM;
} else {
/* whoever sent us that wrongly sized data frame made an error */
/** \todo Find a way to report errors without resorting to
* sending free text. */
send_text_P(PSTR("param length mismatch"));
goto error_restart_nomsg;
}
break;
case STF_PARAM:
if (pstate == STP_READY) {
/* We can only use the personality_param_sram buffer in the
* STP_READY state. By not writing to the buffer after
* transitioning from STP_READY, we keep the content of the
* buffer from the "start measurement" command for sending
* back later.
*/
pparam_sram.params[idx] = byte;
}
idx++;
if (idx < len) {
next_fstate = STF_PARAM;
} else {
next_fstate = STF_CHECKSUM0;
}
break;
case STF_CHECKSUM0:
/* We do not need to explicitly store the checksum here. A
* checksum match will happen implicitly when the checksum
* accumulator turns 0 which the code in uart-comm.c will
* detect for us.
*/
next_fstate = STF_CHECKSUM1;
break;
case STF_CHECKSUM1:
/* We do not need to explicitly store the checksum here. A
* checksum match will happen implicitly when the checksum
* accumulator turns 0 which the code in uart-comm.c will
* detect for us.
*/
if (uart_recv_checksum_matches()) {
/* checksum successful */
pstate = firmware_handle_command(pstate, cmd);
goto restart;
} else {
/** \todo Find a way to report checksum failure without
* resorting to sending free text. */
send_text_P(PSTR("checksum fail"));
goto error_restart_nomsg;
}
break;
}
//uprintf("idx=%u", idx);
goto skip_errors;
error_restart_nomsg:
restart:
next_fstate = STF_MAGIC;
idx = 0;
uart_recv_checksum_reset();
skip_errors:
fstate = next_fstate;
continue;
} /* character received on UART */
} /* while (1) main event loop */
} /* void main_event_loop(void); */
int main(void)
{
initIO();
while (1)
{
if (TCNT1<2000)
{
}
if ((TCNT1>=2000) && (TCNT1<=5500) && (dataOK == 1))
{
//reset channels from high to low
if (TCNT1>=CH1_out && bit_is_set(PORTB, PIN0)) PORTB &= ~(1<<PIN0);
if (TCNT1>=CH2_out && bit_is_set(PORTB, PIN1)) PORTB &= ~(1<<PIN1);
if (TCNT1>=CH3_out && bit_is_set(PORTB, PIN2)) PORTB &= ~(1<<PIN2);
if (TCNT1>=CH4_out && bit_is_set(PORTB, PIN3)) PORTB &= ~(1<<PIN3);
if (TCNT1>=CH5_out && bit_is_set(PORTB, PIN4)) PORTB &= ~(1<<PIN4);
if (TCNT1>=CH6_out && bit_is_set(PORTD, PIN0)) PORTD &= ~(1<<PIN0);
if (TCNT1>=CH7_out && bit_is_set(PORTD, PIN1)) PORTD &= ~(1<<PIN1);
if (TCNT1>=CH8_out && bit_is_set(PORTD, PIN2)) PORTD &= ~(1<<PIN2);
if (TCNT1>=CH9_out && bit_is_set(PORTD, PIN3)) PORTD &= ~(1<<PIN3);
if (TCNT1>=CH10_out && bit_is_set(PORTD, PIN4)) PORTD &= ~(1<<PIN4);
if (TCNT1>=CH11_out && bit_is_set(PORTD, PIN5)) PORTD &= ~(1<<PIN5);
if (TCNT1>=CH12_out && bit_is_set(PORTD, PIN6)) PORTD &= ~(1<<PIN6);
}
if ((TCNT1>5500) && (TCNT1<35000))
{
CH1_out = uniq(rxbuffer[1], rxbuffer[2]);
CH2_out = uniq(rxbuffer[3], rxbuffer[4]);
CH3_out = uniq(rxbuffer[5], rxbuffer[6]);
CH4_out = uniq(rxbuffer[7], rxbuffer[8]);
CH5_out = uniq(rxbuffer[9], rxbuffer[10]);
CH6_out = uniq(rxbuffer[11], rxbuffer[12]);
CH7_out = uniq(rxbuffer[13], rxbuffer[14]);
CH8_out = uniq(rxbuffer[15], rxbuffer[16]);
CH9_out = uniq(rxbuffer[17], rxbuffer[18]);
CH10_out = uniq(rxbuffer[19], rxbuffer[20]);
CH11_out = uniq(rxbuffer[21], rxbuffer[22]);
CH12_out = uniq(rxbuffer[23], rxbuffer[24]);
//ready for output
dataOK = 1;
}
}
return 0; // never reached
}
void adns9500_checks(void)
{
uint8_t it;
uint8_t byte;
// For each ADNS
for(it=1;it<=ADNS9500_NUM;it++)
{
// Set current ADNS CS active
adns9500_spi_cs(it);
_delay_us(ADNS9500_TIMINGS_NCS_SCLK);
//-------------------------------
// Read Motion register
// Read register current value
adns9500_spi_send(ADNS9500_SPIREGISTER_MOTION);
_delay_us(ADNS9500_TIMINGS_SRAD);
byte = adns9500_spi_recv();
DEBUG(ADNS9500_ERROR,"ADNS9500 #%u motion=0x%X",it,byte);
// Check if ADNS is Fault, if true, error.
if( bit_is_set(byte,ADNS9500_MOTION_BIT_FAULT) )
{
adns9500_spi_cs(0);
ERROR(ADNS9500_ERROR,
"ADNS9500 #%u : ADNS is fault, motion=0x%X",
it,byte);
}
// Check LP_CFG* consistency, if not, error.
if( !bit_is_set(byte,ADNS9500_MOTION_BIT_LPVALID) )
{
adns9500_spi_cs(0);
ERROR(ADNS9500_ERROR,
"ADNS9500 #%u : LP_CFG* inconsistent, motion=0x%X",
it,byte);
}
//-------------------------------
// Read Observation register
// delay read-write
_delay_us(ADNS9500_TIMINGS_SRWSRR);
// Clear observation register
adns9500_spi_send(ADNS9500_SPI_WRITE|ADNS9500_SPIREGISTER_OBSERVATION);
adns9500_spi_send(0x00);
// wait for new frame
//(DS don't exactly specify delay here, one frame seems to be max refresh time)
_delay_us(ADNS9500_TIMINGS_FRAME_PERIOD);
_delay_us(ADNS9500_TIMINGS_FRAME_PERIOD);
// Read register current value
adns9500_spi_send(ADNS9500_SPIREGISTER_OBSERVATION);
_delay_us(ADNS9500_TIMINGS_SRAD);
byte = adns9500_spi_recv();
DEBUG(ADNS9500_ERROR,"ADNS9500 #%u observation=0x%X",it,byte);
// Check if ADNS is running on SROM code, if not, error.
if( !bit_is_set(byte,ADNS9500_OBSERVATION_BIT_OB7) )
{
adns9500_spi_cs(0);
ERROR(ADNS9500_ERROR,
"ADNS9500 #%u : ADNS is not running on SROM code, observation=0x%X",
it,byte);
}
// Check if NPD pulse was detected, if true, error.
if( bit_is_set(byte,ADNS9500_OBSERVATION_BIT_OB5) )
{
adns9500_spi_cs(0);
ERROR(ADNS9500_ERROR,
"ADNS9500 #%u : NPD pulse detected, observation=0x%X",
it,byte);
}
//------------------------------------------------
// Good to GO !
adns9500_spi_cs(0);
} /* for(it=1;it<=ADNS9500_NUM;it++) */
}
char uart_read_char()
{
while(!bit_is_set(UCSR0A, RXC0));
return UDR0;
}
/*
* Request SD to send us the slot:barcodes, then wiffle
* through them all labeling them.
*/
static void label_from_barcodes(UAContext *ua, int drive, bool label_encrypt)
{
STORERES *store = ua->jcr->res.wstore;
POOL_DBR pr;
MEDIA_DBR mr, omr;
vol_list_t *vl;
dlist *vol_list = NULL;
bool media_record_exists;
char *slot_list;
int max_slots;
max_slots = get_num_slots_from_SD(ua);
if (max_slots <= 0) {
ua->warning_msg(_("No slots in changer to scan.\n"));
return;
}
slot_list = (char *)malloc(nbytes_for_bits(max_slots));
clear_all_bits(max_slots, slot_list);
if (!get_user_slot_list(ua, slot_list, "slots", max_slots)) {
goto bail_out;
}
vol_list = get_vol_list_from_SD(ua, store, false /* no listall */ , false /*no scan*/);
if (!vol_list) {
ua->warning_msg(_("No Volumes found to label, or no barcodes.\n"));
goto bail_out;
}
/*
* Display list of Volumes and ask if he really wants to proceed
*/
ua->send_msg(_("The following Volumes will be labeled:\n"
"Slot Volume\n"
"==============\n"));
foreach_dlist(vl, vol_list) {
if (!vl->VolName || !bit_is_set(vl->Slot - 1, slot_list)) {
continue;
}
ua->send_msg("%4d %s\n", vl->Slot, vl->VolName);
}
if (!get_yesno(ua, _("Do you want to label these Volumes? (yes|no): ")) ||
(ua->pint32_val == 0)) {
goto bail_out;
}
/*
* Select a pool
*/
memset(&pr, 0, sizeof(pr));
if (!select_pool_dbr(ua, &pr)) {
goto bail_out;
}
/*
* Fire off the label requests
*/
foreach_dlist(vl, vol_list) {
if (!vl->VolName || !bit_is_set(vl->Slot - 1, slot_list)) {
continue;
}
mr.clear();
bstrncpy(mr.VolumeName, vl->VolName, sizeof(mr.VolumeName));
media_record_exists = false;
if (db_get_media_record(ua->jcr, ua->db, &mr)) {
if (mr.VolBytes != 0) {
ua->warning_msg(_("Media record for Slot %d Volume \"%s\" already exists.\n"),
vl->Slot, mr.VolumeName);
mr.Slot = vl->Slot;
mr.InChanger = mr.Slot > 0; /* if slot give assume in changer */
set_storageid_in_mr(store, &mr);
if (!db_update_media_record(ua->jcr, ua->db, &mr)) {
ua->error_msg(_("Error setting InChanger: ERR=%s"), db_strerror(ua->db));
}
continue;
}
media_record_exists = true;
}
mr.InChanger = mr.Slot > 0; /* if slot give assume in changer */
set_storageid_in_mr(store, &mr);
/*
* Deal with creating cleaning tape here. Normal tapes created in send_label_request() below
*/
if (is_cleaning_tape(ua, &mr, &pr)) {
if (media_record_exists) { /* we update it */
mr.VolBytes = 1; /* any bytes to indicate it exists */
bstrncpy(mr.VolStatus, "Cleaning", sizeof(mr.VolStatus));
mr.MediaType[0] = 0;
set_storageid_in_mr(store, &mr);
if (!db_update_media_record(ua->jcr, ua->db, &mr)) {
ua->error_msg("%s", db_strerror(ua->db));
}
} else { /* create the media record */
if (pr.MaxVols > 0 && pr.NumVols >= pr.MaxVols) {
ua->error_msg(_("Maximum pool Volumes=%d reached.\n"), pr.MaxVols);
goto bail_out;
}
set_pool_dbr_defaults_in_media_dbr(&mr, &pr);
bstrncpy(mr.VolStatus, "Cleaning", sizeof(mr.VolStatus));
mr.MediaType[0] = 0;
set_storageid_in_mr(store, &mr);
if (db_create_media_record(ua->jcr, ua->db, &mr)) {
ua->send_msg(_("Catalog record for cleaning tape \"%s\" successfully created.\n"),
mr.VolumeName);
pr.NumVols++; /* this is a bit suspect */
if (!db_update_pool_record(ua->jcr, ua->db, &pr)) {
ua->error_msg("%s", db_strerror(ua->db));
}
} else {
ua->error_msg(_("Catalog error on cleaning tape: %s"), db_strerror(ua->db));
}
}
continue; /* done, go handle next volume */
}
bstrncpy(mr.MediaType, store->media_type, sizeof(mr.MediaType));
/*
* See if we need to generate a new passphrase for hardware encryption.
*/
if (label_encrypt) {
if (!generate_new_encryption_key(ua, &mr)) {
continue;
}
}
mr.Slot = vl->Slot;
send_label_request(ua, &mr, &omr, &pr, false, media_record_exists, drive);
}
bail_out:
free(slot_list);
if (vol_list) {
free_vol_list(vol_list);
}
close_sd_bsock(ua);
return;
}
void blink2(void) {
if (bit_is_set(PORTA,PA2)) cbi(PORTA,PA2); else sbi(PORTA,PA2);
}
void blink(void) {
if (bit_is_set(PORTA,PA0)) cbi(PORTA,PA0); else sbi(PORTA,PA0);
}
void stateCalcPositionNed_i(void)
{
if (bit_is_set(state.pos_status, POS_NED_I)) {
return;
}
int errno = 0;
if (state.ned_initialized_i) {
if (bit_is_set(state.pos_status, POS_NED_F)) {
NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f);
} else if (bit_is_set(state.pos_status, POS_ENU_I)) {
INT32_VECT3_NED_OF_ENU(state.ned_pos_i, state.enu_pos_i);
} else if (bit_is_set(state.pos_status, POS_ENU_F)) {
ENU_BFP_OF_REAL(state.enu_pos_i, state.enu_pos_f);
SetBit(state.pos_status, POS_ENU_I);
INT32_VECT3_NED_OF_ENU(state.ned_pos_i, state.enu_pos_i);
} else if (bit_is_set(state.pos_status, POS_ECEF_I)) {
ned_of_ecef_pos_i(&state.ned_pos_i, &state.ned_origin_i, &state.ecef_pos_i);
} else if (bit_is_set(state.pos_status, POS_ECEF_F)) {
/* transform ecef_f -> ned_f, set status bit, then convert to int */
ned_of_ecef_point_f(&state.ned_pos_f, &state.ned_origin_f, &state.ecef_pos_f);
SetBit(state.pos_status, POS_NED_F);
NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f);
} else if (bit_is_set(state.pos_status, POS_LLA_F)) {
/* transform lla_f -> ecef_f -> ned_f, set status bits, then convert to int */
ecef_of_lla_f(&state.ecef_pos_f, &state.lla_pos_f);
SetBit(state.pos_status, POS_ECEF_F);
ned_of_ecef_point_f(&state.ned_pos_f, &state.ned_origin_f, &state.ecef_pos_f);
SetBit(state.pos_status, POS_NED_F);
NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f);
} else if (bit_is_set(state.pos_status, POS_LLA_I)) {
ned_of_lla_point_i(&state.ned_pos_i, &state.ned_origin_i, &state.lla_pos_i);
} else { /* could not get this representation, set errno */
errno = 1;
}
} else if (state.utm_initialized_f) {
if (bit_is_set(state.pos_status, POS_NED_F)) {
NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f);
} else if (bit_is_set(state.pos_status, POS_ENU_I)) {
INT32_VECT3_NED_OF_ENU(state.ned_pos_i, state.enu_pos_i);
} else if (bit_is_set(state.pos_status, POS_ENU_F)) {
ENU_BFP_OF_REAL(state.enu_pos_i, state.enu_pos_f);
SetBit(state.pos_status, POS_ENU_I);
INT32_VECT3_NED_OF_ENU(state.ned_pos_i, state.enu_pos_i);
} else if (bit_is_set(state.pos_status, POS_UTM_F)) {
/* transform utm_f -> ned_f -> ned_i, set status bits */
NED_OF_UTM_DIFF(state.ned_pos_f, state.utm_pos_f, state.utm_origin_f);
SetBit(state.pos_status, POS_NED_F);
NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f);
} else if (bit_is_set(state.pos_status, POS_LLA_F)) {
/* transform lla_f -> utm_f -> ned_f -> ned_i, set status bits */
utm_of_lla_f(&state.utm_pos_f, &state.lla_pos_f);
SetBit(state.pos_status, POS_UTM_F);
NED_OF_UTM_DIFF(state.ned_pos_f, state.utm_pos_f, state.utm_origin_f);
SetBit(state.pos_status, POS_NED_F);
NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f);
} else if (bit_is_set(state.pos_status, POS_LLA_I)) {
/* transform lla_i -> lla_f -> utm_f -> ned_f -> ned_i, set status bits */
LLA_FLOAT_OF_BFP(state.lla_pos_f, state.lla_pos_i);
SetBit(state.pos_status, POS_LLA_F);
utm_of_lla_f(&state.utm_pos_f, &state.lla_pos_f);
SetBit(state.pos_status, POS_UTM_F);
NED_OF_UTM_DIFF(state.ned_pos_f, state.utm_pos_f, state.utm_origin_f);
SetBit(state.pos_status, POS_NED_F);
NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f);
} else { /* could not get this representation, set errno */
errno = 2;
}
} else { /* ned coordinate system not initialized, set errno */
errno = 3;
}
if (errno) {
//struct NedCoor_i _ned_zero = {0};
//return _ned_zero;
}
/* set bit to indicate this representation is computed */
SetBit(state.pos_status, POS_NED_I);
}
//
// main function
//
int main(void) {
char c;
uint16_t loop = 0;
uint8_t bootloaderEnableFlag = FALSE;
// relocate interrupt vector table to bottom of flash
// in case the bootloader will not be started
myIVSELREG = _BV(IVCE);
myIVSELREG = 0;
// init USART
myUBRRH = (F_CPU/(BAUDRATE*16L)-1) >> 8; // calculate baudrate and set high byte
myUBRRL = (uint8_t)(F_CPU/(BAUDRATE*16L)-1); // and low byte
myUCSRB = _BV(myTXEN) | _BV(myRXEN) | _BV(myRXCIE); // enable transmitter and receiver and receiver interrupt
myUCSRC = myURSEL | _BV(myUCSZ1) | _BV(myUCSZ0); // 8 bit character size, 1 stop bit, no parity
// the bootloader may be activated either if
// the character 'i' (interactive mode) was received from USART
// or the flash is (still) empty
// poll USART receive complete flag 64k times to catch the 'i' reliably
do {
if(bit_is_set(myUCSRA, myRXC))
if(myUDR == 'i')
bootloaderEnableFlag = TRUE;
} while(--loop);
// test if flash is empty (i.e. flash content is 0xff)
if(pgm_read_byte_near(0x0000) == 0xFF) {
bootloaderEnableFlag = TRUE; // set enable flag
}
// check enable flag and start application if FALSE
if(bootloaderEnableFlag == FALSE) {
myUCSRB = 0; // clear USART register to reset default
startApplication(); // start application code
}
//
// now the bootloader code begins
//
// welcome message and prompt
uartPutChar('\r');
uartPutChar('>');
// loop until a valid character is received
do {
c = uartGetChar(); // read a character
if(c == 'f') { // 'f' selects flash programming
uartPutChar('f'); // echo the 'f'
programThisMemory = FLASH; // set flag
}
#ifdef EEPROM_CODE
if(c == 'e') { // 'e' selects eeprom programming
uartPutChar('e'); // echo the 'e'
programThisMemory = EEPROM; // set flag
}
#endif
if(c == 'g') { // 'g' starts the application
uartPutChar('g'); // echo the 'g'
startApplication(); // and jump to 0x0000
}
} while(!programThisMemory); // exit loop when a valid key was pressed
// move interrupt vector table to boot loader area
myIVSELREG = _BV(IVCE);
myIVSELREG = _BV(IVSEL);
uartPutChar('\r'); // set cursor to next line
receiveBufferFull = FALSE; // reset full flag
receiveBufferPointer = (char *)receiveBuffer; // reset buffer pointer to start of buffer
// enable interrupts
sei();
// endless loop
while(1) {
// if buffer is full, parse the buffer and write to flash
if(receiveBufferFull) {
cli(); // disable interrupts
// if parsing produced an error, restart bootloader
if(!parseSrecBuffer(receiveBuffer)) {
uartPutChar('\r'); // set cursor to next line
startBootloader(); // restart the bootloader
}
// was an end-of-file s-record found?
if(srecEndOfFile) {
uartPutChar('O'); // 'OK' indicates successful programming
uartPutChar('K');
loop_until_bit_is_set(myUCSRA, myUDRE); // wait until character is transmitted
startBootloader(); // restart the bootloader
}
receiveBufferFull = FALSE; // reset full flag
receiveBufferPointer = (char *)receiveBuffer; // reset buffer pointer to start of buffer
sei(); // enable interrupts
}
}
}
int main(void)
{
DDRB=0xFF;
DDRD=0xFF;
PORTB=0xFF;
PORTD=0xFF;
PORTA=0x00;
//DDRD=0xFF;
//PORTD=0x00;
ICR1=10000;
TCCR1A=(1<<COM1A1|1<<COM1B1|0<<COM1A0|0<<COM1B0|0<<FOC1A|0<<FOC1B|0<<WGM11|0<<WGM10); //check datasheet (can be written in hex/bin)
TCCR1B=(1<<WGM13|1<<CS11|0<<CS12|0<<CS10|0<<WGM12|0<<ICNC1|0<<ICES1); //check datasheet (can be written in hex/bin)
for(;;)
{
Drive_motor(1,1);
if((bit_is_set(PINA,0))&&(bit_is_set(PINA,1))&&(bit_is_clear(PINA,2))&&(bit_is_set(PINA,3))&&(bit_is_set(PINA,4)))
{
ocr_drive(10000,10000);
}
if((bit_is_set(PINA,0))&&(bit_is_set(PINA,1))&&(bit_is_clear(PINA,2))&&(bit_is_clear(PINA,3))&&(bit_is_set(PINA,4)))
{
ocr_drive(10000,7500);
}
if((bit_is_set(PINA,0))&&(bit_is_clear(PINA,1))&&(bit_is_clear(PINA,2))&&(bit_is_set(PINA,3))&&(bit_is_set(PINA,4)))
{
ocr_drive(7500,10000);
}
if((bit_is_set(PINA,0))&&(bit_is_set(PINA,1))&&(bit_is_clear(PINA,2))&&(bit_is_clear(PINA,3))&&(bit_is_clear(PINA,4)))
{
ocr_drive(10000,5000);
}
if((bit_is_clear(PINA,0))&&(bit_is_clear(PINA,1))&&(bit_is_clear(PINA,2))&&(bit_is_set(PINA,3))&&(bit_is_set(PINA,4)))
{
ocr_drive(5000,10000);
}
if((bit_is_set(PINA,0))&&(bit_is_set(PINA,1))&&(bit_is_set(PINA,2))&&(bit_is_clear(PINA,3))&&(bit_is_clear(PINA,4)))
{
ocr_drive(10000,2500);
}
if((bit_is_clear(PINA,0))&&(bit_is_clear(PINA,1))&&(bit_is_set(PINA,2))&&(bit_is_set(PINA,3))&&(bit_is_set(PINA,4)))
{
ocr_drive(2500,10000);
}
if((bit_is_set(PINA,0))&&(bit_is_set(PINA,1))&&(bit_is_set(PINA,2))&&(bit_is_set(PINA,3))&&(bit_is_clear(PINA,4)))
{
ocr_drive(10000,0);
}
if((bit_is_clear(PINA,0))&&(bit_is_set(PINA,1))&&(bit_is_set(PINA,2))&&(bit_is_set(PINA,3))&&(bit_is_set(PINA,4)))
{
ocr_drive(0,10000);
}
}
}
void stateCalcPositionEnu_f(void)
{
if (bit_is_set(state.pos_status, POS_ENU_F)) {
return;
}
int errno = 0;
if (state.ned_initialized_f) {
if (bit_is_set(state.pos_status, POS_NED_F)) {
VECT3_ENU_OF_NED(state.enu_pos_f, state.ned_pos_f);
} else if (bit_is_set(state.pos_status, POS_ENU_I)) {
ENU_FLOAT_OF_BFP(state.enu_pos_f, state.enu_pos_i);
} else if (bit_is_set(state.pos_status, POS_NED_I)) {
NED_FLOAT_OF_BFP(state.ned_pos_f, state.ned_pos_i);
SetBit(state.pos_status, POS_NED_F);
VECT3_ENU_OF_NED(state.enu_pos_f, state.ned_pos_f);
} else if (bit_is_set(state.pos_status, POS_ECEF_F)) {
enu_of_ecef_point_f(&state.enu_pos_f, &state.ned_origin_f, &state.ecef_pos_f);
} else if (bit_is_set(state.pos_status, POS_ECEF_I)) {
/* transform ecef_i -> enu_i -> enu_f, set status bits */
enu_of_ecef_pos_i(&state.enu_pos_i, &state.ned_origin_i, &state.ecef_pos_i);
SetBit(state.pos_status, POS_ENU_I);
ENU_FLOAT_OF_BFP(state.enu_pos_f, state.enu_pos_i);
} else if (bit_is_set(state.pos_status, POS_LLA_F)) {
enu_of_lla_point_f(&state.enu_pos_f, &state.ned_origin_f, &state.lla_pos_f);
} else if (bit_is_set(state.pos_status, POS_LLA_I)) {
/* transform lla_i -> ecef_i -> enu_i -> enu_f, set status bits */
ecef_of_lla_i(&state.ecef_pos_i, &state.lla_pos_i); /* converts to doubles internally */
SetBit(state.pos_status, POS_ECEF_I);
enu_of_ecef_pos_i(&state.enu_pos_i, &state.ned_origin_i, &state.ecef_pos_i);
SetBit(state.pos_status, POS_ENU_I);
ENU_FLOAT_OF_BFP(state.enu_pos_f, state.enu_pos_i);
} else { /* could not get this representation, set errno */
errno = 1;
}
} else if (state.utm_initialized_f) {
if (bit_is_set(state.pos_status, POS_ENU_I)) {
ENU_FLOAT_OF_BFP(state.enu_pos_f, state.enu_pos_i);
} else if (bit_is_set(state.pos_status, POS_NED_F)) {
VECT3_ENU_OF_NED(state.enu_pos_f, state.ned_pos_f);
} else if (bit_is_set(state.pos_status, POS_NED_I)) {
NED_FLOAT_OF_BFP(state.ned_pos_f, state.ned_pos_i);
SetBit(state.pos_status, POS_NED_F);
VECT3_ENU_OF_NED(state.enu_pos_f, state.ned_pos_f);
} else if (bit_is_set(state.pos_status, POS_UTM_F)) {
ENU_OF_UTM_DIFF(state.enu_pos_f, state.utm_pos_f, state.utm_origin_f);
} else if (bit_is_set(state.pos_status, POS_LLA_F)) {
/* transform lla_f -> utm_f -> enu, set status bits */
utm_of_lla_f(&state.utm_pos_f, &state.lla_pos_f);
SetBit(state.pos_status, POS_UTM_F);
ENU_OF_UTM_DIFF(state.enu_pos_f, state.utm_pos_f, state.utm_origin_f);
} else if (bit_is_set(state.pos_status, POS_LLA_I)) {
/* transform lla_i -> lla_f -> utm_f -> enu, set status bits */
LLA_FLOAT_OF_BFP(state.lla_pos_f, state.lla_pos_i);
SetBit(state.pos_status, POS_LLA_F);
utm_of_lla_f(&state.utm_pos_f, &state.lla_pos_f);
SetBit(state.pos_status, POS_UTM_F);
ENU_OF_UTM_DIFF(state.enu_pos_f, state.utm_pos_f, state.utm_origin_f);
} else { /* could not get this representation, set errno */
errno = 2;
}
} else { /* ned coordinate system not initialized, set errno */
errno = 3;
}
if (errno) {
//struct EnuCoor_f _enu_zero = {0.0f};
//return _enu_zero;
}
/* set bit to indicate this representation is computed */
SetBit(state.pos_status, POS_ENU_F);
}
/*!
* \brief EEPROM emulator.
*
* Forces the chip to re-read the EEPROM contents and emulates a serial
* EEPROM.
*
* If the hardware does not support this feature, then this call will
* never return. Thus, make sure to have the driver properly configured.
*/
static void EmulateNicEeprom(void)
{
#ifdef __AVR_ENHANCED__
register uint8_t clk;
register uint8_t cnt;
register uint8_t val;
/*
* Prepare the EEPROM emulation port bits. Configure the EEDO and
* the EEMU lines as outputs and set EEDO to low and EEMU to high.
*/
outb(PORTC, 0xC0);
outb(DDRC, 0xC0);
/*
* Start EEPROM configuration. Stop/abort any activity and select
* configuration page 3. Setting bit EEM0 will force the controller
* to read the EEPROM contents.
*/
/* Select page 3, stop and abort/complete. */
nic_outlb(NIC_CR, NIC_CR_STP | NIC_CR_RD2 | NIC_CR_PS0 | NIC_CR_PS1);
nic_outlb(NIC_PG3_EECR, NIC_EECR_EEM0);
/*
* We can avoid wasting port pins for EEPROM emulation by using the
* upper bits of the address bus.
*/
/*
* No external memory access beyond this point.
*/
#if defined(__AVR_AT90CAN128__) || defined(__AVR_ATmega2561__)
cbi(XMCRA, SRE);
#else
cbi(MCUCR, SRE);
#endif
/*
* Loop for all EEPROM words.
*/
for(cnt = 0; cnt < sizeof(nic_eeprom); ) {
/*
*
* 1 start bit, always high
* 2 op-code bits
* 7 address bits
* 1 dir change bit, always low
*/
for(clk = 0; clk < 11; clk++) {
while(bit_is_clear(RTL_EESK_PIN, RTL_EESK_BIT));
while(bit_is_set(RTL_EESK_PIN, RTL_EESK_BIT));
}
/*
* Shift out the high byte, MSB first. Our data changes at the EESK
* rising edge. Data is sampled by the Realtek at the falling edge.
*/
val = PRG_RDB(nic_eeprom + cnt);
cnt++;
for(clk = 0x80; clk; clk >>= 1) {
while(bit_is_clear(RTL_EESK_PIN, RTL_EESK_BIT));
if(val & clk)
sbi(RTL_EEDO_PORT, RTL_EEDO_BIT);
while(bit_is_set(RTL_EESK_PIN, RTL_EESK_BIT));
cbi(RTL_EEDO_PORT, RTL_EEDO_BIT);
}
/*
* Shift out the low byte.
*/
val = PRG_RDB(nic_eeprom + cnt);
cnt++;
for(clk = 0x80; clk; clk >>= 1) {
while(bit_is_clear(RTL_EESK_PIN, RTL_EESK_BIT));
if(val & clk)
sbi(RTL_EEDO_PORT, RTL_EEDO_BIT);
while(bit_is_set(RTL_EESK_PIN, RTL_EESK_BIT));
cbi(RTL_EEDO_PORT, RTL_EEDO_BIT);
}
/* 5 remaining clock cycles. */
for(clk = 0; clk < 5; clk++) {
while(bit_is_clear(RTL_EESK_PIN, RTL_EESK_BIT));
while(bit_is_set(RTL_EESK_PIN, RTL_EESK_BIT));
}
}
/*
* Enable memory interface.
*/
#if defined(__AVR_AT90CAN128__) || defined(__AVR_ATmega2561__)
sbi(XMCRA, SRE);
#else
sbi(MCUCR, SRE);
#endif
/* Reset port outputs to default. */
outb(PORTC, 0x00);
outb(DDRC, 0x00);
#endif
}
static int DetectNicEeprom(void)
{
#ifdef __AVR_ENHANCED__
register unsigned int cnt = 0;
cli();
/*
* Prepare the EEPROM emulation port bits. Configure the EEDO
* and the EEMU lines as outputs and set both lines to high.
*/
outb(PORTC, 0xC0);
outb(DDRC, 0xC0);
/*
* Force the chip to re-read the EEPROM contents.
*/
nic_outlb(NIC_CR, NIC_CR_STP | NIC_CR_RD2 | NIC_CR_PS0 | NIC_CR_PS1);
nic_outlb(NIC_PG3_EECR, NIC_EECR_EEM0);
/*
* No external memory access beyond this point.
*/
#if defined(__AVR_AT90CAN128__) || defined(__AVR_ATmega2561__)
cbi(XMCRA, SRE);
#else
cbi(MCUCR, SRE);
#endif
/*
* Check, if the chip toggles our EESK input. If not, we do not
* have EEPROM emulation hardware.
*/
if(bit_is_set(PINC, 5)) {
while(++cnt && bit_is_set(PINC, 5));
}
else {
while(++cnt && bit_is_clear(PINC, 5));
}
/*
* Enable memory interface.
*/
#if defined(__AVR_AT90CAN128__) || defined(__AVR_ATmega2561__)
sbi(XMCRA, SRE);
#else
sbi(MCUCR, SRE);
#endif
/* Reset port outputs to default. */
outb(PORTC, 0x00);
outb(DDRC, 0x00);
/* Wait until controller ready. */
while(nic_inlb(NIC_CR) != (NIC_CR_STP | NIC_CR_RD2));
sei();
return cnt ? 0 : -1;
#else
return -1;
#endif
}
void maca_isr(void) {
// print_packets("maca_isr");
maca_entry++;
if (bit_is_set(*MACA_STATUS, maca_status_ovr))
{ PRINTF("maca overrun\n\r"); }
if (bit_is_set(*MACA_STATUS, maca_status_busy))
{ PRINTF("maca busy\n\r"); }
if (bit_is_set(*MACA_STATUS, maca_status_crc))
{ PRINTF("maca crc error\n\r"); }
if (bit_is_set(*MACA_STATUS, maca_status_to))
{ PRINTF("maca timeout\n\r"); }
if (data_indication_irq()) {
*MACA_CLRIRQ = (1 << maca_irq_di);
dma_rx->length = *MACA_GETRXLVL - 2; /* packet length does not include FCS */
dma_rx->lqi = get_lqi();
dma_rx->rx_time = *MACA_TIMESTAMP;
/* check if received packet needs an ack */
if(dma_rx->data[1] & 0x20) {
/* this wait is necessary to auto-ack */
volatile uint32_t wait_clk;
wait_clk = *MACA_CLK + 200;
while(*MACA_CLK < wait_clk) { continue; }
}
if(maca_rx_callback != 0) { maca_rx_callback(dma_rx); }
add_to_rx(dma_rx);
dma_rx = 0;
}
if (filter_failed_irq()) {
PRINTF("maca filter failed\n\r");
ResumeMACASync();
*MACA_CLRIRQ = (1 << maca_irq_flt);
}
if (checksum_failed_irq()) {
PRINTF("maca checksum failed\n\r");
ResumeMACASync();
*MACA_CLRIRQ = (1 << maca_irq_crc);
}
if (softclock_irq()) {
*MACA_CLRIRQ = (1 << maca_irq_sftclk);
}
if (poll_irq()) {
*MACA_CLRIRQ = (1 << maca_irq_poll);
}
if(action_complete_irq()) {
/* PRINTF("maca action complete %d\n\r", get_field(*MACA_CONTROL,SEQUENCE)); */
if(last_post == TX_POST) {
tx_head->status = get_field(*MACA_STATUS,CODE);
if(maca_tx_callback != 0) { maca_tx_callback(tx_head); }
dma_tx = 0;
free_tx_head();
last_post = NO_POST;
}
ResumeMACASync();
*MACA_CLRIRQ = (1 << maca_irq_acpl);
}
decode_status();
if (*MACA_IRQ != 0)
{ PRINTF("*MACA_IRQ %x\n\r", (unsigned int)*MACA_IRQ); }
if(tx_head != 0) {
post_tx();
} else {
post_receive();
}
}
int
main (void)
{
#ifdef BOOTLOADER_SUPPORT
_IVREG = _BV (IVCE); /* prepare ivec change */
_IVREG = _BV (IVSEL); /* change ivec to bootloader */
#endif
/* Default DDR Config */
#if IO_HARD_PORTS >= 4 && DDR_MASK_A != 0
DDRA = DDR_MASK_A;
#endif
#if DDR_MASK_B != 0
DDRB = DDR_MASK_B;
#endif
#if DDR_MASK_C != 0
DDRC = DDR_MASK_C;
#endif
#if DDR_MASK_D != 0
DDRD = DDR_MASK_D;
#endif
#if IO_HARD_PORTS >= 6
#if DDR_MASK_E != 0
DDRE = DDR_MASK_E;
#endif
#if DDR_MASK_F != 0
DDRF = DDR_MASK_F;
#endif
#endif
#if IO_HARD_PORTS >= 7
#if DDR_MASK_G != 0
DDRG = DDR_MASK_G;
#endif
#endif
#ifdef STATUSLED_POWER_SUPPORT
PIN_SET(STATUSLED_POWER);
#endif
//FIXME: zum ethersex meta system hinzufügen, aber vor allem anderem initalisieren
debug_init();
debug_printf("%S (Debug mode)\n", pstr_E6_VERSION_STRING_LONG);
#ifdef DEBUG_RESET_REASON
if (bit_is_set (mcusr_mirror, BORF))
debug_printf("reset: Brown-out\n");
else if (bit_is_set (mcusr_mirror, PORF))
debug_printf("reset: Power on\n");
else if (bit_is_set (mcusr_mirror, WDRF))
debug_printf("reset: Watchdog\n");
else if (bit_is_set (mcusr_mirror, EXTRF))
debug_printf("reset: Extern\n");
else
debug_printf("reset: Unknown\n");
#endif
#ifdef BOOTLOADER_SUPPORT
/* disable interrupts */
cli ();
wdt_disable();
#endif //BOOTLOADER_SUPPORT
/* enable interrupts */
sei ();
#ifdef USE_WATCHDOG
debug_printf("enabling watchdog\n");
#ifdef DEBUG
/* for debugging, test reset cause and jump to bootloader */
if (MCU_STATUS_REGISTER & _BV (WDRF))
{
debug_printf("bootloader...\n");
jump_to_bootloader();
}
#endif
/* set watchdog to 2 seconds */
wdt_enable(WDTO_2S);
wdt_kick();
#else //USE_WATCHDOG
debug_printf("disabling watchdog\n");
wdt_disable();
#endif //USE_WATCHDOG
#ifdef SPI_SUPPORT
spi_init();
#endif
ethersex_meta_init();
/* must be called AFTER all other initialization */
#ifdef PORTIO_SUPPORT
portio_init();
#elif defined(NAMED_PIN_SUPPORT)
np_simple_init();
#endif
#ifdef ENC28J60_SUPPORT
debug_printf ("enc28j60 revision 0x%x\n",
read_control_register (REG_EREVID));
debug_printf ("mac: %02x:%02x:%02x:%02x:%02x:%02x\n",
uip_ethaddr.addr[0], uip_ethaddr.addr[1], uip_ethaddr.addr[2],
uip_ethaddr.addr[3], uip_ethaddr.addr[4], uip_ethaddr.addr[5]);
#endif
#ifdef STATUSLED_BOOTED_SUPPORT
PIN_SET(STATUSLED_BOOTED);
#endif
ethersex_meta_startup();
/* main loop */
while (1)
{
wdt_kick();
ethersex_meta_mainloop();
#ifdef SD_READER_SUPPORT
if (sd_active_partition == NULL)
{
if (!sd_try_init())
{
#ifdef VFS_SD_SUPPORT
vfs_sd_try_open_rootnode();
#endif
}
wdt_kick();
}
#endif
#ifdef BOOTLOADER_JUMP
if (status.request_bootloader)
{
#ifdef MBR_SUPPORT
mbr_config.bootloader = 1;
write_mbr();
#endif
#ifdef CLOCK_CRYSTAL_SUPPORT
TIMER_8_AS_1_INT_OVERFLOW_OFF;
#endif
#ifdef DCF77_SUPPORT
ACSR &= ~_BV (ACIE);
#endif
cli();
jump_to_bootloader();
}
#endif
#ifndef TEENSY_SUPPORT
if (status.request_wdreset)
{
cli();
wdt_enable(WDTO_15MS);
for (;;);
}
#endif
if (status.request_reset)
{
cli();
void (*reset) (void) = NULL;
reset();
}
}
}
void manual_mode()
{
int discardSensors = 1;
while(1) {
_delay_ms(1);
//printf("\r\nPlease enter the input in the format : <dir> <l1><l0> <r1><r0>\r\n");
//char dir = getchar();
if( bit_is_set(UCSR0A, RXC0) != 0) {
char dir = UDR0;
printf("UDR0 = %c\r\n",dir);
if(dir == 'a') {
printf("Auto mode enabled\r\n");
auto_mode();
}
while( (dir !='d') & (dir !='h') & (dir !='f') & (dir !='b') & (dir !='l') & (dir !='r') & (dir !='s') ) {
printf("Enter valid direction character(f,b,l,r,s,d,h)");
char dir = getchar();
}
if(dir=='d') {
discardSensors = 1; // Discard Sensor Values
}
if(dir=='h') {
discardSensors = 0; // Hold Sensor Values
}
if(dir=='f') {
int speedl1 = getchar();
int speedl0 = getchar();
int speedr1 = getchar();
int speedr0 = getchar();
printf("You entered input as : %c %c%c %c%c\r\n", dir, speedl1,speedl0,speedr1,speedr0);
int speedl = ((speedl1-48)*10 + (speedl0-48))*255/99;
int speedr = ((speedr1-48)*10 + (speedr0-48))*255/99;
moveForward(speedl,speedr);
printf("Robot moves forward\r\n");
}
if(dir=='b') {
int speedl1 = getchar();
int speedl0 = getchar();
int speedr1 = getchar();
int speedr0 = getchar();
printf("You entered input as : %c %c%c %c%c\r\n", dir, speedl1,speedl0,speedr1,speedr0);
int speedl = ((speedl1-48)*10 + (speedl0-48))*255/99;
int speedr = ((speedr1-48)*10 + (speedr0-48))*255/99;
moveBackward(speedl,speedr);
printf("Robot moves backward\r\n");
}
if(dir=='r') {
int speedl1 = getchar();
int speedl0 = getchar();
int speedr1 = getchar();
int speedr0 = getchar();
printf("You entered input as : %c %c%c %c%c\r\n", dir, speedl1,speedl0,speedr1,speedr0);
int speedl = ((speedl1-48)*10 + (speedl0-48))*255/99;
int speedr = ((speedr1-48)*10 + (speedr0-48))*255/99;
moveRight(speedl,speedr);
printf("Robot moves right\r\n");
}
if(dir=='l') {
int speedl1 = getchar();
int speedl0 = getchar();
int speedr1 = getchar();
int speedr0 = getchar();
printf("You entered input as : %c %c%c %c%c\r\n", dir, speedl1,speedl0,speedr1,speedr0);
int speedl = ((speedl1-48)*10 + (speedl0-48))*255/99;
int speedr = ((speedr1-48)*10 + (speedr0-48))*255/99;
moveLeft(speedl,speedr);
printf("Robot moves left\r\n");
}
if(dir=='s') {
printf("You entered input as : %c\r\n", dir);
moveStop();
printf("Robot stops\r\n");
}
}
else if( discardSensors!=1 ) {
_delay_ms(50);
int adc_val0=ReadADC(0);
_delay_ms(50);
int adc_val1=ReadADC(1);
_delay_ms(50);
int adc_val2=ReadADC(2);
printf("Sensor Readings : ClifL = %d, ClifR = %d, Bump = %d\r\n",adc_val0,adc_val1,adc_val2);
if( (adc_val0 < 100) ) {
//if( (adc_val0 < 100) | (adc_val1 < 100) | (adc_val2 < 100) ) {
moveStop();
printf("Robot stops\r\n");
_delay_ms(0500);
moveBackward(160,160);
printf("Robot moves backward\r\n");
_delay_ms(1000);
moveStop();
printf("Robot stops\r\n");
_delay_ms(0500);
moveRight(160,160);
printf("Robot moves Right\r\n");
_delay_ms(0500);
moveStop();
printf("Robot stops\r\n");
_delay_ms(0500);
}
}
}
}
int main(void)
{
unsigned char i;
unsigned char volatile licznikkl=0; //zmienna wykorzystywana do pomiaru czasu
//naciśnięcia przycisków
char volatile przyrost=1; //przyrost zmiany współczynnika wypełnienia sygnału PWM
//tablica komunikatów do wysłania
char *info[5]={
PSTR("\n\rRegulator obrotów silnika DC\n\r"),
PSTR("0 - zatrzymanie silnika\n\r"),
PSTR("1 - start z max. obrotami\n\r"),
PSTR("N - podaj aktualne parametry sterownika\n\r\n"),
PSTR("\n\rAktualne parametry PWM:")
};
union //unia pozwala na bajtowy dostęp do zmiennej int
{
unsigned int pwm;
unsigned char pwmc[2];
}volatile upwm; //aktualny współczynnik wypełnienia sygn. PWM
DDRB=0xff; //PORTB - wy
PORTB=0xff;
DDRD=0x02; //PD1 - wy (RXD), pozostałe we
PORTD=0x02; //podciągania wejścia PD1 (RXD)
UBRR=VUBRR; //ustaw prędkość transmisji
UCR=1<<RXCIE | 1<<TXCIE | 1<<RXEN; //zezwolenie na przerwania od
//odbiornika i nadajnika, zezwolenie na odbiór i nadawanie
TCCR1A=0x83; //PWM 10 bitowy
//zerowanie OC1 po spełnieniu warunku równości podczas liczenia
//w górę, ustawiane podczas liczenia w dół
TCCR1B=0x01; //preskaler=3, co przy 10-bit PWM daje Fwy=ok. 61Hz @8MHz
TCNT1L=0x00; //wstępne ustawienie licznika 1
TCNT1H=0x00;
upwm.pwm=0x3ff; //początkowo silnik włączony, wartość TOP odpowiada wysokiemu
//poziomowi na wyjściu OC1 (PB3)
OCR1H=upwm.pwmc[1]; //wpisz aktualnie ustawiony współczynnik do rejestrów
OCR1L=upwm.pwmc[0]; //OCR1 timera1
sei(); //włącz przerwania
for(i=0;i<5;i++) //wyślij winietkę
{
wyslijtekstROM(info[i]); //wysłanie pojedynczej linii tekstu
while(bit_is_set(UCR,TXEN)); //trzeba zaczekać, aż zostanie wysłana do końca
}
while(1) //główna pętla programu
{
if(fodbznak) //czy odebrano jakiś znak?
{
fodbznak=0; //tak
switch (komenda) //interpretacja komendy i wykonanie odpowiedniej akcji
{
case '.': //odebrano "." - zwiększ prędkość
upwm.pwm+=przyrost; //zwiększ PWM
if(upwm.pwm>0x3ff)
{
upwm.pwm=0x3ff; //jeśli przekroczono wartość TOP, to ustaw TOP
}
czekaj(150*tau); //eliminacja powtórnej interpretacji
//naciśnięcia przycisku
licznikkl++; //mierz długość naciśnięcia przycisku
break;
case ',': //odebrano "," - zmniejsz prędkość
upwm.pwm-=przyrost; //zmniejsz PWM
if(upwm.pwm>0x3ff)
{
upwm.pwm=0; //jeśli przekroczono wartość zero to ustaw zero
}
czekaj(150*tau); //eliminacja powtórnej interpretacji
//naciśnięcia przycisku
licznikkl++; //mierz długość naciśnięcia przycisku
break;
case '0': //odebrano "0" - zatrzymaj silnik
upwm.pwm=0; //silnik STOP
break;
case '1': //odebrano "1" - ustaw max obroty silnika
upwm.pwm=0x3ff; //silnik na MAX
break;
case 'n':
case 'N':
wyslijtekstROM(info[4]); //wysłanie pojedynczej linii tekstu
while(bit_is_set(UCR,TXEN)); //trzeba zaczekać,
//aż zostanie wysłana do końca
wyslijtekst("0x"); //wyślij prefiks dla liczb heksadecymalnych
while(bit_is_set(UCR,TXEN)); //trzeba zaczekać,
//aż zostanie wysłana do końca
itoa(upwm.pwm,fifosio,16); //konwersja liczby int (hex)
//na łańcuch znakowy
wyslijtekst(fifosio); //wyślij aktualną wartość PWM do PC-ta
while(bit_is_set(UCR,TXEN)); //trzeba zaczekać,
//aż zostanie wysłana do końca
break;
}
if(licznikkl>6)
{
przyrost=+16; //wykryto długie naciśnięcie klawisza,
//zwiększ krok regulacji
licznikkl=6; //dalej już nie zwiększaj kroku
cbi(PORTB,1); //zapal diodę LED2
}
OCR1H=upwm.pwmc[1]; //wpisz aktualnie ustawiony współczynnik do rejestrów
OCR1L=upwm.pwmc[0]; //OCR1 timera1
}
else
{ //jeśli cisza na linii, ustaw parametry spoczynkowe
licznikkl=0;
przyrost=1;
sbi(PORTB,1); //zgaś diodę LED2
}
}
}
// return TRUE if conversion is complete
u08 a2dIsComplete(void)
{
return bit_is_set(ADCSR, ADSC);
}
void blink1(void) {
if (bit_is_set(PORTA,PA1)) cbi(PORTA,PA1); else sbi(PORTA,PA1);
}
void maca_isr(void) {
// print_packets("maca_isr");
maca_entry++;
if (bit_is_set(*MACA_STATUS, maca_status_ovr))
{
PRINTF("maca overrun\n\r");
}
if (bit_is_set(*MACA_STATUS, maca_status_busy))
{
PRINTF("maca busy\n\r");
}
if (bit_is_set(*MACA_STATUS, maca_status_crc))
{
PRINTF("maca crc error\n\r");
}
if (bit_is_set(*MACA_STATUS, maca_status_to))
{
PRINTF("maca timeout\n\r");
}
if (data_indication_irq()) {
*MACA_CLRIRQ = (1 << maca_irq_di);
dma_rx->length = *MACA_GETRXLVL - 2; /* packet length does not include FCS */
dma_rx->lqi = get_lqi();
// PRINTF("maca data ind %x %d\n\r", dma_rx, dma_rx->length);
if(maca_rx_callback != 0) {
maca_rx_callback(dma_rx);
}
add_to_rx(dma_rx);
dma_rx = 0;
}
if (filter_failed_irq()) {
PRINTF("maca filter failed\n\r");
ResumeMACASync();
*MACA_CLRIRQ = (1 << maca_irq_flt);
}
if (checksum_failed_irq()) {
PRINTF("maca checksum failed\n\r");
ResumeMACASync();
*MACA_CLRIRQ = (1 << maca_irq_crc);
}
if (softclock_irq()) {
*MACA_CLRIRQ = (1 << maca_irq_sftclk);
}
if (poll_irq()) {
*MACA_CLRIRQ = (1 << maca_irq_poll);
}
if(action_complete_irq()) {
/* PRINTF("maca action complete %d\n\r", get_field(*MACA_CONTROL,SEQUENCE)); */
if(last_post == TX_POST) {
if(maca_tx_callback != 0) {
maca_tx_callback(tx_head);
}
dma_tx = 0;
free_tx_head();
last_post = NO_POST;
}
ResumeMACASync();
*MACA_CLRIRQ = (1 << maca_irq_acpl);
}
decode_status();
if (*MACA_IRQ != 0)
{
PRINTF("*MACA_IRQ %x\n\r", *MACA_IRQ);
}
if(tx_head != 0) {
post_tx();
} else {
post_receive();
}
}
void check_rx() {
if (bit_is_set(UCSR0A, RXC0)){
buffer_size = uart_read_string(buffer, 255);
}
}
/*
* Save the new resource by chaining it into the head list for
* the resource. If this is pass 2, we update any resource
* pointers because they may not have been defined until
* later in pass 1.
*/
bool save_resource(int type, RES_ITEM *items, int pass)
{
URES *res;
int rindex = type - R_FIRST;
int i;
int error = 0;
/*
* Ensure that all required items are present
*/
for (i = 0; items[i].name; i++) {
if (items[i].flags & CFG_ITEM_REQUIRED) {
if (!bit_is_set(i, res_all.res_monitor.hdr.item_present)) {
Emsg2(M_ERROR_TERM, 0, _("%s item is required in %s resource, but not found.\n"),
items[i].name, resources[rindex]);
}
}
/* If this triggers, take a look at lib/parse_conf.h */
if (i >= MAX_RES_ITEMS) {
Emsg1(M_ERROR_TERM, 0, _("Too many items in %s resource\n"), resources[rindex]);
}
}
/*
* During pass 2 in each "store" routine, we looked up pointers
* to all the resources referrenced in the current resource, now we
* must copy their addresses from the static record to the allocated
* record.
*/
if (pass == 2) {
switch (type) {
/*
* Resources not containing a resource
*/
case R_MONITOR:
case R_CLIENT:
case R_STORAGE:
case R_DIRECTOR:
case R_CONSOLE_FONT:
break;
default:
Emsg1(M_ERROR, 0, _("Unknown resource type %d in save_resource.\n"), type);
error = 1;
break;
}
/*
* Note, the resource name was already saved during pass 1,
* so here, we can just release it.
*/
if (res_all.res_monitor.hdr.name) {
free(res_all.res_monitor.hdr.name);
res_all.res_monitor.hdr.name = NULL;
}
if (res_all.res_monitor.hdr.desc) {
free(res_all.res_monitor.hdr.desc);
res_all.res_monitor.hdr.desc = NULL;
}
return (error == 0);
}
/*
* Common
*/
if (!error) {
res = (URES *)malloc(resources[rindex].size);
memcpy(res, &res_all, resources[rindex].size);
if (!res_head[rindex]) {
res_head[rindex] = (RES *)res; /* store first entry */
Dmsg3(900, "Inserting first %s res: %s index=%d\n",
res_to_str(type), res->res_monitor.name(), rindex);
} else {
RES *next, *last;
/*
* Add new res to end of chain
*/
for (last = next = res_head[rindex]; next; next=next->next) {
last = next;
if (strcmp(next->name, res->res_monitor.name()) == 0) {
Emsg2(M_ERROR_TERM, 0,
_("Attempt to define second %s resource named \"%s\" is not permitted.\n"),
resources[rindex].name, res->res_monitor.name());
}
}
last->next = (RES *)res;
Dmsg4(900, "Inserting %s res: %s index=%d pass=%d\n",
res_to_str(type), res->res_monitor.name(), rindex, pass);
}
}
return (error == 0);
}
void uart_send_char(uint8_t data)
{
while(!bit_is_set(UCSR0A, UDRE0));
UDR0 = data;
}
/** \brief Function to be called when a message from FBW is available */
static inline void telecommand_task( void ) {
uint8_t mode_changed = FALSE;
copy_from_to_fbw();
uint8_t really_lost = bit_is_set(fbw_state->status, STATUS_RADIO_REALLY_LOST) && (pprz_mode == PPRZ_MODE_AUTO1 || pprz_mode == PPRZ_MODE_MANUAL);
if (pprz_mode != PPRZ_MODE_HOME && pprz_mode != PPRZ_MODE_GPS_OUT_OF_ORDER && launch) {
if (too_far_from_home) {
pprz_mode = PPRZ_MODE_HOME;
mode_changed = TRUE;
}
if (really_lost) {
pprz_mode = RC_LOST_MODE;
mode_changed = TRUE;
}
}
if (bit_is_set(fbw_state->status, AVERAGED_CHANNELS_SENT)) {
bool_t pprz_mode_changed = pprz_mode_update();
mode_changed |= pprz_mode_changed;
#if defined RADIO_CALIB && defined RADIO_CONTROL_SETTINGS
bool_t calib_mode_changed = RcSettingsModeUpdate(fbw_state->channels);
rc_settings(calib_mode_changed || pprz_mode_changed);
mode_changed |= calib_mode_changed;
#endif
}
mode_changed |= mcu1_status_update();
if ( mode_changed )
PERIODIC_SEND_PPRZ_MODE(DefaultChannel);
#if defined RADIO_CONTROL || defined RADIO_CONTROL_AUTO1
/** In AUTO1 mode, compute roll setpoint and pitch setpoint from
* \a RADIO_ROLL and \a RADIO_PITCH \n
*/
if (pprz_mode == PPRZ_MODE_AUTO1) {
/** Roll is bounded between [-AUTO1_MAX_ROLL;AUTO1_MAX_ROLL] */
h_ctl_roll_setpoint = FLOAT_OF_PPRZ(fbw_state->channels[RADIO_ROLL], 0., -AUTO1_MAX_ROLL);
/** Pitch is bounded between [-AUTO1_MAX_PITCH;AUTO1_MAX_PITCH] */
h_ctl_pitch_setpoint = FLOAT_OF_PPRZ(fbw_state->channels[RADIO_PITCH], 0., AUTO1_MAX_PITCH);
} /** Else asynchronously set by \a h_ctl_course_loop() */
/** In AUTO1, throttle comes from RADIO_THROTTLE
In MANUAL, the value is copied to get it in the telemetry */
if (pprz_mode == PPRZ_MODE_MANUAL || pprz_mode == PPRZ_MODE_AUTO1) {
v_ctl_throttle_setpoint = fbw_state->channels[RADIO_THROTTLE];
}
/** else asynchronously set by v_ctl_climb_loop(); */
mcu1_ppm_cpt = fbw_state->ppm_cpt;
#endif // RADIO_CONTROL
vsupply = fbw_state->vsupply;
current = fbw_state->current;
#ifdef RADIO_CONTROL
if (!estimator_flight_time) {
if (pprz_mode == PPRZ_MODE_AUTO2 && fbw_state->channels[RADIO_THROTTLE] > THROTTLE_THRESHOLD_TAKEOFF) {
launch = TRUE;
}
}
#endif
}
/* Save the new resource by chaining it into the head list for
* the resource. If this is pass 2, we update any resource
* pointers (currently only in the Job resource).
*/
void save_resource(int type, RES_ITEM *items, int pass)
{
URES *res;
int rindex = type - R_FIRST;
int i;
int error = 0;
/*
* Ensure that all required items are present
*/
for (i = 0; items[i].name; i++) {
if (items[i].flags & CFG_ITEM_REQUIRED) {
if (!bit_is_set(i, res_all.res_dir.hdr.item_present)) {
Emsg2(M_ABORT, 0, _("%s item is required in %s resource, but not found.\n"),
items[i].name, resources[rindex]);
}
}
}
/* During pass 2, we looked up pointers to all the resources
* referrenced in the current resource, , now we
* must copy their address from the static record to the allocated
* record.
*/
if (pass == 2) {
switch (type) {
/* Resources not containing a resource */
case R_CONSOLE:
case R_DIRECTOR:
break;
default:
Emsg1(M_ERROR, 0, _("Unknown resource type %d\n"), type);
error = 1;
break;
}
/* Note, the resoure name was already saved during pass 1,
* so here, we can just release it.
*/
if (res_all.res_dir.hdr.name) {
free(res_all.res_dir.hdr.name);
res_all.res_dir.hdr.name = NULL;
}
if (res_all.res_dir.hdr.desc) {
free(res_all.res_dir.hdr.desc);
res_all.res_dir.hdr.desc = NULL;
}
return;
}
/* Common */
if (!error) {
res = (URES *)malloc(resources[rindex].size);
memcpy(res, &res_all, resources[rindex].size);
if (!res_head[rindex]) {
res_head[rindex] = (RES *)res; /* store first entry */
} else {
RES *next, *last;
for (last=next=res_head[rindex]; next; next=next->next) {
last = next;
if (bstrcmp(next->name, res->res_dir.hdr.name)) {
Emsg2(M_ERROR_TERM, 0,
_("Attempt to define second %s resource named \"%s\" is not permitted.\n"),
resources[rindex].name, res->res_dir.hdr.name);
}
}
last->next = (RES *)res;
Dmsg2(90, "Inserting %s res: %s\n", res_to_str(type),
res->res_dir.hdr.name);
}
}
}
void parse_ins_msg(void)
{
uint8_t offset = 0;
if (xsens_id == XSENS_ReqLeverArmGpsAck_ID) {
xsens_gps_arm_x = XSENS_ReqLeverArmGpsAck_x(xsens_msg_buf);
xsens_gps_arm_y = XSENS_ReqLeverArmGpsAck_y(xsens_msg_buf);
xsens_gps_arm_z = XSENS_ReqLeverArmGpsAck_z(xsens_msg_buf);
DOWNLINK_SEND_IMU_MAG_SETTINGS(DefaultChannel, DefaultDevice, &xsens_declination, &xsens_declination, &xsens_gps_arm_x,
&xsens_gps_arm_y, &xsens_gps_arm_z);
} else if (xsens_id == XSENS_Error_ID) {
xsens_errorcode = XSENS_Error_errorcode(xsens_msg_buf);
} else if (xsens_id == XSENS_MTData2_ID) {
for (offset = 0; offset < xsens_len;) {
uint8_t code1 = xsens_msg_buf[offset];
uint8_t code2 = xsens_msg_buf[offset + 1];
int subpacklen = (int)xsens_msg_buf[offset + 2];
offset += 3;
if (code1 == 0x10) {
if (code2 == 0x10) {
// UTC
} else if (code2 == 0x20) {
// Packet Counter
}
if (code2 == 0x30) {
// ITOW
}
} else if (code1 == 0x20) {
if (code2 == 0x30) {
// Attitude Euler
ins_phi = XSENS_DATA_Euler_roll(xsens_msg_buf, offset) * M_PI / 180;
ins_theta = XSENS_DATA_Euler_pitch(xsens_msg_buf, offset) * M_PI / 180;
ins_psi = XSENS_DATA_Euler_yaw(xsens_msg_buf, offset) * M_PI / 180;
new_ins_attitude = 1;
}
} else if (code1 == 0x40) {
if (code2 == 0x10) {
// Delta-V
ins_ax = XSENS_DATA_Acceleration_x(xsens_msg_buf, offset) * 100.0f;
ins_ay = XSENS_DATA_Acceleration_y(xsens_msg_buf, offset) * 100.0f;
ins_az = XSENS_DATA_Acceleration_z(xsens_msg_buf, offset) * 100.0f;
}
} else if (code1 == 0x80) {
if (code2 == 0x20) {
// Rate Of Turn
ins_p = XSENS_DATA_RateOfTurn_x(xsens_msg_buf, offset) * M_PI / 180;
ins_q = XSENS_DATA_RateOfTurn_y(xsens_msg_buf, offset) * M_PI / 180;
ins_r = XSENS_DATA_RateOfTurn_z(xsens_msg_buf, offset) * M_PI / 180;
}
} else if (code1 == 0x30) {
if (code2 == 0x10) {
// Baro Pressure
}
} else if (code1 == 0xE0) {
if (code2 == 0x20) {
// Status Word
xsens_msg_statusword = XSENS_DATA_StatusWord_status(xsens_msg_buf, offset);
//gps.tow = xsens_msg_statusword;
#if USE_GPS_XSENS
if (bit_is_set(xsens_msg_statusword, 2) && bit_is_set(xsens_msg_statusword, 1)) {
if (bit_is_set(xsens_msg_statusword, 23) && bit_is_set(xsens_msg_statusword, 24)) {
gps.fix = GPS_FIX_3D;
} else {
gps.fix = GPS_FIX_2D;
}
} else { gps.fix = GPS_FIX_NONE; }
#endif
}
} else if (code1 == 0x88) {
if (code2 == 0x40) {
gps.week = XSENS_DATA_GpsSol_week(xsens_msg_buf, offset);
gps.num_sv = XSENS_DATA_GpsSol_numSv(xsens_msg_buf, offset);
gps.pacc = XSENS_DATA_GpsSol_pAcc(xsens_msg_buf, offset);
gps.sacc = XSENS_DATA_GpsSol_sAcc(xsens_msg_buf, offset);
gps.pdop = XSENS_DATA_GpsSol_pDop(xsens_msg_buf, offset);
} else if (code2 == 0xA0) {
// SVINFO
gps.tow = XSENS_XDI_GpsSvInfo_iTOW(xsens_msg_buf + offset);
#if USE_GPS_XSENS
gps.nb_channels = XSENS_XDI_GpsSvInfo_nch(xsens_msg_buf + offset);
gps.last_3dfix_ticks = sys_time.nb_sec_rem;
gps.last_3dfix_time = sys_time.nb_sec;
uint8_t i;
// Do not write outside buffer
for (i = 0; i < Min(gps.nb_channels, GPS_NB_CHANNELS); i++) {
uint8_t ch = XSENS_XDI_GpsSvInfo_chn(xsens_msg_buf + offset, i);
if (ch > gps.nb_channels) { continue; }
gps.svinfos[ch].svid = XSENS_XDI_GpsSvInfo_svid(xsens_msg_buf + offset, i);
gps.svinfos[ch].flags = XSENS_XDI_GpsSvInfo_bitmask(xsens_msg_buf + offset, i);
gps.svinfos[ch].qi = XSENS_XDI_GpsSvInfo_qi(xsens_msg_buf + offset, i);
gps.svinfos[ch].cno = XSENS_XDI_GpsSvInfo_cnr(xsens_msg_buf + offset, i);
}
#endif
}
} else if (code1 == 0x50) {
if (code2 == 0x10) {
//gps.hmsl = XSENS_DATA_Altitude_h(xsens_msg_buf,offset)* 1000.0f;
} else if (code2 == 0x20) {
// Altitude Elipsoid
gps.utm_pos.alt = XSENS_DATA_Altitude_h(xsens_msg_buf, offset) * 1000.0f;
// Compute geoid (MSL) height
float geoid_h = wgs84_ellipsoid_to_geoid(lla_f.lat, lla_f.lon);
gps.hmsl = gps.utm_pos.alt - (geoid_h * 1000.0f);
SetBit(gps.valid_fields, GPS_VALID_HMSL_BIT);
//gps.tow = geoid_h * 1000.0f; //gps.utm_pos.alt;
} else if (code2 == 0x40) {
// LatLong
#ifdef GPS_LED
LED_TOGGLE(GPS_LED);
#endif
gps.last_3dfix_ticks = sys_time.nb_sec_rem;
gps.last_3dfix_time = sys_time.nb_sec;
gps.week = 0; // FIXME
lla_f.lat = RadOfDeg(XSENS_DATA_LatLon_lat(xsens_msg_buf, offset));
lla_f.lon = RadOfDeg(XSENS_DATA_LatLon_lon(xsens_msg_buf, offset));
// Set the real UTM zone
gps.utm_pos.zone = (DegOfRad(lla_f.lon) + 180) / 6 + 1;
utm_f.zone = nav_utm_zone0;
// convert to utm
utm_of_lla_f(&utm_f, &lla_f);
// copy results of utm conversion
gps.utm_pos.east = utm_f.east * 100;
gps.utm_pos.north = utm_f.north * 100;
SetBit(gps.valid_fields, GPS_VALID_POS_UTM_BIT);
gps_xsens_publish();
}
} else if (code1 == 0xD0) {
if (code2 == 0x10) {
// Velocity
ins_vx = ((INS_FORMAT)XSENS_DATA_VelocityXYZ_x(xsens_msg_buf, offset));
ins_vy = ((INS_FORMAT)XSENS_DATA_VelocityXYZ_y(xsens_msg_buf, offset));
ins_vz = ((INS_FORMAT)XSENS_DATA_VelocityXYZ_z(xsens_msg_buf, offset));
gps.ned_vel.x = ins_vx;
gps.ned_vel.y = ins_vy;
gps.ned_vel.z = ins_vx;
SetBit(gps.valid_fields, GPS_VALID_VEL_NED_BIT);
}
}
if (subpacklen < 0) {
subpacklen = 0;
}
offset += subpacklen;
}
/*
gps.pacc = XSENS_DATA_RAWGPS_hacc(xsens_msg_buf,offset) / 100;
gps.sacc = XSENS_DATA_RAWGPS_sacc(xsens_msg_buf,offset) / 100;
gps.pdop = 5; // FIXME Not output by XSens
*/
/*
*/
/*
ins_mx = XSENS_DATA_Calibrated_magX(xsens_msg_buf,offset);
ins_my = XSENS_DATA_Calibrated_magY(xsens_msg_buf,offset);
ins_mz = XSENS_DATA_Calibrated_magZ(xsens_msg_buf,offset);
*/
/*
xsens_time_stamp = XSENS_DATA_TimeStamp_ts(xsens_msg_buf,offset);
#if USE_GPS_XSENS
gps.tow = xsens_time_stamp;
#endif
*/
}
}
void parse_ins_msg( void ) {
uint8_t offset = 0;
if (xsens_id == XSENS_ReqOutputModeAck_ID) {
xsens_output_mode = XSENS_ReqOutputModeAck_mode(xsens_msg_buf);
}
else if (xsens_id == XSENS_ReqOutputSettings_ID) {
xsens_output_settings = XSENS_ReqOutputSettingsAck_settings(xsens_msg_buf);
}
else if (xsens_id == XSENS_ReqMagneticDeclinationAck_ID) {
xsens_declination = DegOfRad (XSENS_ReqMagneticDeclinationAck_declination(xsens_msg_buf) );
DOWNLINK_SEND_IMU_MAG_SETTINGS(DefaultChannel,&xsens_declination,&xsens_declination,&xsens_gps_arm_x,&xsens_gps_arm_y,&xsens_gps_arm_z);
}
else if (xsens_id == XSENS_ReqLeverArmGpsAck_ID) {
xsens_gps_arm_x = XSENS_ReqLeverArmGpsAck_x(xsens_msg_buf);
xsens_gps_arm_y = XSENS_ReqLeverArmGpsAck_y(xsens_msg_buf);
xsens_gps_arm_z = XSENS_ReqLeverArmGpsAck_z(xsens_msg_buf);
DOWNLINK_SEND_IMU_MAG_SETTINGS(DefaultChannel,&xsens_declination,&xsens_declination,&xsens_gps_arm_x,&xsens_gps_arm_y,&xsens_gps_arm_z);
}
else if (xsens_id == XSENS_Error_ID) {
xsens_errorcode = XSENS_Error_errorcode(xsens_msg_buf);
}
#ifdef USE_GPS_XSENS
else if (xsens_id == XSENS_GPSStatus_ID) {
gps.nb_channels = XSENS_GPSStatus_nch(xsens_msg_buf);
gps.num_sv = 0;
gps.last_fix_time = cpu_time_sec;
uint8_t i;
// Do not write outside buffer
for(i = 0; i < Min(gps.nb_channels, GPS_NB_CHANNELS); i++) {
uint8_t ch = XSENS_GPSStatus_chn(xsens_msg_buf,i);
if (ch > gps.nb_channels) continue;
gps.svinfos[ch].svid = XSENS_GPSStatus_svid(xsens_msg_buf, i);
gps.svinfos[ch].flags = XSENS_GPSStatus_bitmask(xsens_msg_buf, i);
gps.svinfos[ch].qi = XSENS_GPSStatus_qi(xsens_msg_buf, i);
gps.svinfos[ch].cno = XSENS_GPSStatus_cnr(xsens_msg_buf, i);
if (gps.svinfos[ch].flags > 0)
{
gps.num_sv++;
}
}
}
#endif
else if (xsens_id == XSENS_MTData_ID) {
/* test RAW modes else calibrated modes */
//if ((XSENS_MASK_RAWInertial(xsens_output_mode)) || (XSENS_MASK_RAWGPS(xsens_output_mode))) {
if (XSENS_MASK_RAWInertial(xsens_output_mode)) {
ins_p = XSENS_DATA_RAWInertial_gyrX(xsens_msg_buf,offset);
ins_q = XSENS_DATA_RAWInertial_gyrY(xsens_msg_buf,offset);
ins_r = XSENS_DATA_RAWInertial_gyrZ(xsens_msg_buf,offset);
offset += XSENS_DATA_RAWInertial_LENGTH;
}
if (XSENS_MASK_RAWGPS(xsens_output_mode)) {
#if defined(USE_GPS_XSENS_RAW_DATA) && defined(USE_GPS_XSENS)
#ifdef GPS_LED
LED_TOGGLE(GPS_LED);
#endif
gps.last_fix_time = cpu_time_sec;
gps.week = 0; // FIXME
gps.tow = XSENS_DATA_RAWGPS_itow(xsens_msg_buf,offset) * 10;
gps.lla_pos.lat = RadOfDeg(XSENS_DATA_RAWGPS_lat(xsens_msg_buf,offset));
gps.lla_pos.lon = RadOfDeg(XSENS_DATA_RAWGPS_lon(xsens_msg_buf,offset));
gps.lla_pos.alt = XSENS_DATA_RAWGPS_alt(xsens_msg_buf,offset);
/* Set the real UTM zone */
gps.utm_pos.zone = (DegOfRad(gps.lla_pos.lon/1e7)+180) / 6 + 1;
lla_f.lat = ((float) gps.lla_pos.lat) / 1e7;
lla_f.lon = ((float) gps.lla_pos.lon) / 1e7;
utm_f.zone = nav_utm_zone0;
/* convert to utm */
utm_of_lla_f(&utm_f, &lla_f);
/* copy results of utm conversion */
gps.utm_pos.east = utm_f.east*100;
gps.utm_pos.north = utm_f.north*100;
gps.utm_pos.alt = gps.lla_pos.alt;
ins_x = utm_f.east;
ins_y = utm_f.north;
// Altitude: Xsens LLH gives ellipsoid height
ins_z = -(INS_FORMAT)XSENS_DATA_RAWGPS_alt(xsens_msg_buf,offset) / 1000.;
// Compute geoid (MSL) height
float hmsl;
WGS84_ELLIPSOID_TO_GEOID(lla_f.lat,lla_f.lon,hmsl);
gps.hmsl = XSENS_DATA_RAWGPS_alt(xsens_msg_buf,offset) - (hmsl * 1000.0f);
ins_vx = ((INS_FORMAT)XSENS_DATA_RAWGPS_vel_n(xsens_msg_buf,offset)) / 100.;
ins_vy = ((INS_FORMAT)XSENS_DATA_RAWGPS_vel_e(xsens_msg_buf,offset)) / 100.;
ins_vz = ((INS_FORMAT)XSENS_DATA_RAWGPS_vel_d(xsens_msg_buf,offset)) / 100.;
gps.ned_vel.x = XSENS_DATA_RAWGPS_vel_n(xsens_msg_buf,offset);
gps.ned_vel.y = XSENS_DATA_RAWGPS_vel_e(xsens_msg_buf,offset);
gps.ned_vel.z = XSENS_DATA_RAWGPS_vel_d(xsens_msg_buf,offset);
gps.pacc = XSENS_DATA_RAWGPS_hacc(xsens_msg_buf,offset) / 100;
gps.sacc = XSENS_DATA_RAWGPS_sacc(xsens_msg_buf,offset) / 100;
gps.pdop = 5; // FIXME Not output by XSens
#endif
offset += XSENS_DATA_RAWGPS_LENGTH;
}
//} else {
if (XSENS_MASK_Temp(xsens_output_mode)) {
offset += XSENS_DATA_Temp_LENGTH;
}
if (XSENS_MASK_Calibrated(xsens_output_mode)) {
uint8_t l = 0;
if (!XSENS_MASK_AccOut(xsens_output_settings)) {
ins_ax = XSENS_DATA_Calibrated_accX(xsens_msg_buf,offset);
ins_ay = XSENS_DATA_Calibrated_accY(xsens_msg_buf,offset);
ins_az = XSENS_DATA_Calibrated_accZ(xsens_msg_buf,offset);
l++;
}
if (!XSENS_MASK_GyrOut(xsens_output_settings)) {
ins_p = XSENS_DATA_Calibrated_gyrX(xsens_msg_buf,offset);
ins_q = XSENS_DATA_Calibrated_gyrY(xsens_msg_buf,offset);
ins_r = XSENS_DATA_Calibrated_gyrZ(xsens_msg_buf,offset);
l++;
}
if (!XSENS_MASK_MagOut(xsens_output_settings)) {
ins_mx = XSENS_DATA_Calibrated_magX(xsens_msg_buf,offset);
ins_my = XSENS_DATA_Calibrated_magY(xsens_msg_buf,offset);
ins_mz = XSENS_DATA_Calibrated_magZ(xsens_msg_buf,offset);
l++;
}
offset += l * XSENS_DATA_Calibrated_LENGTH / 3;
}
if (XSENS_MASK_Orientation(xsens_output_mode)) {
if (XSENS_MASK_OrientationMode(xsens_output_settings) == 0x00) {
float q0 = XSENS_DATA_Quaternion_q0(xsens_msg_buf,offset);
float q1 = XSENS_DATA_Quaternion_q1(xsens_msg_buf,offset);
float q2 = XSENS_DATA_Quaternion_q2(xsens_msg_buf,offset);
float q3 = XSENS_DATA_Quaternion_q3(xsens_msg_buf,offset);
float dcm00 = 1.0 - 2 * (q2*q2 + q3*q3);
float dcm01 = 2 * (q1*q2 + q0*q3);
float dcm02 = 2 * (q1*q3 - q0*q2);
float dcm12 = 2 * (q2*q3 + q0*q1);
float dcm22 = 1.0 - 2 * (q1*q1 + q2*q2);
ins_phi = atan2(dcm12, dcm22);
ins_theta = -asin(dcm02);
ins_psi = atan2(dcm01, dcm00);
offset += XSENS_DATA_Quaternion_LENGTH;
}
if (XSENS_MASK_OrientationMode(xsens_output_settings) == 0x01) {
ins_phi = XSENS_DATA_Euler_roll(xsens_msg_buf,offset) * M_PI / 180;
ins_theta = XSENS_DATA_Euler_pitch(xsens_msg_buf,offset) * M_PI / 180;
ins_psi = XSENS_DATA_Euler_yaw(xsens_msg_buf,offset) * M_PI / 180;
offset += XSENS_DATA_Euler_LENGTH;
}
if (XSENS_MASK_OrientationMode(xsens_output_settings) == 0x10) {
offset += XSENS_DATA_Matrix_LENGTH;
}
new_ins_attitude = 1;
}
if (XSENS_MASK_Auxiliary(xsens_output_mode)) {
uint8_t l = 0;
if (!XSENS_MASK_Aux1Out(xsens_output_settings)) {
l++;
}
if (!XSENS_MASK_Aux2Out(xsens_output_settings)) {
l++;
}
offset += l * XSENS_DATA_Auxiliary_LENGTH / 2;
}
if (XSENS_MASK_Position(xsens_output_mode)) {
#if (!defined(USE_GPS_XSENS_RAW_DATA)) && defined(USE_GPS_XSENS)
gps.last_fix_time = cpu_time_sec;
lla_f.lat = RadOfDeg(XSENS_DATA_Position_lat(xsens_msg_buf,offset));
lla_f.lon = RadOfDeg(XSENS_DATA_Position_lon(xsens_msg_buf,offset));
gps.lla_pos.lat = (int32_t)(lla_f.lat * 1e7);
gps.lla_pos.lon = (int32_t)(lla_f.lon * 1e7);
gps.utm_pos.zone = (DegOfRad(lla_f.lon)+180) / 6 + 1;
/* convert to utm */
utm_of_lla_f(&utm_f, &lla_f);
/* copy results of utm conversion */
gps.utm_pos.east = utm_f.east*100;
gps.utm_pos.north = utm_f.north*100;
ins_x = utm_f.east;
ins_y = utm_f.north;
ins_z = XSENS_DATA_Position_alt(xsens_msg_buf,offset);//TODO is this hms or above ellipsoid?
gps.hmsl = ins_z * 1000;
#endif
offset += XSENS_DATA_Position_LENGTH;
}
if (XSENS_MASK_Velocity(xsens_output_mode)) {
#if (!defined(USE_GPS_XSENS_RAW_DATA)) && defined(USE_GPS_XSENS)
ins_vx = XSENS_DATA_Velocity_vx(xsens_msg_buf,offset);
ins_vy = XSENS_DATA_Velocity_vy(xsens_msg_buf,offset);
ins_vz = XSENS_DATA_Velocity_vz(xsens_msg_buf,offset);
#endif
offset += XSENS_DATA_Velocity_LENGTH;
}
if (XSENS_MASK_Status(xsens_output_mode)) {
xsens_msg_status = XSENS_DATA_Status_status(xsens_msg_buf,offset);
#ifdef USE_GPS_XSENS
if (bit_is_set(xsens_msg_status,2)) gps.fix = GPS_FIX_3D; // gps fix
else if (bit_is_set(xsens_msg_status,1)) gps.fix = 0x01; // efk valid
else gps.fix = GPS_FIX_NONE;
#endif
offset += XSENS_DATA_Status_LENGTH;
}
if (XSENS_MASK_TimeStamp(xsens_output_settings)) {
xsens_time_stamp = XSENS_DATA_TimeStamp_ts(xsens_msg_buf,offset);
#ifdef USE_GPS_XSENS
gps.tow = xsens_time_stamp;
#endif
offset += XSENS_DATA_TimeStamp_LENGTH;
}
if (XSENS_MASK_UTC(xsens_output_settings)) {
xsens_hour = XSENS_DATA_UTC_hour(xsens_msg_buf,offset);
xsens_min = XSENS_DATA_UTC_min(xsens_msg_buf,offset);
xsens_sec = XSENS_DATA_UTC_sec(xsens_msg_buf,offset);
xsens_nanosec = XSENS_DATA_UTC_nanosec(xsens_msg_buf,offset);
xsens_year = XSENS_DATA_UTC_year(xsens_msg_buf,offset);
xsens_month = XSENS_DATA_UTC_month(xsens_msg_buf,offset);
xsens_day = XSENS_DATA_UTC_day(xsens_msg_buf,offset);
offset += XSENS_DATA_UTC_LENGTH;
}
//}
}
}
int main(void) {
uint8_t i, j;
char c;
int8_t bxi = -1;
char buffer[ROWS];
// OSCCAL = 101; // determined experimentally for /my/ ATtiny2313
uart_38400();
// UCSRB |= (1 << RXCIE); // enable rx interrupt. causes glitches.
printstr_p(PSTR("\nWelcome to the LED sign.\nTo update, type the letter 'u' followed by 32 bytes and a newline.\n"));
sei();
// synch_init();
ROW_PORT = 0xff; // 1 == off
ROW_DDR = 0xff;
COLUMN_DDR = _BV(5) | _BV(4) | _BV(3) | _BV(2);
TCCR0B = _BV(CS01) | _BV(CS00); // ck / 64
OCR0A = 0;
TIMSK = _BV(TOIE0) | _BV(OCIE0A); // enable overflow and compare interrupts
sei();
while (1) {
if (intflags.sort) {
intflags.sort = 0;
pwm_copy(p[pdi % 2], bitmap[pdi], ROWS);
}
while (bit_is_set(UCSRA, RXC) && !intflags.sort) {
c = UDR;
if (bxi < 0) {
if (c == 'u') { // 'update'
printstr_p(PSTR("u"));
bxi++;
break;
}
} else if (bxi < (ROWS * COLUMNS)) {
buffer[bxi % ROWS] = c;
if((bxi % ROWS) == (ROWS-1)) {
j = (bxi / ROWS);
for(i=0; i<ROWS; i++) {
bitmap[j][i] = buffer[i];
}
itoa(j, buffer, 10);
printstr(buffer);
}
bxi++;
} else {
printstr_p(PSTR(" OK\n"));
bxi = -1;
}
}
}
return 0;
}
void adc_wait_stop()
{
cbi(ADCSRA, ADIE);
while(bit_is_set(ADCSRA, ADSC));
}
int
main(void)
{
wdt_enable(WDTO_2S);
#if defined(CUL_ARDUINO)
clock_prescale_set(clock_div_1); // for 8MHz clock div schould be 1
#endif
MARK433_PORT |= _BV( MARK433_BIT ); // Pull 433MHz marker
MARK915_PORT |= _BV( MARK915_BIT ); // Pull 915MHz marker
// if we had been restarted by watchdog check the REQ BootLoader byte in the
// EEPROM ...
if(bit_is_set(MCUSR,WDRF) && erb(EE_REQBL)) {
ewb( EE_REQBL, 0 ); // clear flag
start_bootloader();
}
// Setup the timers. Are needed for watchdog-reset
OCR0A = 249; // Timer0: 0.008s = 8MHz/256/250
TCCR0B = _BV(CS02);
TCCR0A = _BV(WGM01);
TIMSK0 = _BV(OCIE0A);
TCCR1A = 0;
TCCR1B = _BV(CS11) | _BV(WGM12); // Timer1: 1us = 8MHz/8
MCUSR &= ~(1 << WDRF); // Enable the watchdog
led_init();
spi_init();
eeprom_init();
USB_Init();
fht_init();
tx_init();
input_handle_func = analyze_ttydata;
#ifdef HAS_RF_ROUTER
rf_router_init();
display_channel = (DISPLAY_USB|DISPLAY_RFROUTER);
#else
display_channel = DISPLAY_USB;
#endif
checkFrequency();
LED_OFF();
for(;;) {
USB_USBTask();
CDC_Task();
RfAnalyze_Task();
Minute_Task();
#ifdef HAS_FASTRF
FastRF_Task();
#endif
#ifdef HAS_RF_ROUTER
rf_router_task();
#endif
#ifdef HAS_ASKSIN
rf_asksin_task();
#endif
#ifdef HAS_MORITZ
rf_moritz_task();
#endif
#ifdef HAS_RWE
rf_rwe_task();
#endif
#ifdef HAS_RFNATIVE
native_task();
#endif
#ifdef HAS_KOPP_FC
kopp_fc_task();
#endif
#ifdef HAS_MBUS
rf_mbus_task();
#endif
#ifdef HAS_ZWAVE
rf_zwave_task();
#endif
}
}
