int main(void)
{
    port_init();
	serialport_init();
	timer_init();
	tempSensor_init();
	//alpha_init();
	
	// activate interrupts
	sei();
	
	uint8_t i,j;						// multi used index variables
	uint16_t k;
	
	uint8_t messageCtr = 0;				// message counter for automatic messages 
	
	CARRIAGE aDataSet;					// a data set used for communication
	
	uint8_t temperature[24];			// the temperatures, actual only 0-7 are in use
	uint8_t spare_temperature[8];		// the spare values for the aida values (actual not in use)
	uint8_t spare_temperatureTimer = 5; // timeout timer for AIDA temperature (use spare values until values are older then 4 seconds)
	
	uint16_t literPerHour = 0;
		
	CHANNELDATA channel_data[NUMBEROFCHANNELS];		// the permanent data of the 4 channels (fan 1-3 and analog)
	CHANNELSTATUS channel_status[NUMBEROFCHANNELS];	// the actual status of the channels
	
	LEDDATA led_data;					// data for the led channels
	uint16_t ledCtr = 0;				// a rotary counter value for led sweep calculation, incremented every 100 ms
	
	ALERTDATA alert_data;				// data for alert
	ALERTSTATUS alert_status;			// actual status
	
	uint8_t time[3] = {0,0,0};			// = {h,m,s}
	uint16_t pcStatus[4] = {0,0,0,0};	// = {CPU_CLK,CPU%,GPU%,RAM%}
	
	uint16_t pulsePerLiter;
	
	
	if (eeprom_read_word(&eeprom_eepromVersionChecker) != EEPROMVERSION_BITS)			// EEPROM is not OK
	{		
		
		// serialport_printf("#EEPROM FAILED, START TO OVERWRITE IT!#\r");
		
		for (j=0;j<NUMBEROFCHANNELS;j++)
		{
			for (i=0;i<sizeof(CHANNELDATA);i++)
			{
				eeprom_write_byte(((uint8_t *)&eeprom_channel_data[j])+i,pgm_read_byte(((uint8_t *)&default_channel_data)+i));
			}			
		}
		
		// serialport_printf("#CHANNELS COMPLETE#\r");
				
		for (i=0;i<8;i++)
		{
			eeprom_write_byte((&eeprom_spare_temperature[i]),pgm_read_byte(&default_spare_temperature[i]));
		}
		
		// serialport_printf("#SPARE TEMPERATURES COMPLETE#\r");
		
		for (i=0;i<sizeof(LEDDATA);i++)
		{
			eeprom_write_byte(((uint8_t *)&eeprom_led_data)+i,pgm_read_byte(((uint8_t *)&default_led_data)+i));
		}
		
		// serialport_printf("#LED COMPLETE#\r");
		
		for (i=0;i<sizeof(ALERTDATA);i++)
		{
			eeprom_write_byte(((uint8_t *)&eeprom_alert_data)+i,pgm_read_byte(((uint8_t *)&default_alert_data)+i));
		}
		
		// serialport_printf("#ALERT COMPLETE#\r");
		
		eeprom_write_word(&eeprom_pulsePerLiter,pgm_read_word(&default_pulsePerLiter));
		
		// serialport_printf("#PULSEPERLITER COMPLETE#\r");
		
		for (k=0;k<sizeof(ALPHANUMERIC);k++)
		{
			eeprom_write_byte(((uint8_t *)&eeprom_alphaDisplay)+k,pgm_read_byte(((uint8_t *)&default_alphaDisplay)+k));
		}
		
		// serialport_printf("\r#DISPLAY COMPLETE#\r");
		
		eeprom_write_word(&eeprom_eepromVersionChecker,EEPROMVERSION_BITS);
		
		// serialport_printf("#EEPROMVERSION COMPLETE#\r");
		// serialport_printf("#EEPROM OVERRIDE COMPLETE, START#\r");
	}
	
	
	pulsePerLiter = eeprom_read_word(&eeprom_pulsePerLiter);	// impulses per liter of the flow meter
	timer_setImpulsePerLiter(pulsePerLiter);							// inform the timer
	
	//ALPHANUMERIC alphaDisplay;
	//uint8_t screenIndex = 0;
	//uint8_t screenCountdown;
	//uint8_t screenOverlayMessageCountdown = 0;
//.........这里部分代码省略.........

/// <summary>
/// Get one line of text from the program memory and put it into the buffer
/// </summary>
void SerialMenu::getLine(uint8_t stringIndex)
{
    strcpy_P(_buffer, (char*)pgm_read_word(&(ConfigMenu_string_table[stringIndex])));
}

////////////////////////////////////////////////////////////////////////////////////////////////////
//Parse HTML message transferred by parameter, buf. It returns 1 when it encounters to the EOF of HTML file, ohterwise 0
uint8_t HTMLParser::Parsing(char * buf)
{
	uint8_t i, retval, index = 0;
	char msgbuf[10];
	char *tmpbuf, *tmpbuf2;


	if(bInParsing == NO_PARSING)
	{

		memset(parserBuf, 0, PARSEBUF_SIZE); 
		parserBufIndex = 0;
	}
		
	while(buf[index] != '\0')
	{
		if(buf[index] == 0x0a)
		{
			if(parserBuf[parserBufIndex - 1] == 0x0d)
			{
				if(parserBufIndex > 1)
				{					
					/////////////////////////
					// Do Parsing a sentence terminated by CR and LF
					Serial.println((char *)parserBuf);

					tmpbuf = strtok((char *)parserBuf, " ");

					Serial.println((char *)tmpbuf);
					
					memset(msgbuf, 0, 10);
				       strcpy_P((char *)msgbuf, (char*)pgm_read_word(&(CMDMsg_table[0]))); // Necessary casts and dereferencing, just copy. 
				//	Serial.println((char *)msgbuf);
					
					if(!strcmp((const char*)tmpbuf, (const char*)msgbuf)) 	// GET
					{
						tmpbuf = strtok(NULL, " ");
						memset(msgbuf, 0, 10);
						Serial.println((char *)tmpbuf);
						
				       	strcpy_P((char *)msgbuf, (char*)pgm_read_word(&(CMDMsg_table[1]))); // Necessary casts and dereferencing, just copy. 
						if(strstr((char const*)tmpbuf, (const char *)msgbuf) != NULL)
						{
							if((tmpbuf2 = strchr((char const*)tmpbuf, '=')) != NULL)
							{
								memset(msgbuf, 0, 10);
				       			strcpy_P((char *)msgbuf, (char*)pgm_read_word(&(CMDMsg_table[2]))); // Necessary casts and dereferencing, just copy. 
								if(strstr((char const*)tmpbuf2, (const char *)msgbuf) != NULL){
									Serial.println((char *)msgbuf);
									SetParam(FW_CMD);
								}
								
								memset(msgbuf, 0, 10);
				       			strcpy_P((char *)msgbuf, (char*)pgm_read_word(&(CMDMsg_table[3]))); // Necessary casts and dereferencing, just copy. 
								if(strstr((char const*)tmpbuf2, (const char *)msgbuf) != NULL){
									SetParam(BW_CMD);
									Serial.println((char *)msgbuf);
								}

								memset(msgbuf, 0, 10);
				       			strcpy_P((char *)msgbuf, (char*)pgm_read_word(&(CMDMsg_table[4]))); // Necessary casts and dereferencing, just copy. 
								if(strstr((char const*)tmpbuf2, (const char *)msgbuf) != NULL){
									Serial.println((char *)msgbuf);
									SetParam(RT_CMD);
								}
								

								memset(msgbuf, 0, 10);
				       			strcpy_P((char *)msgbuf, (char*)pgm_read_word(&(CMDMsg_table[5]))); // Necessary casts and dereferencing, just copy. 
								if(strstr((char const*)tmpbuf2, (const char *)msgbuf) != NULL){
									SetParam(LF_CMD);
									Serial.println((char *)msgbuf);
								}
							}
								
						}

						tmpbuf = strtok(NULL, " ");
						Serial.println((char *)tmpbuf);
					}
					memset(parserBuf, 0, PARSEBUF_SIZE);
					parserBufIndex = 0;
				}else
				{
					bInParsing = NO_PARSING;
					return 1; // Parsing was finished.
				}
			}
			index++;
		}else 
		{			
			parserBuf[parserBufIndex++] = buf[index++];
		}
	}

	if(parserBufIndex > 0)
		bInParsing = IN_PARSING;
	
//.........这里部分代码省略.........

char* monthStr(uint8_t month)
{
    strcpy_P(buffer, (PGM_P)pgm_read_word(&(monthNames_P[month])));
	return buffer;
}

/***************************************************************************** 
 * Accessors - get various data from an object given the index
 * _get_format() 	- return format string for an index
 */
char *_get_format(const index_t i, char *format)
{
	strncpy_P(format, (PGM_P)pgm_read_word(&cfgArray[i].format), CMD_FORMAT_LEN);
	return (format);
}

/*-----------------------------------------------------------------------------
*  Ausgangszustand lesen
*/
BOOL DigOutState(TDigOutNumber number) {

   TFuncState fState = (TFuncState)pgm_read_word(&sDigOutFuncs[number].fState);
   return fState();
}

void __act_setSpeed(__ACTUATOR* act, DRIVE_SPEED speed){
	// clamp it to the given range
	speed = CLAMP(speed, DRIVE_SPEED_MIN, DRIVE_SPEED_MAX);

	// Get the driver class
	const __ACTUATOR_DRIVER_CLASS* driver=act->class;

	// Call the setSpeed method on the class
	if(driver){
		void (*fn)(__ACTUATOR*,DRIVE_SPEED speed) = (void (*)(__ACTUATOR*, DRIVE_SPEED speed))pgm_read_word(&driver->setSpeed);
		if(fn!=null){
			fn(act,(act->inverted) ? speed*-1 : speed);
		}
	}

	// Store the new speed - do this at the end so that the setSpeed method
	// can access the previous speed
	act->required_speed = speed;
}

//--------------------------------------------------------------------
//[LEG INVERSE KINEMATICS] Calculates the angles of the coxa, femur and tibia for the given position of the feet
//IKFeetPosX            - Input position of the Feet X
//IKFeetPosY            - Input position of the Feet Y
//IKFeetPosZ            - Input Position of the Feet Z
//IKSolution            - Output TRUE if the solution is possible
//IKSolutionWarning     - Output TRUE if the solution is NEARLY possible
//IKSolutionError    - Output TRUE if the solution is NOT possible
//mFemurAngles           - Output Angle of Femur in degrees
//mTibiaAngles           - Output Angle of Tibia in degrees
//mCoxaAngles            - Output Angle of Coxa in degrees
//--------------------------------------------------------------------
u8 PhoenixCore::getLegIK(u8 leg, s16 IKFeetPosX, s16 IKFeetPosY, s16 IKFeetPosZ)
{
    u32    IKSW2;            //Length between Shoulder and Wrist, decimals = 2
    u32    IKA14;            //Angle of the line S>W with respect to the ground in radians, decimals = 4
    u32    IKA24;            //Angle of the line S>W with respect to the femur in radians, decimals = 4
    s16            IKFeetPosXZ;    //Diagonal direction from Input X and Z
#if (CONFIG_DOF_PER_LEG == 4)
    // these were shorts...
    long            TarsOffsetXZ;    //Vector value \ ;
    long            TarsOffsetY;     //Vector value / The 2 DOF IK calcs (femur and tibia) are based upon these vectors
    long            TarsToGroundAngle1;    //Angle between tars and ground. Note: the angle are 0 when the tars are perpendicular to the ground
    long            TGA_A_H4;
    long            TGA_B_H3;
#else
    #define TarsOffsetXZ 0	    // Vector value
    #define TarsOffsetY  0	    //Vector value / The 2 DOF IK calcs (femur and tibia) are based upon these vectors
#endif

    long            Temp1;
    long            Temp2;
    long            T3;
    long            hyp2XY;
    u8              ret;
#if (CONFIG_DOF_PER_LEG == 4)
    s16             sin4;
    s16             cos4;
#endif

    //Calculate IKCoxaAngle and IKFeetPosXZ
    s16 atan4 = arctan2 (IKFeetPosX, IKFeetPosZ, &hyp2XY);
    mCoxaAngles[leg] = (((long)atan4*180) / 3141) + (s16)pgm_read_word(&TBL_COXA_ANGLE[leg]);

    //Length between the Coxa and tars [foot]
    IKFeetPosXZ = hyp2XY / DEC_EXP_2;

#if (CONFIG_DOF_PER_LEG == 4)
    // Some legs may have the 4th DOF and some may not, so handle this here...
    //Calc the TarsToGroundAngle1:
    if ((u8)pgm_read_byte(&TBL_TARS_LENGTH[leg])) {    // We allow mix of 3 and 4 DOF legs...
        TarsToGroundAngle1 = -cTarsConst + cTarsMulti*IKFeetPosY + ((long)(IKFeetPosXZ*cTarsFactorA))/DEC_EXP_1 - ((long)(IKFeetPosXZ*IKFeetPosY)/(cTarsFactorB));
        if (IKFeetPosY < 0)     //Always compensate TarsToGroundAngle1 when IKFeetPosY it goes below zero
            TarsToGroundAngle1 = TarsToGroundAngle1 - ((long)(IKFeetPosY*cTarsFactorC)/DEC_EXP_1);     //TGA base, overall rule
        if (TarsToGroundAngle1 > 400)
            TGA_B_H3 = 200 + (TarsToGroundAngle1/2);
        else
            TGA_B_H3 = TarsToGroundAngle1;

        if (TarsToGroundAngle1 > 300)
            TGA_A_H4 = 240 + (TarsToGroundAngle1/5);
        else
            TGA_A_H4 = TarsToGroundAngle1;

        if (IKFeetPosY > 0)    //Only compensate the TarsToGroundAngle1 when it exceed 30 deg (A, H4 PEP note)
            TarsToGroundAngle1 = TGA_A_H4;
        else if (((IKFeetPosY <= 0) & (IKFeetPosY > -10))) // linear transition between case H3 and H4 (from PEP: H4-K5*(H3-H4))
            TarsToGroundAngle1 = (TGA_A_H4 -(((long)IKFeetPosY*(TGA_B_H3-TGA_A_H4))/DEC_EXP_1));
        else                //IKFeetPosY <= -10, Only compensate TGA1 when it exceed 40 deg
            TarsToGroundAngle1 = TGA_B_H3;

        //Calc Tars Offsets:
        sincos(TarsToGroundAngle1, &sin4, &cos4);
        TarsOffsetXZ = ((long)sin4*(u8)pgm_read_byte(&TBL_TARS_LENGTH[leg]))/DEC_EXP_4;
        TarsOffsetY = ((long)cos4*(u8)pgm_read_byte(&TBL_TARS_LENGTH[leg]))/DEC_EXP_4;
    } else {
        TarsOffsetXZ = 0;
        TarsOffsetY = 0;
    }
#endif

    //Using GetAtan2 for solving IKA1 and IKSW
    //IKA14 - Angle between SW line and the ground in radians
    IKA14 = arctan2(IKFeetPosY-TarsOffsetY, IKFeetPosXZ-(u8)pgm_read_byte(&TBL_COXA_LENGTH[leg])-TarsOffsetXZ, &hyp2XY);

    //IKSW2 - Length between femur axis and tars
    IKSW2 = hyp2XY;

    //IKA2 - Angle of the line S>W with respect to the femur in radians
    Temp1 = (( ((long)(u8)pgm_read_byte(&TBL_FEMUR_LENGTH[leg])*(u8)pgm_read_byte(&TBL_FEMUR_LENGTH[leg])) -
                ((long)(u8)pgm_read_byte(&TBL_TIBIA_LENGTH[leg])*(u8)pgm_read_byte(&TBL_TIBIA_LENGTH[leg])) )*DEC_EXP_4 + ((long)IKSW2*IKSW2));
    Temp2 = (long)(2*(u8)pgm_read_byte(&TBL_FEMUR_LENGTH[leg]))*DEC_EXP_2 * (u32)IKSW2;
    T3 = Temp1 / (Temp2/DEC_EXP_4);
    IKA24 = arccos (T3 );

    //IKFemurAngle
    if (mBoolUpsideDown)
        mFemurAngles[leg] = (long)(IKA14 + IKA24) * 180 / 3141 - 900 + OFFSET_FEMUR_HORN(leg);//Inverted, up side down
    else
        mFemurAngles[leg] = -(long)(IKA14 + IKA24) * 180 / 3141 + 900 + OFFSET_FEMUR_HORN(leg);//Normal
//.........这里部分代码省略.........

void enterPassword(int delta_x, int delta_y)
{
    char pw_display_str[PW_SIZE] = "";
    char number[PW_SIZE] = "";

    if (delta_y < MNEG)
    {
        if(pw_index!=PW_SIZE)
        {
            if (pw_display[pw_index]!=0)
                pw_display[pw_index] = pw_display[pw_index] - 1;
            else
                pw_display[pw_index] = 9;
        }
        else
        {
            check_pw = true;
        }
    }
    else if (delta_y > MPOS)
    {
        if(pw_index!=PW_SIZE)
        {
            if (pw_display[pw_index]!=9)
                pw_display[pw_index] = pw_display[pw_index] + 1;
            else
                pw_display[pw_index] = 0;
        }
        else
        {
            check_pw = true;
        }
    }
    else if (delta_x < MNEG)
    {
        if (pw_index!=0)
            pw_index--;
        else
            pw_index = PW_SIZE;
    }
    else if (delta_x > MPOS)
    {
        if (pw_index!=PW_SIZE)
            pw_index++;
        else
            pw_index = 0;
    }

    for(int i = 0; i<PW_SIZE; i++)
    {
        itoa(pw_display[i], number, 10);
        if(i)
            strcat(pw_display_str, number);
        else
            strcpy(pw_display_str, number);
    }

    strcpy(pw_guess, pw_display_str);
    if (check_pw)
    {
        lock_status = password.is(pw_guess);
        if (lock_status)
        {
            miConfig.sibling = &miConfLock;
            for(int i=0;i<PW_SIZE;i++) { pw_display[i] = 0; }
            menu.start();
            return;
        }
        else
        {
            miConfig.sibling = &miConfUnlock;
        }
        check_pw = false;
    }

    if (lock_status)
        strcpy_P(screen.title, strConfLock);
    else
        strcpy_P(screen.title, strConfUnlock);

    strcpy_P(screen.content[0], strNull);
    strcat_P(pw_display_str, space);
    strcat_P(pw_display_str, ok);
    strcpy(screen.content[1], pw_display_str);
    strcpy_P(screen.content[2], (char*)pgm_read_word(&(indicators[pw_index])));
}

u8 PhoenixCore::loop(void)
{
    bool            allDown;
    long            lBodyX;         //Output Position X of feet with Rotation
    long            lBodyY;         //Output Position Y of feet with Rotation
    long            lBodyZ;         //Output Position Z of feet with Rotation
    u8              ret = STATUS_OK;

    //Start time
    mTimerStart = millis();

    if (mCommitTime != 0) {
        if (mTimerStart >= mCommitTime) {
            mServo->commit(mCurServoMoveTime);
            mCommitTime = 0;
            mTimerStart = millis();
        } else {
            return ret;
        }
    }

    // every 500ms
    if (mTimerStart - mTimerLastCheck > 500) {
        mCurVolt = mServo->getBattVolt();
        mTimerLastCheck = mTimerStart;

        if (mCurVolt < CONFIG_VOLT_OFF) {
            mVoltWarnBeepCnt++;
            if (mVoltWarnBeepCnt > 10) {

                return STATUS_BATT_FAIL;
            }
            else
                ret |= STATUS_BATT_WARN;
        } else {
            mVoltWarnBeepCnt = 0;
        }
    }

    if (mBoolUpsideDown) {
        mPtrCtrlState->c3dTravelLen.x = -mPtrCtrlState->c3dTravelLen.x;
        mPtrCtrlState->c3dBodyPos.x = -mPtrCtrlState->c3dBodyPos.x;
        mPtrCtrlState->c3dSingleLeg.x = -mPtrCtrlState->c3dSingleLeg.x;
        mPtrCtrlState->c3dBodyRot.z = -mPtrCtrlState->c3dBodyRot.z;
    }

    //Single leg control
    allDown = ctrlSingleLeg();

    //doGait
    doGaitSeq();

    //Balance calculations
    mTotalTransX = 0;     //reset values used for calculation of balance
    mTotalTransZ = 0;
    mTotalTransY = 0;
    mTotalXBal1  = 0;
    mTotalYBal1  = 0;
    mTotalZBal1  = 0;

    if (mPtrCtrlState->fBalanceMode) {
        for (u8 i = 0; i < CONFIG_NUM_LEGS / 2; i++) {    // balance calculations for all Right legs
            calcBalOneLeg(i, -mLegPosXs[i]+mGaitPosXs[i],
                          mLegPosZs[i]+mGaitPosZs[i],
                          (mLegPosYs[i]-(s16)pgm_read_word(&TBL_INT_POS_Y[i]))+mGaitPosYs[i]);
        }

        for (u8 i = CONFIG_NUM_LEGS / 2; i < CONFIG_NUM_LEGS; i++) {    // balance calculations for all Right legs
            calcBalOneLeg(i, mLegPosXs[i]+mGaitPosXs[i],
                          mLegPosZs[i]+mGaitPosZs[i],
                          (mLegPosYs[i]-(s16)pgm_read_word(&TBL_INT_POS_Y[i]))+mGaitPosYs[i]);
        }
        balanceBody();
    }

    //Do IK for all Right legs
    for (u8 i = 0; i < CONFIG_NUM_LEGS / 2; i++) {
        getBodyIK(i,
                  -mLegPosXs[i]+mPtrCtrlState->c3dBodyPos.x+mGaitPosXs[i] - mTotalTransX,
                  mLegPosZs[i]+mPtrCtrlState->c3dBodyPos.z+mGaitPosZs[i] - mTotalTransZ,
                  mLegPosYs[i]+mPtrCtrlState->c3dBodyPos.y+mGaitPosYs[i] - mTotalTransY,
                  mGaitRotYs[i],
                  &lBodyX, &lBodyY, &lBodyZ);

        ret |= getLegIK(i, mLegPosXs[i]-mPtrCtrlState->c3dBodyPos.x+lBodyX-(mGaitPosXs[i] - mTotalTransX),
                        mLegPosYs[i]+mPtrCtrlState->c3dBodyPos.y-lBodyY+mGaitPosYs[i] - mTotalTransY,
                        mLegPosZs[i]+mPtrCtrlState->c3dBodyPos.z-lBodyZ+mGaitPosZs[i] - mTotalTransZ);
    }

    //Do IK for all Left legs
    for (u8 i = CONFIG_NUM_LEGS / 2; i < CONFIG_NUM_LEGS; i++) {
        getBodyIK(i,
                  mLegPosXs[i]-mPtrCtrlState->c3dBodyPos.x+mGaitPosXs[i] - mTotalTransX,
                  mLegPosZs[i]+mPtrCtrlState->c3dBodyPos.z+mGaitPosZs[i] - mTotalTransZ,
                  mLegPosYs[i]+mPtrCtrlState->c3dBodyPos.y+mGaitPosYs[i] - mTotalTransY,
                  mGaitRotYs[i],
                  &lBodyX, &lBodyY, &lBodyZ);

        ret |= getLegIK(i, mLegPosXs[i]+mPtrCtrlState->c3dBodyPos.x-lBodyX+mGaitPosXs[i] - mTotalTransX,
                       mLegPosYs[i]+mPtrCtrlState->c3dBodyPos.y-lBodyY+mGaitPosYs[i] - mTotalTransY,
//.........这里部分代码省略.........

bool PhoenixCore::ctrlSingleLeg(void)
{
    bool allDown = TRUE;

    for (u8 i = 0; i < CONFIG_NUM_LEGS; i++) {
        if (mLegPosYs[i] != (s16)pgm_read_word(&TBL_INT_POS_Y[i])) {
            allDown = FALSE;
            break;
        }
    }

    if (mPtrCtrlState->bSingleLegCurSel < CONFIG_NUM_LEGS) {
        if (mPtrCtrlState->bSingleLegCurSel != mPtrCtrlState->bSingleLegOldSel) {
            if (allDown) { //Lift leg a bit when it got selected
                mLegPosYs[mPtrCtrlState->bSingleLegCurSel] = (s16)pgm_read_word(&TBL_INT_POS_Y[mPtrCtrlState->bSingleLegCurSel]) - 20;
                //Store current status
                mPtrCtrlState->bSingleLegOldSel = mPtrCtrlState->bSingleLegCurSel;
            } else {//Return prev leg back to the init position
                mLegPosXs[mPtrCtrlState->bSingleLegOldSel] = (s16)pgm_read_word(&TBL_INT_POS_X[mPtrCtrlState->bSingleLegOldSel]);
                mLegPosYs[mPtrCtrlState->bSingleLegOldSel] = (s16)pgm_read_word(&TBL_INT_POS_Y[mPtrCtrlState->bSingleLegOldSel]);
                mLegPosZs[mPtrCtrlState->bSingleLegOldSel] = (s16)pgm_read_word(&TBL_INT_POS_Z[mPtrCtrlState->bSingleLegOldSel]);
            }
        }
        else if (!mPtrCtrlState->fSingleLegHold) {
            //mLegPosYs[mPtrCtrlState->bSingleLegCurSel] = mLegPosYs[mPtrCtrlState->bSingleLegCurSel]+mPtrCtrlState->c3dSingleLeg.y;
            mLegPosYs[mPtrCtrlState->bSingleLegCurSel] = (s16)pgm_read_word(&TBL_INT_POS_Y[mPtrCtrlState->bSingleLegCurSel])+mPtrCtrlState->c3dSingleLeg.y;// Using DIY remote Zenta prefer it this way
            mLegPosXs[mPtrCtrlState->bSingleLegCurSel] = (s16)pgm_read_word(&TBL_INT_POS_X[mPtrCtrlState->bSingleLegCurSel])+mPtrCtrlState->c3dSingleLeg.x;
            mLegPosZs[mPtrCtrlState->bSingleLegCurSel] = (s16)pgm_read_word(&TBL_INT_POS_Z[mPtrCtrlState->bSingleLegCurSel])+mPtrCtrlState->c3dSingleLeg.z;
        }
    } else {//All legs to init position
        if (!allDown) {
            for(u8 i = 0; i < CONFIG_NUM_LEGS; i++) {
                mLegPosXs[i] = (s16)pgm_read_word(&TBL_INT_POS_X[i]);
                mLegPosYs[i] = (s16)pgm_read_word(&TBL_INT_POS_Y[i]);
                mLegPosZs[i] = (s16)pgm_read_word(&TBL_INT_POS_Z[i]);
            }
        }
        if (mPtrCtrlState->bSingleLegOldSel != 255)
            mPtrCtrlState->bSingleLegOldSel = 255;
    }
    return allDown;
}

const char* pin_def_t::get_name_P(void) const
{
  return reinterpret_cast<const char*>(pgm_read_word(&name));
}

bool pin_def_t::is_valid_P(void) const
{
  return pgm_read_word(&name) != 0;
}

void m_ctcss_submenu(char key)
{
	char print = 1;
	uint8_t ctcss_index;
	uint8_t	ctcss_index_other;
	
	m_reset_timer();

	if(m_state == CTCSS_SEL_TX)
	{
		ctcss_index = config.ctcssIndexTx;
		ctcss_index_other = config.ctcssIndexRx;
	}	
	else
	{
		ctcss_index = config.ctcssIndexRx;
		ctcss_index_other = config.ctcssIndexTx;
	}	


	// avoid "same as RX" / "same as TX" for bith settings
	// if it happens (eg invalid config), force current index to "CTCSS off" position
	if(!ctcss_index && (ctcss_index == ctcss_index_other)){
		ctcss_index = 1;
	}

	if (key != -1)
	{
		switch(key)
		{
			case KC_D1:
			{
				ctcss_index = ctcss_index < CTCSS_TABMAX-1 ? ctcss_index+1 : 0;
				// "same as RX" / "same as TX" can only be selected for TX or RX
				// otherwise there will be a paradoxon and the whole universe will cease to exist ;)
				if(!ctcss_index && (ctcss_index == ctcss_index_other)){
					ctcss_index++;
				}
				break;
			}
			case KC_D2:
			{
				ctcss_index = ctcss_index ? ctcss_index-1 : CTCSS_TABMAX-1;
				// "same as RX" / "same as TX" can only be selected for TX or RX
				// otherwise there will be a paradoxon and the whole universe will cease to exist ;)
				if(!ctcss_index && (ctcss_index == ctcss_index_other)){
					ctcss_index=CTCSS_TABMAX-1;
				}
				break;
			}
			case KC_ENTER:
			{
				print = 0;

				if(m_state == CTCSS_SEL_TX)
				{
					uint16_t freq;

					if(ctcss_index){
						freq = pgm_read_word(&ctcss_tab[ctcss_index]);
					}
					else{
						freq = pgm_read_word(&ctcss_tab[ctcss_index_other]);
					}						
						
					if(freq)
					{						
						tone_start_pl(freq);
					}
					else
					{
						tone_stop_pl();
						tone_decoder_stop();
						lcd_cpos(0);
						printf_P(PSTR("TX CTCSS"));
						lcd_fill();
						lcd_cpos(0);
						vTaskDelay(100);
						printf_P(PSTR("OFF"));
						lcd_fill();
						vTaskDelay(100);
					}
				}
				else
				{
					// CTCSS_SEL_RX
					uint16_t freq;

					if(ctcss_index){
						freq = pgm_read_word(&ctcss_tab[ctcss_index]);
					}
					else{
						freq = pgm_read_word(&ctcss_tab[ctcss_index_other]);
					}
									
					if(freq)
					{
						tone_decoder_start_freq(freq);
					}
					else
//.........这里部分代码省略.........

/*-----------------------------------------------------------------------------
*  Ausgang einschalten
*/
void DigOutOn(TDigOutNumber number) {

   TFuncOn fOn = (TFuncOn)pgm_read_word(&sDigOutFuncs[number].fOn);
   fOn();
}

//.........这里部分代码省略.........
    }

    //调试关闭
    //nrf_write_reg(EN_AA,0x00);
    nrf_write_reg(EN_RXADDR,0x02);
    //nrf_write_reg(SETUP_RETR,0x00);
    nrf_write_reg(RX_PW_P1,0x20);

    //set_module_tx();
    nrf_write_reg(NRF_CONFIG,0x0b);
    nrf_write_reg(CONFIG, nrf_read_reg(CONFIG) | _BV(PRIM_RX));
    nrf_write_reg(STATUS, _BV(RX_DR) | _BV(TX_DS) | _BV(MAX_RT) );
    set_nrf24l01_ce_h();
    nrf_write_buf(RX_ADDR_P0,(uint8_t*)&addresses[0],5);
    set_nrf24l01_ce_h();
    if(nrf_read_reg(FEATURE) & _BV(EN_ACK_PAY))
    {
        flush_tx();
    }

    flush_rx();

    print_status(get_status());
    nrf_write_reg(SETUP_AW,0x03);
    print_byte_register("SETUP_AW\t",SETUP_AW,1);
    print_address_register("RX_ADDR_P0-1",RX_ADDR_P0,2);
    print_byte_register("RX_ADDR_P2-5",RX_ADDR_P2,4);

    print_address_register("TX_ADDR\t",TX_ADDR,1);

    print_byte_register("RX_PW_P0-6",RX_PW_P0,6);


    print_byte_register("EN_AA\t",EN_AA,1);
    print_byte_register("EN_RXADDR",EN_RXADDR,1);
    print_byte_register("RF_CH\t",RF_CH,1);
    print_byte_register("RF_SETUP",RF_SETUP,1);

    print_byte_register("CONFIG\t",NRF_CONFIG,1);
    print_byte_register("DYNPD/FEATURE",DYNPD,2);


    UARTprintf("Data Rate\t = %s\r\n", pgm_read_word(&rf24_datarate_e_str_P[getDataRate()]));
    UARTprintf("Model\t\t = %s\r\n",   pgm_read_word(&rf24_model_e_str_P[isPVariant()]));
    UARTprintf("CRC Length\t = %s\r\n",pgm_read_word(&rf24_crclength_e_str_P[getCRCLength()]));
    UARTprintf("PA Power\t = %s\r\n",  pgm_read_word(&rf24_pa_dbm_e_str_P[getPALevel()]));

    Init_Timer_A();

    set_nrf24l01_ce_h();
    //将业务数据写入:WR_TX_PLOAD

    uint8_t fifo_status,status,state,i;
    while(1)
    {
        fifo_status=spi_read_reg(FIFO_STATUS);
        if(fifo_status&0x02)
        {
            status=spi_read_reg(STATUS);
            if(status&_BV(RX_DR))
            {
                state=spi_send_byte(RD_RX_PLOAD);
                for(i=0;i<RX_PLOAD_WIDTH;i++)
                {
                    status=spi_send_byte(0xff);
                    //buf[i]=status;
                }
                nrf_write_reg(FLUSH_RX,0xFF);
                //UARTprintf(".");
                counter++;
            }
            if(status &0x02)
            {
                nrf_write_reg(FLUSH_RX,0xFF);
                //UARTprintf(".");
                counter++;
            }

            nrf_rx_packet(data);
        }
    }

    while(available(0))
    {
        //UARTprintf(".");
        if(nrf_rx_packet(data) == 0)
        {
            counter++;
        }
        
        //UARTprintf(".");
        //_delay_ms(50);
        /*
        set_nrf24l01_ce_l();
        nrf_write_buf(WR_TX_PLOAD,data,TX_PLOAD_WIDTH);
        set_nrf24l01_ce_h();
        _delay_ms(30);
        */
    }
}

/*-----------------------------------------------------------------------------
*  Ausgang ausschalten
*/
void DigOutOff(TDigOutNumber number) {

   TFuncOff fOff = (TFuncOff)pgm_read_word(&sDigOutFuncs[number].fOff);
   fOff();
}

void waves_init()
{
  waves_currentWaveformNumber = 0;
  waves_currentWaveformPointer =
    (uint8_t*)pgm_read_word(&waves_waveformArray[waves_currentWaveformNumber]);
}

/*-----------------------------------------------------------------------------
*  Ausgang wechseln
*/
void DigOutToggle(TDigOutNumber number) {

   TFuncToggle fToggle = (TFuncToggle)pgm_read_word(&sDigOutFuncs[number].fToggle);
   fToggle();
}

void Initialize(void){
	int i;

	cli();

	//Initialize I/O registers
	u16 val;
	u8 *ptr;
	for(u8 j=0;j<(sizeof(io_table)>>1);j++){
		val=pgm_read_word(&io_table[j]);
		ptr=(u8*)(val&0xff);
		*ptr=val>>8;
	}

	if(!isEepromFormatted()) FormatEeprom();

	//InitSoundPort(); //ramp-up sound to avoid click

	#if SOUND_MIXER == MIXER_TYPE_VSYNC
	
		//Initialize the mixer buffer
		//ramp up to avoid initial click
		for(int j=0;j<MIX_BANK_SIZE*2;j++){
			mix_buf[j]=0x80;//(i<128?i:128);
		}

		mix_pos=mix_buf;
		mix_bank=0;
	#endif

	#if MIXER_CHAN4_TYPE == 0
		//initialize LFSR		
		tr4_barrel_lo=1;
		tr4_barrel_hi=1;		
		tr4_params=0b00000001; //15 bits no divider (1)
	#endif

	#if UART == 1
		InitUartRxBuffer();
		InitUartTxBuffer();
	#endif

	#if SNES_MOUSE == 1
		snesMouseEnabled=false;
	#endif

	//silence all sound channels
	for(i=0;i<CHANNELS;i++){
		mixer.channels.all[i].volume=0;
	}

	//set sync parameters. starts at odd field, in pre-eq pulses, line 1, vsync flag cleared
	sync_phase=0;
	sync_flags=0;
	sync_pulse=SYNC_PRE_EQ_PULSES+SYNC_EQ_PULSES+SYNC_POST_EQ_PULSES;

	//set rendering parameters
	render_lines_count=FRAME_LINES;
	first_render_line=FIRST_RENDER_LINE;

	joypad1_status_hi=0;
	joypad2_status_hi=0;
	sound_enabled=1;

	InitializeVideoMode();

	sei();

	DisplayLogo();

}