From 13f8dd7556fd756722cd669c1f5d5b9311a49777 Mon Sep 17 00:00:00 2001 From: John Hansen Date: Fri, 21 Jan 2011 00:44:37 +0100 Subject: replace many array indexes with pointer access Imported from NXT Enhanced Firmware. --- AT91SAM7S256/Source/c_input.c | 19 ++++++++++--------- 1 file changed, 10 insertions(+), 9 deletions(-) (limited to 'AT91SAM7S256/Source/c_input.c') diff --git a/AT91SAM7S256/Source/c_input.c b/AT91SAM7S256/Source/c_input.c index 47ca7c0..ffd129b 100644 --- a/AT91SAM7S256/Source/c_input.c +++ b/AT91SAM7S256/Source/c_input.c @@ -226,8 +226,11 @@ void cInputCtrl(void) for (Tmp = 0; Tmp < NO_OF_INPUTS; Tmp++) { + UBYTE sType = IOMapInput.Inputs[Tmp].SensorType; + UBYTE *pType = &IOMapInput.Inputs[Tmp].SensorType; + UBYTE oldType = VarsInput.OldSensorType[Tmp]; - if ((IOMapInput.Inputs[Tmp].SensorType) != (VarsInput.OldSensorType[Tmp])) + if (sType != oldType) { /* Clear all variables for this sensor */ @@ -239,9 +242,10 @@ void cInputCtrl(void) memset(&(VarsInput.VarsColor[Tmp]),0 ,sizeof(VarsInput.VarsColor[Tmp])); /* Setup the pins for the new sensortype */ - cInputSetupType(Tmp, &(IOMapInput.Inputs[Tmp].SensorType), VarsInput.OldSensorType[Tmp]); + cInputSetupType(Tmp, pType, oldType); + sType = *pType; IOMapInput.Inputs[Tmp].InvalidData = INVALID_DATA; - VarsInput.OldSensorType[Tmp] = IOMapInput.Inputs[Tmp].SensorType; + VarsInput.OldSensorType[Tmp] = sType; } else { @@ -250,12 +254,9 @@ void cInputCtrl(void) /* A type change has been carried out earlier - waiting for valid data */ /* The color sensor requires special startup sequence with communication */ - if (((IOMapInput.Inputs[Tmp].SensorType) == COLORFULL) || - ((IOMapInput.Inputs[Tmp].SensorType) == COLORRED) || - ((IOMapInput.Inputs[Tmp].SensorType) == COLORGREEN)|| - ((IOMapInput.Inputs[Tmp].SensorType) == COLORBLUE) || - ((IOMapInput.Inputs[Tmp].SensorType) == COLOREXIT) || - ((IOMapInput.Inputs[Tmp].SensorType) == COLORNONE)) + if ((sType == COLORFULL) || (sType == COLORRED) || + (sType == COLORGREEN)|| (sType == COLORBLUE) || + (sType == COLOREXIT) || (sType == COLORNONE)) { cInputCalcSensorValues(Tmp); } -- cgit v1.2.3 From 862733114f99059b8503a0eece085c801d4d76f4 Mon Sep 17 00:00:00 2001 From: John Hansen Date: Fri, 21 Jan 2011 00:44:59 +0100 Subject: factorize code in input module Imported from NXT Enhanced Firmware. --- AT91SAM7S256/Source/c_input.c | 219 +++++++----------------------------------- 1 file changed, 35 insertions(+), 184 deletions(-) (limited to 'AT91SAM7S256/Source/c_input.c') diff --git a/AT91SAM7S256/Source/c_input.c b/AT91SAM7S256/Source/c_input.c index ffd129b..e118291 100644 --- a/AT91SAM7S256/Source/c_input.c +++ b/AT91SAM7S256/Source/c_input.c @@ -288,158 +288,58 @@ void cInputCtrl(void) void cInputCalcSensorValues(UBYTE No) { + UBYTE sType = IOMapInput.Inputs[No].SensorType; - switch(IOMapInput.Inputs[No].SensorType) + switch(sType) { case SWITCH: - { - UWORD InputVal; - - dInputGetRawAd(&InputVal, No); - IOMapInput.Inputs[No].ADRaw = InputVal; - cInputCalcSensorValue( InputVal, - &(IOMapInput.Inputs[No].SensorRaw), - &(IOMapInput.Inputs[No].SensorValue), - &(IOMapInput.Inputs[No].SensorBoolean), - &(VarsInput.InputDebounce[No]), - &(VarsInput.SampleCnt[No]), - &(VarsInput.LastAngle[No]), - &(VarsInput.EdgeCnt[No]), - ((IOMapInput.Inputs[No].SensorMode) & SLOPEMASK), - ((IOMapInput.Inputs[No].SensorMode) & MODEMASK)); - } - break; - case TEMPERATURE: - { - UWORD InputVal; - - dInputGetRawAd(&InputVal, No); - IOMapInput.Inputs[No].ADRaw = InputVal; - if (InputVal < 290) - { - InputVal = 290; - } - else - { - if (InputVal > 928) - { - InputVal = 928; - } - } - InputVal = TempConvTable[(InputVal) - 197]; - InputVal = InputVal + 200; - InputVal = (UWORD)(((SLONG)InputVal * (SLONG)1023)/(SLONG)900); - cInputCalcSensorValue( InputVal, - &(IOMapInput.Inputs[No].SensorRaw), - &(IOMapInput.Inputs[No].SensorValue), - &(IOMapInput.Inputs[No].SensorBoolean), - &(VarsInput.InputDebounce[No]), - &(VarsInput.SampleCnt[No]), - &(VarsInput.LastAngle[No]), - &(VarsInput.EdgeCnt[No]), - ((IOMapInput.Inputs[No].SensorMode) & SLOPEMASK), - ((IOMapInput.Inputs[No].SensorMode) & MODEMASK)); - } - break; - case REFLECTION: - { - UWORD InputVal; - - dInputGetRawAd(&InputVal, No); - IOMapInput.Inputs[No].ADRaw = InputVal; - cInputCalcFullScale(&InputVal, REFLECTIONSENSORMIN, REFLECTIONSENSORPCTDYN, TRUE); - cInputCalcSensorValue( InputVal, - &(IOMapInput.Inputs[No].SensorRaw), - &(IOMapInput.Inputs[No].SensorValue), - &(IOMapInput.Inputs[No].SensorBoolean), - &(VarsInput.InputDebounce[No]), - &(VarsInput.SampleCnt[No]), - &(VarsInput.LastAngle[No]), - &(VarsInput.EdgeCnt[No]), - ((IOMapInput.Inputs[No].SensorMode) & SLOPEMASK), - ((IOMapInput.Inputs[No].SensorMode) & MODEMASK)); - } - break; - case ANGLE: - { - UWORD InputVal; - - dInputGetRawAd(&InputVal, No); - IOMapInput.Inputs[No].ADRaw = InputVal; - cInputCalcSensorValue( InputVal, - &(IOMapInput.Inputs[No].SensorRaw), - &(IOMapInput.Inputs[No].SensorValue), - &(IOMapInput.Inputs[No].SensorBoolean), - &(VarsInput.InputDebounce[No]), - &(VarsInput.SampleCnt[No]), - &(VarsInput.LastAngle[No]), - &(VarsInput.EdgeCnt[No]), - ((IOMapInput.Inputs[No].SensorMode) & SLOPEMASK), - ((IOMapInput.Inputs[No].SensorMode) & MODEMASK)); - } - break; - - /* Dual case intended */ case LIGHT_ACTIVE: case LIGHT_INACTIVE: - { - UWORD InputVal; - - dInputGetRawAd(&InputVal, No); - IOMapInput.Inputs[No].ADRaw = InputVal; - cInputCalcFullScale(&InputVal, NEWLIGHTSENSORMIN, NEWLIGHTSENSORPCTDYN, TRUE); - cInputCalcSensorValue( InputVal, - &(IOMapInput.Inputs[No].SensorRaw), - &(IOMapInput.Inputs[No].SensorValue), - &(IOMapInput.Inputs[No].SensorBoolean), - &(VarsInput.InputDebounce[No]), - &(VarsInput.SampleCnt[No]), - &(VarsInput.LastAngle[No]), - &(VarsInput.EdgeCnt[No]), - ((IOMapInput.Inputs[No].SensorMode) & SLOPEMASK), - ((IOMapInput.Inputs[No].SensorMode) & MODEMASK)); - - } - break; - - /* Dual case intended */ case SOUND_DB: case SOUND_DBA: - { - UWORD InputVal; - - dInputGetRawAd(&InputVal, No); - IOMapInput.Inputs[No].ADRaw = InputVal; - cInputCalcFullScale(&InputVal, NEWSOUNDSENSORMIN, NEWSOUNDSENSORPCTDYN, TRUE); - cInputCalcSensorValue( InputVal, - &(IOMapInput.Inputs[No].SensorRaw), - &(IOMapInput.Inputs[No].SensorValue), - &(IOMapInput.Inputs[No].SensorBoolean), - &(VarsInput.InputDebounce[No]), - &(VarsInput.SampleCnt[No]), - &(VarsInput.LastAngle[No]), - &(VarsInput.EdgeCnt[No]), - ((IOMapInput.Inputs[No].SensorMode) & SLOPEMASK), - ((IOMapInput.Inputs[No].SensorMode) & MODEMASK)); - - } - break; - case CUSTOM: { UWORD InputVal; - /* Setup and read digital IO */ - cInputSetupCustomSensor(No); - dInputRead0(No, &(IOMapInput.Inputs[No].DigiPinsIn)); - dInputRead1(No, &(IOMapInput.Inputs[No].DigiPinsIn)); + if (sType == CUSTOM) { + /* Setup and read digital IO */ + cInputSetupCustomSensor(No); + dInputRead0(No, &(IOMapInput.Inputs[No].DigiPinsIn)); + dInputRead1(No, &(IOMapInput.Inputs[No].DigiPinsIn)); + } dInputGetRawAd(&InputVal, No); IOMapInput.Inputs[No].ADRaw = InputVal; - cInputCalcFullScale(&InputVal, IOMapInput.Inputs[No].CustomZeroOffset, IOMapInput.Inputs[No].CustomPctFullScale, FALSE); + + if (sType == REFLECTION) + { + cInputCalcFullScale(&InputVal, REFLECTIONSENSORMIN, REFLECTIONSENSORPCTDYN, TRUE); + } + else if (sType == TEMPERATURE) + { + if (InputVal < 290) + InputVal = 290; + else if (InputVal > 928) + InputVal = 928; + InputVal = TempConvTable[(InputVal) - 197]; + InputVal = InputVal + 200; + InputVal = (UWORD)(((SLONG)InputVal * (SLONG)1023)/(SLONG)900); + } + else if (sType == LIGHT_ACTIVE || sType == LIGHT_INACTIVE) + { + cInputCalcFullScale(&InputVal, NEWLIGHTSENSORMIN, NEWLIGHTSENSORPCTDYN, TRUE); + } + else if (sType == SOUND_DB || sType == SOUND_DBA) + { + cInputCalcFullScale(&InputVal, NEWSOUNDSENSORMIN, NEWSOUNDSENSORPCTDYN, TRUE); + } + else if (sType == CUSTOM) + { + cInputCalcFullScale(&InputVal, IOMapInput.Inputs[No].CustomZeroOffset, IOMapInput.Inputs[No].CustomPctFullScale, FALSE); + } cInputCalcSensorValue( InputVal, &(IOMapInput.Inputs[No].SensorRaw), &(IOMapInput.Inputs[No].SensorValue), @@ -450,7 +350,6 @@ void cInputCalcSensorValues(UBYTE No) &(VarsInput.EdgeCnt[No]), ((IOMapInput.Inputs[No].SensorMode) & SLOPEMASK), ((IOMapInput.Inputs[No].SensorMode) & MODEMASK)); - } break; @@ -1136,13 +1035,6 @@ void cInputSetupType(UBYTE Port, UBYTE *pType, UBYTE OldType) break; case REFLECTION: - { - dInputSetActive(Port); - dInputClearDigi0(Port); - dInputClearDigi1(Port); - } - break; - case ANGLE: { dInputSetActive(Port); @@ -1216,50 +1108,9 @@ void cInputSetupType(UBYTE Port, UBYTE *pType, UBYTE OldType) break; case COLORFULL: - { - VarsInput.InvalidTimer[Port] = INVALID_RELOAD_COLOR; - dInputSetInactive(Port); - dInputSetDigi0(Port); - dInputSetDirInDigi1(Port); - IOMapInput.Colors[Port].CalibrationState = SENSORCAL; - VarsInput.VarsColor[Port].ColorInitState = 0; - - } - break; - case COLORRED: - { - VarsInput.InvalidTimer[Port] = INVALID_RELOAD_COLOR; - dInputSetInactive(Port); - dInputSetDigi0(Port); - dInputSetDirInDigi1(Port); - IOMapInput.Colors[Port].CalibrationState = SENSORCAL; - VarsInput.VarsColor[Port].ColorInitState = 0; - } - break; - case COLORGREEN: - { - VarsInput.InvalidTimer[Port] = INVALID_RELOAD_COLOR; - dInputSetInactive(Port); - dInputSetDigi0(Port); - dInputSetDirInDigi1(Port); - IOMapInput.Colors[Port].CalibrationState = SENSORCAL; - VarsInput.VarsColor[Port].ColorInitState = 0; - } - break; - case COLORBLUE: - { - VarsInput.InvalidTimer[Port] = INVALID_RELOAD_COLOR; - dInputSetInactive(Port); - dInputSetDigi0(Port); - dInputSetDirInDigi1(Port); - IOMapInput.Colors[Port].CalibrationState = SENSORCAL; - VarsInput.VarsColor[Port].ColorInitState = 0; - } - break; - case COLORNONE: { VarsInput.InvalidTimer[Port] = INVALID_RELOAD_COLOR; -- cgit v1.2.3 From ea4e008921f457d8111ed237f25b710817c6b34c Mon Sep 17 00:00:00 2001 From: John Hansen Date: Fri, 21 Jan 2011 23:38:39 +0100 Subject: remove unused values in temperature conversion table Imported from NXT Enhanced Firmware. --- AT91SAM7S256/Source/c_input.c | 17 +++-------------- 1 file changed, 3 insertions(+), 14 deletions(-) (limited to 'AT91SAM7S256/Source/c_input.c') diff --git a/AT91SAM7S256/Source/c_input.c b/AT91SAM7S256/Source/c_input.c index e118291..667a167 100644 --- a/AT91SAM7S256/Source/c_input.c +++ b/AT91SAM7S256/Source/c_input.c @@ -69,12 +69,7 @@ enum const SWORD TempConvTable[] = { - 1500, 1460, 1430, 1400, 1380, 1360, 1330, 1310, 1290, 1270, 1250, 1230, 1220, 1200, 1190, 1170, - 1160, 1150, 1140, 1130, 1110, 1100, 1090, 1080, 1070, 1060, 1050, 1040, 1030, 1020, 1010, 1000, - 994, 988, 982, 974, 968, 960, 954, 946, 940, 932, 926, 918, 912, 906, 900, 894, - 890, 884, 878, 874, 868, 864, 858, 854, 848, 844, 838, 832, 828, 822, 816, 812, - 808, 802, 798, 794, 790, 786, 782, 780, 776, 772, 768, 764, 762, 758, 754, 750, - 748, 744, 740, 736, 732, 730, 726, 722, 718, 716, 712, 708, 704, 700, 696, 694, + 700, 696, 694, 690, 688, 684, 682, 678, 674, 672, 668, 666, 662, 660, 656, 654, 650, 648, 644, 642, 640, 638, 634, 632, 630, 628, 624, 622, 620, 616, 614, 612, 610, 608, 604, 602, 600, 598, 596, 592, 590, 588, 586, 584, 582, 580, 578, 576, 574, 572, 570, @@ -114,13 +109,7 @@ const SWORD TempConvTable[] = -98, -98, -100, -102, -104, -106, -106, -108, -110, -112, -114, -114, -116, -118, -120, -120, -122, -124, -126, -128, -130, -130, -132, -134, -136, -138, -140, -142, -144, -146, -146, -148, -150, -152, -154, -156, -158, -160, -162, -164, -166, -166, -168, -170, -172, -174, -176, -178, - -180, -182, -184, -186, -188, -190, -192, -194, -196, -196, -198, -200, -202, -204, -206, -208, - -210, -212, -214, -216, -218, -220, -224, -226, -228, -230, -232, -234, -236, -238, -242, -246, - -248, -250, -254, -256, -260, -262, -264, -268, -270, -274, -276, -278, -282, -284, -286, -290, - -292, -296, -298, -300, -306, -308, -312, -316, -320, -324, -326, -330, -334, -338, -342, -344, - -348, -354, -358, -362, -366, -370, -376, -380, -384, -388, -394, -398, -404, -410, -416, -420, - -428, -432, -440, -446, -450, -460, -468, -476, -484, -492, -500, -510, -524, -534, -546, -560, - -572, -588, -600, -630, -656, -684, -720, -770 + -180, -182, -184, -186, -188, -190, -192, -194, -196, -196, -198, -200 }; static IOMAPINPUT IOMapInput; @@ -324,7 +313,7 @@ void cInputCalcSensorValues(UBYTE No) InputVal = 290; else if (InputVal > 928) InputVal = 928; - InputVal = TempConvTable[(InputVal) - 197]; + InputVal = TempConvTable[(InputVal) - 290]; InputVal = InputVal + 200; InputVal = (UWORD)(((SLONG)InputVal * (SLONG)1023)/(SLONG)900); } -- cgit v1.2.3 From 8aa1d4b314f219f8d5b30eced790efae0f79b77d Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sat, 22 Jan 2011 14:35:13 +0100 Subject: replace RCX temperature conversion table with a polynomial --- AT91SAM7S256/Source/c_input.c | 65 ++++++++++++------------------------------- 1 file changed, 18 insertions(+), 47 deletions(-) (limited to 'AT91SAM7S256/Source/c_input.c') diff --git a/AT91SAM7S256/Source/c_input.c b/AT91SAM7S256/Source/c_input.c index 667a167..2cb9be1 100644 --- a/AT91SAM7S256/Source/c_input.c +++ b/AT91SAM7S256/Source/c_input.c @@ -66,52 +66,6 @@ enum CUSTOM_SETUP = 0x80 }; - -const SWORD TempConvTable[] = -{ - 700, 696, 694, - 690, 688, 684, 682, 678, 674, 672, 668, 666, 662, 660, 656, 654, 650, 648, 644, - 642, 640, 638, 634, 632, 630, 628, 624, 622, 620, 616, 614, 612, 610, 608, 604, - 602, 600, 598, 596, 592, 590, 588, 586, 584, 582, 580, 578, 576, 574, 572, 570, - 568, 564, 562, 560, 558, 556, 554, 552, 550, 548, 546, 544, 542, 540, 538, 536, - 534, 532, 530, 528, 526, 524, 522, 520, 518, 516, 514, 512, 510, 508, 508, 506, - 504, 502, 500, 498, 496, 494, 494, 492, 490, 488, 486, 486, 484, 482, 480, 478, - 476, 476, 474, 472, 470, 468, 468, 466, 464, 462, 460, 458, 458, 456, 454, 452, - 450, 448, 448, 446, 444, 442, 442, 440, 438, 436, 436, 434, 432, 432, 430, 428, - 426, 426, 424, 422, 420, 420, 418, 416, 416, 414, 412, 410, 408, 408, 406, 404, - 404, 402, 400, 398, 398, 396, 394, 394, 392, 390, 390, 388, 386, 386, 384, 382, - 382, 380, 378, 378, 376, 374, 374, 372, 370, 370, 368, 366, 366, 364, 362, 362, - 360, 358, 358, 356, 354, 354, 352, 350, 350, 348, 348, 346, 344, 344, 342, 340, - 340, 338, 338, 336, 334, 334, 332, 332, 330, 328, 328, 326, 326, 324, 322, 322, - 320, 320, 318, 316, 316, 314, 314, 312, 310, 310, 308, 308, 306, 304, 304, 302, - 300, 300, 298, 298, 296, 296, 294, 292, 292, 290, 290, 288, 286, 286, 284, 284, - 282, 282, 280, 280, 278, 278, 276, 274, 274, 272, 272, 270, 270, 268, 268, 266, - 264, 264, 262, 262, 260, 260, 258, 258, 256, 254, 254, 252, 252, 250, 250, 248, - 248, 246, 244, 244, 242, 240, 240, 240, 238, 238, 236, 236, 234, 234, 232, 230, - 230, 228, 228, 226, 226, 224, 224, 222, 220, 220, 218, 218, 216, 216, 214, 214, - 212, 212, 210, 210, 208, 208, 206, 204, 204, 202, 202, 200, 200, 198, 198, 196, - 196, 194, 194, 192, 190, 190, 188, 188, 186, 186, 184, 184, 182, 182, 180, 180, - 178, 178, 176, 176, 174, 174, 172, 172, 170, 170, 168, 168, 166, 166, 164, 164, - 162, 162, 160, 160, 158, 156, 156, 154, 154, 152, 152, 150, 150, 148, 148, 146, - 146, 144, 144, 142, 142, 140, 140, 138, 136, 136, 136, 134, 134, 132, 130, 130, - 128, 128, 126, 126, 124, 124, 122, 122, 120, 120, 118, 118, 116, 116, 114, 114, - 112, 110, 110, 108, 108, 106, 106, 104, 104, 102, 102, 100, 100, 98, 98, 96, - 94, 94, 92, 92, 90, 90, 88, 88, 86, 86, 84, 82, 82, 80, 80, 78, - 78, 76, 76, 74, 74, 72, 72, 70, 70, 68, 68, 66, 66, 64, 62, 62, - 60, 60, 58, 56, 56, 54, 54, 52, 52, 50, 50, 48, 48, 46, 46, 44, - 44, 42, 40, 40, 38, 38, 36, 34, 34, 32, 32, 30, 30, 28, 28, 26, - 24, 24, 22, 22, 20, 20, 18, 16, 16, 14, 14, 12, 12, 10, 10, 8, - 6, 6, 4, 2, 2, 0, 0, -2, -4, -4, -6, -6, -8, -10, -10, -12, - -12, -14, -16, - 16, -18, -20, -20, -22, -22, -24, -26, -26, -28, -30, -30, -32, - -34, -34, -36, -36, -38, -40, -40, -42, -42, -44, -46, -46, -48, -50, -50, -52, - -54, -54, -56, -58, -58, -60, -60, -62, -64, -66, -66, -68, -70, -70, -72, -74, - -76, -76, -78, -80, -80, -82, -84, -86, -86, -88, -90, -90, -92, -94, -94, -96, - -98, -98, -100, -102, -104, -106, -106, -108, -110, -112, -114, -114, -116, -118, -120, -120, - -122, -124, -126, -128, -130, -130, -132, -134, -136, -138, -140, -142, -144, -146, -146, -148, - -150, -152, -154, -156, -158, -160, -162, -164, -166, -166, -168, -170, -172, -174, -176, -178, - -180, -182, -184, -186, -188, -190, -192, -194, -196, -196, -198, -200 -}; - static IOMAPINPUT IOMapInput; static VARSINPUT VarsInput; @@ -139,6 +93,7 @@ void cInputSetupCustomSensor(UBYTE Port); void cInputCalcSensorValues(UBYTE No); UBYTE cInputInitColorSensor(UBYTE Port, UBYTE *pInitStatus); void cInputCalibrateColor(COLORSTRUCT *pC, UWORD *pNewVals); +SWORD cInputTempConv(UWORD InputVal); void cInputInit(void* pHeader) { @@ -313,7 +268,7 @@ void cInputCalcSensorValues(UBYTE No) InputVal = 290; else if (InputVal > 928) InputVal = 928; - InputVal = TempConvTable[(InputVal) - 290]; + InputVal = cInputTempConv(InputVal - 290); InputVal = InputVal + 200; InputVal = (UWORD)(((SLONG)InputVal * (SLONG)1023)/(SLONG)900); } @@ -1165,6 +1120,22 @@ void cInputSetupCustomSensor(UBYTE Port) } +SWORD cInputTempConv(UWORD InputVal) +{ + static const long long TempCoeff[] = { -5425ll, 9261399ll, -6686663252ll, + 2573629857807ll, -822478326197838ll, 195856762719738784ll }; + const unsigned int TempCoeffShift = 48; + /* Replace the original table with polynomial. */ + int i; + long long Input = InputVal; + long long Output = TempCoeff[0]; + for (i = 1; i < sizeof TempCoeff / sizeof TempCoeff[0]; i++) + Output = Output * Input + TempCoeff[i]; + /* Round. */ + return Output + (1ll << TempCoeffShift - 1) >> TempCoeffShift; +} + + UBYTE cInputInitColorSensor(UBYTE Port, UBYTE *pInitStatus) { -- cgit v1.2.3