// get random seed from wideband noise rssi
void radio_init () {
hal_disableIRQs();
// manually reset radio
#ifdef CFG_sx1276_radio
hal_pin_rst(0); // drive RST pin low
#else
hal_pin_rst(1); // drive RST pin high
#endif
hal_waitUntil(os_getTime()+ms2osticks(1)); // wait >100us
hal_pin_rst(2); // configure RST pin floating!
hal_waitUntil(os_getTime()+ms2osticks(5)); // wait 5ms
opmode(OPMODE_SLEEP);
// some sanity checks, e.g., read version number
u1_t v = readReg(RegVersion);
#ifdef CFG_sx1276_radio
ASSERT(v == 0x12 );
#elif CFG_sx1272_radio
ASSERT(v == 0x22);
#else
#error Missing CFG_sx1272_radio/CFG_sx1276_radio
#endif
// seed 15-byte randomness via noise rssi
rxlora(RXMODE_RSSI);
while( (readReg(RegOpMode) & OPMODE_MASK) != OPMODE_RX ); // continuous rx
for(int i=1; i<16; i++) {
for(int j=0; j<8; j++) {
u1_t b; // wait for two non-identical subsequent least-significant bits
while( (b = readReg(LORARegRssiWideband) & 0x01) == (readReg(LORARegRssiWideband) & 0x01) );
randbuf[i] = (randbuf[i] << 1) | b;
}
}
randbuf[0] = 16; // set initial index
#ifdef CFG_sx1276mb1_board
// chain calibration
writeReg(RegPaConfig, 0);
// Launch Rx chain calibration for LF band
writeReg(FSKRegImageCal, (readReg(FSKRegImageCal) & RF_IMAGECAL_IMAGECAL_MASK)|RF_IMAGECAL_IMAGECAL_START);
while((readReg(FSKRegImageCal)&RF_IMAGECAL_IMAGECAL_RUNNING) == RF_IMAGECAL_IMAGECAL_RUNNING){ ; }
// Sets a Frequency in HF band
u4_t frf = 868000000;
writeReg(RegFrfMsb, (u1_t)(frf>>16));
writeReg(RegFrfMid, (u1_t)(frf>> 8));
writeReg(RegFrfLsb, (u1_t)(frf>> 0));
// Launch Rx chain calibration for HF band
writeReg(FSKRegImageCal, (readReg(FSKRegImageCal) & RF_IMAGECAL_IMAGECAL_MASK)|RF_IMAGECAL_IMAGECAL_START);
while((readReg(FSKRegImageCal) & RF_IMAGECAL_IMAGECAL_RUNNING) == RF_IMAGECAL_IMAGECAL_RUNNING) { ; }
#endif /* CFG_sx1276mb1_board */
opmode(OPMODE_SLEEP);
hal_enableIRQs();
}
/**
* Überprüft ob sich die Räder drehen
*/
boolean_t is_driving(){
portTickType lastTime = os_getTime();
uint8_t first = PDR01_P5;
while (os_getTime() - lastTime < 500){
if (first == 1 && PDR01_P5 == 0) return TRUE;
if (first == 0 && PDR01_P5 == 1) return TRUE;
}
return FALSE;
}
// called by EXTI_IRQHandler
// (set preprocessor option CFG_EXTI_IRQ_HANDLER=sensorirq)
void sensorirq () {
if((EXTI->PR & (1<<INP_PIN)) != 0) { // pending
EXTI->PR = (1<<INP_PIN); // clear irq
// run application callback function in 50ms (debounce)
os_setTimedCallback(&irqjob, os_getTime()+ms2osticks(50), irqjob.func);
}
}
static void blinkfunc (osjob_t* j) {
// toggle LED
ledstate = !ledstate;
debug_led(ledstate);
// reschedule blink job
os_setTimedCallback(j, os_getTime()+ms2osticks(100), blinkfunc);
}
void do_send(osjob_t* j){
// Serial.print("Time: ");
// Serial.println(millis() / 1000);
printf("Time: %lu\n", (unsigned int) (millis() / 1000));
// Show TX channel (channel numbers are local to LMIC)
// Serial.print("Send, txCnhl: ");
// Serial.println(LMIC.txChnl);
// Serial.print("Opmode check: ");
printf("Send, txCnhl: %u Opmode check: ", LMIC.txChnl);
// Check if there is not a current TX/RX job running
if (LMIC.opmode & (1 << 7)) {
//Serial.println("OP_TXRXPEND, not sending");
printf("OP_TXRXPEND, not sending\n");
} else {
//Serial.println("ok");
printf("ok\n");
// Prepare upstream data transmission at the next possible time.
// LMIC_setTxData2(1, mydata, sizeof(mydata)-1, 0);
char buf[128];
sprintf(buf, "NYC TTN test packet %u\n", xmit_count++);
LMIC_setTxData2(1, (unsigned char *) buf, strlen(buf)-1, 0);
printf("sending %s\n", buf);
}
// Schedule a timed job to run at the given timestamp (absolute system time)
os_setTimedCallback(j, os_getTime()+sec2osticks(TRANSMIT_INTERVAL), do_send);
}
static void battery_task(void) {
uint16_t batRaw;
portTickType lastWakeTime = os_getTime();
for (;;) {
os_frequency(&lastWakeTime, 50);
batRaw = ADC_GetValue(10);
batRaw *= 39;
batRaw /= 4;
batRaw += 550;
measures[nextPos] = batRaw;
nextPos = (nextPos + 1) % AVERAGING_WINDOW_SIZE;
}
}
//set Parameters for driving and fill the driveParameterBuffer for intertask communication
void drive(int8_t speed, int8_t directionPercent, portTickType duration_ms) {
driveParameters par;
#ifdef NAVIGATION_THREAD
par.direction = directionPercent;
par.speed = speed;
par.duration = duration_ms;
par.startTime = os_getTime();
xQueueSendToFront(driveParameterBuffer, &par, (portTickType)10);
//#endif
#else
Drive_SetServo(directionPercent);
Drive_SetMotor(speed);
#endif
}
/**
* Hilfsmethode zum Fahren Etappen fester Länge
*/
void driveDistance(void){
if (DriveLength > 0){
if (wheel_turns <= 0){
wheel_turns = (DriveLength)/HALL_RESOLUTION;
realdistance = 0;
//Zwei Wechsel der Sensorwerte = 1 Radumdrehung = 21,04cm
if ((DriveLength*10) % (int16_t)(HALL_RESOLUTION*10) > ((DriveLength*10)/2)) wheel_turns++; //Faktor 10 weil kein Mod mit double möglich
set_distanceReached(FALSE);
}
if (Drive_Speed > 3 && wheel_turns > 2) wheel_turns = wheel_turns - 2; //weil bremsen so lange dauert
if ( init == FALSE){
driveTime = os_getTime();
init = TRUE;
}
if (distance - wheel_turns < 0){
Seg_Hex(0x07);
}
if (distance - wheel_turns >= 0){
//Werte zurücksetzen
wheel_turns = 0;
init = FALSE;
drive(0,0,0);
Drive_SetMotor(0);
realdistance = 0;
DriveLength = 0;
set_distanceReached(TRUE);
Seg_Hex(0x08);
}
}
}
void bt_send_sensor_data(void) {
portTickType lastWakeTime;
wirelessMessage_t msg;
int16_t bat;
msg.data[0] = 0;
msg.dataLength = 16;
msg.destinationId = 0;
msg.messageType = PT_ANDROID_SENSORDATA;
msg.priority = 0;
lastWakeTime = os_getTime();
for (;;) {
os_frequency(&lastWakeTime, 100);
bat = Battery_GetVoltage();
msg.data[1] = Us_Data.Front_Distance & 0xff;
msg.data[2] = (Us_Data.Front_Distance >> 8) & 0xff;
msg.data[3] = Us_Data.Rear_Distance & 0xff;
msg.data[4] = (Us_Data.Rear_Distance >> 8) & 0xff;
msg.data[5] = Us_Data.Left_Distance & 0xff;
msg.data[6] = (Us_Data.Left_Distance >> 8) & 0xff;
msg.data[7] = Us_Data.Right_Distance & 0xff;
msg.data[8] = (Us_Data.Right_Distance >> 8) & 0xff;
msg.data[9] = linefound;
msg.data[10] = averagePos & 0xff;
msg.data[11] = (averagePos >> 8) & 0xff;
msg.data[12] = (averagePos >> 16) & 0xff;
msg.data[13] = (averagePos >> 24) & 0xff;
msg.data[14] = bat & 0xff;
msg.data[15] = (bat >> 8) & 0xff;
wirelessSend(WI_IF_BLUETOOTH, &msg);
}
}
// called by hal ext IRQ handler
// (radio goes to stanby mode after tx/rx operations)
void radio_irq_handler (u1_t dio) {
ostime_t now = os_getTime();
if( (readReg(RegOpMode) & OPMODE_LORA) != 0) { // LORA modem
u1_t flags = readReg(LORARegIrqFlags);
if( flags & IRQ_LORA_TXDONE_MASK ) {
// save exact tx time
LMIC.txend = now - us2osticks(43); // TXDONE FIXUP
} else if( flags & IRQ_LORA_RXDONE_MASK ) {
// save exact rx time
if(getBw(LMIC.rps) == BW125) {
now -= LORA_RXDONE_FIXUP[getSf(LMIC.rps)];
}
LMIC.rxtime = now;
// read the PDU and inform the MAC that we received something
LMIC.dataLen = (readReg(LORARegModemConfig1) & SX1272_MC1_IMPLICIT_HEADER_MODE_ON) ?
readReg(LORARegPayloadLength) : readReg(LORARegRxNbBytes);
// set FIFO read address pointer
writeReg(LORARegFifoAddrPtr, readReg(LORARegFifoRxCurrentAddr));
// now read the FIFO
readBuf(RegFifo, LMIC.frame, LMIC.dataLen);
// read rx quality parameters
LMIC.snr = readReg(LORARegPktSnrValue); // SNR [dB] * 4
LMIC.rssi = readReg(LORARegPktRssiValue) - 125 + 64; // RSSI [dBm] (-196...+63)
} else if( flags & IRQ_LORA_RXTOUT_MASK ) {
// indicate timeout
LMIC.dataLen = 0;
}
// mask all radio IRQs
writeReg(LORARegIrqFlagsMask, 0xFF);
// clear radio IRQ flags
writeReg(LORARegIrqFlags, 0xFF);
} else { // FSK modem
u1_t flags1 = readReg(FSKRegIrqFlags1);
u1_t flags2 = readReg(FSKRegIrqFlags2);
if( flags2 & IRQ_FSK2_PACKETSENT_MASK ) {
// save exact tx time
LMIC.txend = now;
} else if( flags2 & IRQ_FSK2_PAYLOADREADY_MASK ) {
// save exact rx time
LMIC.rxtime = now;
// read the PDU and inform the MAC that we received something
LMIC.dataLen = readReg(FSKRegPayloadLength);
// now read the FIFO
readBuf(RegFifo, LMIC.frame, LMIC.dataLen);
// read rx quality parameters
LMIC.snr = 0; // determine snr
LMIC.rssi = 0; // determine rssi
} else if( flags1 & IRQ_FSK1_TIMEOUT_MASK ) {
// indicate timeout
LMIC.dataLen = 0;
} else {
//while(1);
// Do not infinite loop when interrupt triggers unexpectedly
// Instead, call ASSERT, which calls hal_failed, so there is at least a
// traceable error. (MK)
ASSERT(0);
}
}
// go from stanby to sleep
opmode(OPMODE_SLEEP);
// run os job (use preset func ptr)
os_setCallback(&LMIC.osjob, LMIC.osjob.func);
}
/*
* set drive parameters from driveParameterBuffer if new parameters are available,
* otherwise stop the car when specified duration expires for security reasons
*/
static void driveTask(void) {
portTickType duration_ms = 0, directionPercent = 0, currentTime = 0,
lastWakeTime = 0, startTime = 0;
driveParameters par;
int8_t queueReturnValue, speed = 0;
double Ergebnis[3];
gps_reducedData_t *own;
lastWakeTime = os_getTime();
for (;;) {
// calls[6]++;
#ifdef SLAM_SCENARIO
os_frequency(&lastWakeTime, 1);
#else
os_frequency(&lastWakeTime, 60);
#endif
currentTime = os_getTime();
//get new drive commands from queue
queueReturnValue = xQueueReceive(driveParameterBuffer, &par, 10);
if (queueReturnValue == pdTRUE) {
directionPercent = par.direction;
speed = par.speed;
startTime = par.startTime;
duration_ms = par.duration;
}
if (hall != start){
start = !start;
if (os_getTime() - driveTime > 400){
distance++;
#ifdef SLAM_SCENARIO
Drive_SetMotor(0);
os_wait(200);
own = get_ownCoords();
SLAM_Algorithm((int32_t)CoordinatesToMap((int32_t)own->x),(int32_t)CoordinatesToMap((int32_t)own->y),(int32_t)uint16DegreeRelativeToX(own->angle),get_accumulateSteeringAngle()/MAX_STEERING_ANGLE, (int32_t) HALL_RESOLUTION, Ergebnis);
// send_Element(22);
// send_Element((int16_t)Ergebnis[0]);
// send_Element((int16_t)Ergebnis[1]);
// send_Element(-1);
set_ownCoords(MapToCoordinates((int16_t)Ergebnis[0]),MapToCoordinates((int16_t)Ergebnis[1]),MapToCoordinates((int16_t)Ergebnis[2]));
accumulateSteeringAngle(INT_LEAST8_MAX);
#endif
}
}
//set speed and direction if specified duration not expired, else stop the car
if (duration_ms < 600){
if (currentTime - startTime < duration_ms) {
#ifdef DEBUG2
printf("\nDRIVE\nset motor speed to %d and angle to %d\n\r", speed,
directionPercent);
#endif
Drive_SetServo(directionPercent);
Drive_SetMotor(speed);
driveDistance();
} else {
Drive_SetMotor(0); //stop if time period expired;
distance = 0;
realdistance = 0;
}
}
else{
Drive_SetServo(directionPercent);
Drive_SetMotor(speed);
if (speed == 0){
distance = 0;
realdistance = 0;
}
driveDistance();
}
}
}
void MappingControllerThread(void) {
int16_t backLength; // the distance to the center of a crossing
int16_t w_right, w_left, w_front, w_rear; // the widths of a crossing
// needed for collecting us sensor data for the information about edges
int8_t us_pos = 0;
uint8_t wait_period = 1;
int8_t k = 1;
map_node_info node_info; // info about the crossing that shall be saved in the map
for (;;) {
// Seg_Hex(0x8);
os_wait(250);
// description of the algorithm for collecting this data:
// for every edge each 10 ( = NUMBER_US_VALUES) values of us left and right sensor data
// shall be saved. Therefore the array us_values is filled alternately with
// left (even positions) and right (odd positions) us sensor data. If the array is full,
// every second value will be deleted and afterwards only every second measured
// us sensor data is saved in the array to keep the distance between two values constant.
// and so on.
// --> The array will always be at least half-full and contain us sensor values with
// equal distances between them.
if (k == wait_period && getWidthFront()==0) {
// collect us values for edge information
if (us_pos < 2 * NUMBER_US_VALUES) {
us_values[us_pos] = Us_Data.Left_Distance; // even value: left
us_values[us_pos + 1] = Us_Data.Right_Distance; // odd value: right
us_pos += 2;
} else {
// take every second value and increment the wait period
// the array is half-full afterwards
uint8_t n;
for (n = 0; n < NUMBER_US_VALUES; n += 2) {
us_values[n] = us_values[2 * n];
us_values[n + 1] = us_values[2 * n + 1];
}
us_pos /= 2;
wait_period = wait_period<<=1; // wait_period * 2
}
k = 1; // reset k
} else
k++;
// ---------------------
// START of crossing
if (getCrossingStarted() == 1) {
uint8_t edge_length;
// calculate length of last edge
edge_length = calculateDrivenLen(os_getTime() - getTimeEdgeOld());
// add edge to map and increase the currentNodeNumber
addEdge(currentNodeNumber, ++currentNodeNumber, edge_length);
// reset value
resetCrossingStarted();
}
// ---------------------
// END of crossing
if (getCrossingDetected() == 0)
continue;
#ifdef MAPPINGCONTROLLER_DEBUG
printf("Crossing detected, MappingController is handling it...\r\n");
#endif
stopCrossingAnalyzer();
stopWallFollow();
// get the widths and add this information to the node info
w_left = getWidthLeft();
w_right = getWidthRight();
w_front = getWidthFront();
w_rear = getWidthRear();
node_info.w0 = w_front;
node_info.w1 = w_left;
node_info.w2 = w_rear;
node_info.w3 = w_right;
// Filter open doors etc.
// the width of a corridor must be bigger than 40cm
if (w_front < 40)
w_front = 0;
if (w_rear < 40)
w_rear = 0;
if (w_left < 40)
w_left = 0;
if (w_right < 40)
w_right = 0;
// Drive backwards to center of crossing
if (w_left != 0 && w_right != 0)
backLength = (w_right + w_left) / 4;
else
backLength = (w_right + w_left) / 2;
if (backLength == 0) // dead end
backLength = 20;
if (w_rear == 0)
backLength -= 20;
backLength *= -1; // backwards driving
#ifdef MAPPINGCONTROLLER_DEBUG
printf("Driving %dcm\r\n", backLength);
#endif
driveLen(backLength);
// Handle different crossing types
// width_front = direction the car came from
// width_rear = direction the car was heading to
// side_end: 1: coming up, 2: left, 3: straight, 4: right
// add information about the edge to the map and resume driving
if (w_left == 0 && w_right == 0 && w_front != 0 && w_rear == 0) {
noteCrossing(DeadEnd);
addEdgeInfo(last_edge_side, 1, NUMBER_US_VALUES, us_values);
last_edge_side = 1;
node_info.crossing_type = DeadEnd;
turn180(RIGHT, BACKWARDS);
resumeDriving(0);
} else if (w_left != 0 && w_right == 0 && w_front != 0 && w_rear != 0) {
noteCrossing(LeftStraight);
addEdgeInfo(last_edge_side, 3, NUMBER_US_VALUES, us_values);
last_edge_side = 3;
node_info.crossing_type = LeftStraight;
// Behavior: drive straight ahead!
resumeDriving(w_left / 2 + 50);
} else if (w_left == 0 && w_right != 0 && w_front != 0 && w_rear != 0) {
noteCrossing(RightStraight);
addEdgeInfo(last_edge_side, 4, NUMBER_US_VALUES, us_values);
last_edge_side = 4;
node_info.crossing_type = RightStraight;
turn90(RIGHT, BACKWARDS);
resumeDriving(w_rear / 2 + 50);
} else if (w_left == 0 && w_right != 0 && w_front != 0 && w_rear == 0) {
noteCrossing(RightOnly);
addEdgeInfo(last_edge_side, 4, NUMBER_US_VALUES, us_values);
last_edge_side = 4;
node_info.crossing_type = RightOnly;
turn90(RIGHT, BACKWARDS);
resumeDriving(w_rear / 2 + 50);
} else if (w_left != 0 && w_right == 0 && w_front != 0 && w_rear == 0) {
noteCrossing(LeftOnly);
addEdgeInfo(last_edge_side, 2, NUMBER_US_VALUES, us_values);
last_edge_side = 2;
node_info.crossing_type = LeftOnly;
turn90(LEFT, BACKWARDS);
resumeDriving(w_rear / 2 + 50);
} else if (w_left != 0 && w_right != 0 && w_front != 0 && w_rear == 0) {
noteCrossing(LeftRight);
addEdgeInfo(last_edge_side, 2, NUMBER_US_VALUES, us_values);
last_edge_side = 2;
node_info.crossing_type = LeftRight;
turn90(LEFT, BACKWARDS);
resumeDriving(w_rear / 2 + 50);
} else if (w_left != 0 && w_right != 0 && w_front != 0 && w_rear != 0) {
noteCrossing(All);
addEdgeInfo(last_edge_side, 3, NUMBER_US_VALUES, us_values);
last_edge_side = 3;
node_info.crossing_type = All;
// Behavior: drive straight ahead!
resumeDriving(w_left / 2 + 50);
} else { // dunno
resumeDriving(0);
}
// add information about the current node
addNodeInfo(currentNodeNumber, node_info);
// reset values
for (k = 0; k < 2 * NUMBER_US_VALUES; k++) {
us_values[k] = 0;
}
}
}