//! 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
}
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));
}
}
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();
}
//-------------------------------------------
// 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();
}
}
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
}
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)();
}
//-------------------------------------------
// 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)
}
//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
}