Exemplo n.º 1
0
//!  Checks Target Vdd  - Updates Target Vdd LED & status
//!
//!  Updates \ref cable_status
//!
uint8_t bdm_checkTargetVdd(void) {
#if (HW_CAPABILITY&CAP_VDDSENSE)
   if (bdm_targetVddMeasure() > VDD_2v) {
      redLedOn();
      if (bdm_option.targetVdd == BDM_TARGET_VDD_OFF)
         cable_status.power = BDM_TARGET_VDD_EXT;
      else
         cable_status.power = BDM_TARGET_VDD_INT;
   }
   else {
      redLedOff();
      if (bdm_option.targetVdd == BDM_TARGET_VDD_OFF)
         cable_status.power = BDM_TARGET_VDD_NONE;
      else {
    	 // Possible overload
         cable_status.power = BDM_TARGET_VDD_ERR;
         VDD_OFF();
      }
   }
#else
   // No target Vdd sensing - assume external Vdd is present
   cable_status.power = BDM_TARGET_VDD_EXT;
#endif // CAP_VDDSENSE
   if ((cable_status.power == BDM_TARGET_VDD_NONE) ||
       (cable_status.power == BDM_TARGET_VDD_ERR))
      return BDM_RC_VDD_NOT_PRESENT;
   return BDM_RC_OK;
}
void led_initialise(void) {
#ifdef LED_GREEN_PORT
    SIM->SCGC5 |=  PORT_CLOCK_MASK(LED_GREEN_PORT);
    greenLedOff();
    PDDR(LED_GREEN_PORT)               |= (1<<LED_GREEN_NUM);
    PCR(LED_GREEN_PORT, LED_GREEN_NUM)  = PORT_PCR_MUX(1)|PORT_PCR_DSE_MASK;
#endif

#ifdef LED_RED_PORT
    SIM->SCGC5 |=  PORT_CLOCK_MASK(LED_RED_PORT);
    redLedOff();
    PDDR(LED_RED_PORT)             |= (1<<LED_RED_NUM);
    PCR(LED_RED_PORT, LED_RED_NUM)  = PORT_PCR_MUX(1)|PORT_PCR_DSE_MASK;
#endif

#ifdef LED_BLUE_PORT
    SIM->SCGC5 |=  PORT_CLOCK_MASK(LED_BLUE_PORT);
    blueLedOff();
    PDDR(LED_BLUE_PORT)              |= (1<<LED_BLUE_NUM);
    PCR(LED_BLUE_PORT, LED_BLUE_NUM)  = PORT_PCR_MUX(1)|PORT_PCR_DSE_MASK;
#endif

#ifdef LED_ORANGE_PORT
    SIM->SCGC5 |=  PORT_CLOCK_MASK(LED_ORANGE_PORT);
    orangeLedOff();
    PDDR(LED_ORANGE_PORT)                |= (1<<LED_ORANGE_NUM);
    PCR(LED_ORANGE_PORT, LED_ORANGE_NUM)  = PORT_PCR_MUX(1)|PORT_PCR_DSE_MASK;
#endif
}
Exemplo n.º 3
0
Arquivo: main.c Projeto: atiselsts/osw
void appMain(void)
{
    static uint8_t buffer[100];

    radioSetTxPower(TX_POWER);
    radioSetReceiveHandle(rcvRadio);
    radioOn();

    uint32_t lastTime = getTimeMs();

    for (;;) {
        int8_t rssi = radioGetLastRSSI();

        if ((int32_t) getTimeMs() - (int32_t) lastTime > 1000) {
            radioSend(buffer, sizeof(buffer));

            PRINTF("rssi=%d\n", rssi);
            lastTime = getTimeMs();
            if (rxOk) --rxOk;

            if (!rxOk) redLedOn();
            else redLedOff();
        }

        greenLedToggle();
        busyWait(getWaitInterval(rssi));
    }
}
Exemplo n.º 4
0
static void calcNewProb(float sdev, bool wasMoving, bool pastBack,
                        bool pastFwd, bool nowFwd, bool diffFromPrevious)
{
    static int8_t fullBNVariables[TOTAL_STATES];
    fullBNVariables[MOVING] = NOT_SET;
    fullBNVariables[WAS_MOVING] = wasMoving;
    fullBNVariables[PAST_BRAKE] = pastBack;
    fullBNVariables[PAST_ACCEL] = pastFwd;
    fullBNVariables[ACCEL] = nowFwd;
    fullBNVariables[SSD] = sdev >= THRESH_LOW;
    fullBNVariables[MSD] = sdev >= THRESH_MEDIUM;
    fullBNVariables[LSD] = sdev >= THRESH_HIGH;
    fullBNVariables[DIFF] = diffFromPrevious;

    float probability = fullBNConditionalProbability(MOVING, fullBNVariables);

//    PRINTF("probability=%u%%\n\n", (uint16_t)(probability * 100));

    bayesStandingMode = probability < 0.5;

    if (bayesStandingMode) redLedOff();
    else redLedOn();

    clearGlobalVectors();
}
Exemplo n.º 5
0
//-------------------------------------------
//      Entry point for the application
//-------------------------------------------
void appMain(void)
{
    PRINTF("\n\nAccelerometer data gathering app\n");

    extFlashWake();

    // start with clean and new...
    cleanFlash();

    accelOn();

    userButtonEnable(onUserButton);

    mdelay(START_DELAY);

    uint16_t lastSecond;
    uint16_t samplesPerSecond;

    for (;;) {
        while (!isActiveMode);

        redLedOn();

        lastSecond = getTimeSec();
        samplesPerSecond = 0;

        uint16_t i;
        for (i = 0; isActiveMode; i++) {
            if (i % 10 == 0) redLedToggle();

            uint32_t now = getTimeMs();
            uint32_t endTime = now + MAIN_LOOP_LENGTH;

            Packet_t p;
            p.timestamp = now;

            p.acc_x = accelReadX();
            p.acc_y = accelReadY();
            p.acc_z = accelReadZ();

            printPacket(&p);

            samplesPerSecond++;

            uint16_t currentSecond = getTimeSec();
            if (currentSecond != lastSecond) {
                PRINTF("samples per second = %u\n", samplesPerSecond);
                lastSecond = currentSecond;
                samplesPerSecond = 0;
            } 

            while (timeAfter32(endTime, getTimeMs()));
        }

        redLedOff();
    }
}
Exemplo n.º 6
0
void onBlinkTimer(void *x)
{
    redLedOn();
    mdelay(100);
    redLedOff();

    uint32_t now = getTimeMs();
    uint32_t untilFrameEnd = BLINK_INTERVAL - now % BLINK_INTERVAL;
    alarmSchedule(&blinkTimer, untilFrameEnd);
}
void led_initialise(void) {
#ifdef LED_GREEN_PORT
   greenLedOff();
   PDDR(LED_GREEN_PORT)               |= (1<<LED_GREEN_NUM);
#endif

#ifdef LED_RED_PORT
   redLedOff();
   PDDR(LED_RED_PORT)             |= (1<<LED_RED_NUM);
#endif

#ifdef LED_BLUE_PORT
   blueLedOff();
   PDDR(LED_BLUE_PORT)              |= (1<<LED_BLUE_NUM);
#endif

#ifdef LED_ORANGE_PORT
   orangeLedOff();
   PDDR(LED_ORANGE_PORT)                |= (1<<LED_ORANGE_NUM);
#endif
}
Exemplo n.º 8
0
int main(void)
{
    // Make sure all interrupts are disabled.
    // This must be the first step, because in some cases interupt vectors are totally wrong
    // (e.g. when we get here after a soft reboot from another application)
    msp430ClearAllInterruptsNosave();

    msp430WatchdogStop();

    ledsInit();

    flashLeds(LEDS_BOOTLOADER_START);

    BootParams_t bootParams;
    intFlashRead(BOOT_PARAMS_ADDRESS, &bootParams, sizeof(bootParams));

    ++bootParams.bootRetryCount;
    if (bootParams.bootRetryCount > MAX_RETRY_COUNT) {
        bootParams.bootRetryCount = 0;
        bootParams.extFlashAddress = GOLDEN_IMAGE_ADDRESS;
        bootParams.doReprogramming = 1;
    }

    // make sure internal flash address is sane
    if (bootParams.intFlashAddress == 0xffff || bootParams.intFlashAddress < BOOTLOADER_END) {
        bootParams.intFlashAddress = SYSTEM_CODE_START;
    }

    // read voltage, and quit if not enough for writing in flash
    if (readVoltage() < THRESHOLD_VOLTAGE) {
        flashLeds(LEDS_LOW_BATTERY);
        goto exec;
    }

    // write the updated info back in flash
    intFlashErase(BOOT_PARAMS_ADDRESS, sizeof(bootParams));
    intFlashWrite(BOOT_PARAMS_ADDRESS, &bootParams, sizeof(bootParams));

    if (bootParams.doReprogramming) {
        redLedOn();

        // will be using external flash
        extFlashInit();
        extFlashWake();

        uint32_t extAddress = bootParams.extFlashAddress;

        // read number of blocks
        uint16_t imageBlockCount;
        extFlashRead(extAddress, &imageBlockCount, sizeof(uint16_t));
        extAddress += 2;

        while (imageBlockCount) {
            // read a block from external flash
            ExternalFlashBlock_t block;
            extFlashRead(extAddress, &block, sizeof(block));

            if (block.crc != crc16((uint8_t *)&block, sizeof(block) - 2)) {
                // the best we can do is to reboot now;
                // after a few tries the golden image will be loaded
                flashLeds(LEDS_CRC_ERROR);
                // no need to disable all of the interrupts (they already are),
                // just write in watchdog timer wihout password, it will generate reset.
                watchdogRebootSimple();
            }

            bool firstBlockInChunk = block.address & 0x1;
            block.address &= ~0x1;
            if (firstBlockInChunk) {
                // prepare internal flash to be written
                intFlashErase(block.address, INT_FLASH_SEGMENT_SIZE);
            }

            // program internal flash
            COMPILE_TIME_ASSERT(sizeof(block.data) == INT_FLASH_BLOCK_SIZE, ifs);
            intFlashWriteBlock(block.address, block.data, INT_FLASH_BLOCK_SIZE);

            --imageBlockCount;
            extAddress += sizeof(ExternalFlashBlock_t);
        }

        extFlashSleep();
        redLedOff();
    }

#if ENABLE_BOOT_DELAY
    // delay for a second or so to allow the user to interrupt booting (by pressing the reset button)
    flashLeds(LEDS_BOOTLOADER_END);
#endif

    // execute the program
  exec:
    ((ApplicationStartExec)bootParams.intFlashAddress)();
}
Exemplo n.º 9
0
//-------------------------------------------
//      Entry point for the application
//-------------------------------------------
void appMain(void)
{
    while (1)
    {
        static uint_t i;
        // test 1: counter 0-7
        for (i = 0; i < 8; ++i) {
            ledsSet(i);
            msleep(PAUSE);
        }

        // test 2: all off, then red on/off, then green on/off, finally blue on/off
        ledsSet(0);
        msleep(PAUSE);
        redLedOn();
        msleep(PAUSE);
        redLedOff();
        msleep(PAUSE);

        greenLedOn();
        msleep(PAUSE);
        greenLedOff();
        msleep(PAUSE);

        blueLedOn();
        msleep(PAUSE);
        blueLedOff();
        msleep(PAUSE);

        // test 3: all on, then blue off, green off, red off
        ledsSet(7);
        msleep(PAUSE);
        blueLedOff();
        msleep(PAUSE);
        greenLedOff();
        msleep(PAUSE);
        redLedOff();
        msleep(PAUSE);

        // test 4: repeat last two tests with toggle
        redLedToggle();
        msleep(PAUSE);
        redLedToggle();
        msleep(PAUSE);

        greenLedToggle();
        msleep(PAUSE);
        greenLedToggle();
        msleep(PAUSE);

        blueLedToggle();
        msleep(PAUSE);
        blueLedToggle();
        msleep(PAUSE);

        ledsSet(7);
        msleep(PAUSE);
        blueLedToggle();
        msleep(PAUSE);
        greenLedToggle();
        msleep(PAUSE);
        redLedToggle();
        msleep(PAUSE);

        // test 5: check that isOn functions work
        ledsSet(0);
        ASSERT(!redLedGet());
        ASSERT(!greenLedGet());
        ASSERT(!blueLedGet());
        ledsSet(7);
        ASSERT(redLedGet());
        ASSERT(greenLedGet());
        ASSERT(blueLedGet());
    } // EOF while (1)
}
Exemplo n.º 10
0
//should be called from controller_commit, probably disable timeout beforehand
void cn_calc_force_lookup(float minAccel, float maxAccel) {
	//sword i,j;
	sword x,y;
	float angle,dist;
	float angle2, axisAngle;
	float ftemp;
	float p1x,p1y,p2x,p2y,p3x,p3y;
	float abx,aby;
	float a,b,c;
	float axisPx[3], axisPy[3];
	float baxPx[3], baxPy[3];
	float circAngleDelta;
	uword axisI;
	udword curIdx = 0;	
	//udword now = cn_getRTC();
	//udword ttime;
	
	//cn_stop_command_timeout();
#ifdef VMC
redLedOff();
#endif	
	//c is the vector distance along the robot axis vector to the edge of the circle segment from the edge in between axis
	c = - minAccel/(maxAccel*2);  //c = minAccel * cos(2*PI/3) / maxAccel;
	c += sqrt( c*c - minAccel*minAccel/(maxAccel*maxAccel) +1);	
	
	axisPy[0] = 0;//sin(0)*maxAccel;
	axisPx[0] = maxAccel;//cos(0)*maxAccel;
	
	axisPy[1] = sin(TWO_PI_THIRD)*maxAccel;
	axisPx[1] = cos(TWO_PI_THIRD)*maxAccel;
	
	axisPy[2] = sin(FOUR_PI_THIRD)*maxAccel;
	axisPx[2] = cos(FOUR_PI_THIRD)*maxAccel;
	
	baxPy[0]  = sin(PI_THIRD)*minAccel;
	baxPx[0]  = cos(PI_THIRD)*minAccel;

	baxPy[1]  = 0;//*minAccel; //sin(3*PI/3)
	baxPx[1]  = -minAccel; //cos(3*PI/3)

	baxPy[2]  = sin(5*PI_THIRD)*minAccel;
	baxPx[2]  = cos(5*PI_THIRD)*minAccel;
	
	//the delta angle between the axis and the circle segment edge:
	p3x = baxPx[0] + axisPx[0]*c;
    p3y = baxPy[0] + axisPy[0]*c;
	circAngleDelta = ABS(DMOD(atan2(p3y,p3x),TWO_PI));
	
	for(x = -FORCE_RANGE; x <= FORCE_RANGE; ++x) { 
		//ttime = cn_getRTC();
		//printf("I: %d T: %lu\n",x,(ttime-now));
		//ledToggle();
		//now = cn_getRTC();
		
		for(y = -FORCE_RANGE; y <= FORCE_RANGE; ++y) {
		
//				x =i - FORCE_RANGE;
//				y =j - FORCE_RANGE;
				dist = sqrt(x*x+y*y); //the length of the force vector
				angle = atan2(y,x);   //its angle
				if (angle < 0) angle += TWO_PI;
				
							
				if(angle > FOUR_PI_THIRD) {
					axisI = 2;
					axisAngle = FOUR_PI_THIRD;
				}
				else if (angle > TWO_PI_THIRD) {
					 axisI=1;
					 axisAngle = TWO_PI_THIRD;
				}
				else {
					axisI = 0;
					axisAngle = 0;
				}
				
				//angle2 = DMOD(angle,TWO_PI_THIRD);
				angle2 = angle - axisAngle;
				
//				if (angle > 2*TWO_PI_THIRD) ftemp = 2*TWO_PI_THIRD;
//    			else if (angle > TWO_PI_THIRD) ftemp = TWO_PI_THIRD;
//    			else ftemp =0; 
				
				//Calculate the relevant line segment:
				
				if (angle2 < PI_THIRD) {
					//axisAngle = ftemp;
					p1y = axisPy[axisI];//sin(ftemp)*maxAccel;
					p1x = axisPx[axisI];//cos(ftemp)*maxAccel;
					p2y = baxPy[axisI];//sin(ftemp+PI_THIRD)*minAccel;
					p2x = baxPx[axisI];//cos(ftemp+PI_THIRD)*minAccel;
				}				
				else {
					//axisAngle = ftemp+TWO_PI_THIRD;
					axisAngle +=TWO_PI_THIRD;
					p2y = baxPy[axisI];//sin(ftemp+PI_THIRD)*minAccel;
					p2x = baxPx[axisI];//cos(ftemp+PI_THIRD)*minAccel;
					axisI = (axisI+1 ) % 3;
					p1y = axisPy[axisI];//sin(ftemp+TWO_PI_THIRD)*maxAccel;
					p1x = axisPx[axisI];//cos(ftemp+TWO_PI_THIRD)*maxAccel;
				}
				//Linear Combination:
				b = (y-x/p1x*p1y) / (p2y-p2x/p1x*p1y);
    			a = (x-b*p2x) / p1x;
    		
    		
    			if (dist <= maxAccel && b <= 1.0) { //vector is within 'green area'
					lookup[curIdx].x = 0;
					lookup[curIdx].y = 0;
					curIdx++;
	    			continue;
    			}
				//simplified table:
				
				a = CLAMP(a,0.0,1.0);
				b = CLAMP(b,0.0,1.0);
				abx = a * p1x + b * p2x;
				aby = a * p1y + b * p2y;
				ftemp = sqrt(abx*abx+aby*aby);
				if (ftemp > maxAccel) {
					ftemp = maxAccel/ftemp;
					abx *= ftemp;
					aby *= ftemp;
				}
				lookup[curIdx].x = (sword)ROUND((float)x-abx);
				lookup[curIdx].y = (sword)ROUND((float)y-aby);
				curIdx++;
				//simplified table end
				/*
				//the point at the edge of the relevant circle segment:
				p3x = p2x + p1x*c;
    			p3y = p2y + p1y*c;				
				
				abx = p3x-p2x;
    			aby = p3y-p2y;
				//the vector distance  along the line parallel to the robots axis
    			ftemp = ( (x - p2x)*(p3x - p2x) + (y - p2y)*(p3y - p2y) )/ (abx*abx + aby*aby);				
				ftemp = CLAMP(ftemp,0.0,1.0);

				
				
				
				
				if (dist >= maxAccel && circAngleDelta >= ABS(angle-axisAngle)) {
				//vector is mapped onto circle segment
					lookup[curIdx].x = (sword)ROUND((float)x-(((float)x)*maxAccel/dist));
					lookup[curIdx].y = (sword)ROUND((float)y-(((float)y)*maxAccel/dist));
					curIdx++;
					continue;
        		}
				lookup[curIdx].x = (sword)ROUND((float)x-(p2x+ftemp*abx));  		
    			lookup[curIdx].y = (sword)ROUND((float)y-(p2y+ftemp*aby));
				curIdx++;
				*/
		}
	}
//	greenLedOn();
#ifdef VMC
redLedOn();
#endif
}