WebSVN - FlightCtrl - Diff - Rev 903 and 916 - /branches/V0.69k CRK HexaLotte/spi.c



        // initialize data packet to NaviControl
        ToNaviCtrl.Sync1 = SPI_TXSYNCBYTE1;
        ToNaviCtrl.Sync2 = SPI_TXSYNCBYTE2;

 
        ToNaviCtrl.Command = SPI_CMD_USER;
        ToNaviCtrl.IntegralNick = 0;
        ToNaviCtrl.IntegralRoll = 0;
        SPI_RxDataValid = 0;

{
        int16_t tmp;
        cli(); // stop all interrupts to avoid writing of new data during update of that packet.

 
        // update content of packet to NaviCtrl
        ToNaviCtrl.IntegralNick = (int16_t) (IntegralNick / 108);
        ToNaviCtrl.IntegralRoll  = (int16_t) (IntegralRoll  / 108);
        ToNaviCtrl.GyroHeading = YawGyroHeading / YAW_GYRO_DEG_FACTOR;
        ToNaviCtrl.GyroNick = Reading_GyroNick;
        ToNaviCtrl.GyroRoll = Reading_GyroRoll;
        ToNaviCtrl.GyroYaw = Reading_GyroYaw;
        ToNaviCtrl.AccNick = (int16_t) ACC_AMPLIFY * (NaviAccNick / NaviCntAcc);
        ToNaviCtrl.AccRoll =  (int16_t) ACC_AMPLIFY * (NaviAccRoll / NaviCntAcc);

        NaviCntAcc = 0; NaviAccNick = 0; NaviAccRoll = 0;
 
        switch(ToNaviCtrl.Command)
        {

                        ToNaviCtrl.Param.Byte[6] = FCParam.UserParam7;
                        ToNaviCtrl.Param.Byte[7] = FCParam.UserParam8;
                        break;

 
                case SPI_CMD_STICK:
                        tmp = PPM_in[ParamSet.ChannelAssignment[CH_GAS]];  if(tmp > 127) tmp = 127; else if(tmp < -128) tmp = -128;
                        ToNaviCtrl.Param.Byte[0] = (int8_t) tmp;
                        tmp = PPM_in[ParamSet.ChannelAssignment[CH_YAW]]; if(tmp > 127) tmp = 127; else if(tmp < -128) tmp = -128;
                        ToNaviCtrl.Param.Byte[1] = (int8_t) tmp;
                        tmp = PPM_in[ParamSet.ChannelAssignment[CH_ROLL]]; if(tmp > 127) tmp = 127; else if(tmp < -128) tmp = -128;
                        ToNaviCtrl.Param.Byte[2] = (int8_t) tmp;
                        tmp = PPM_in[ParamSet.ChannelAssignment[CH_NICK]]; if(tmp > 127) tmp = 127; else if(tmp < -128) tmp = -128;
                        ToNaviCtrl.Param.Byte[3] = (int8_t) tmp;
                        ToNaviCtrl.Param.Byte[4] = (uint8_t) Poti1;
                        ToNaviCtrl.Param.Byte[5] = (uint8_t) Poti2;
                        ToNaviCtrl.Param.Byte[6] = (uint8_t) Poti3;
                        ToNaviCtrl.Param.Byte[7] = (uint8_t) Poti4;
                        ToNaviCtrl.Param.Byte[8] = (uint8_t) RC_Quality;
                        ToNaviCtrl.Param.Byte[9] = (uint8_t) MotorsOn;

                        break;
 
                case SPI_CMD_CAL_COMPASS:

 
        // analyze content of packet from NaviCtrl if valid
        if (SPI_RxDataValid)
        {
                // update gps controls
                if(abs(FromNaviCtrl.GPS_Nick) < 512 && abs(FromNaviCtrl.GPS_Roll) < 512 && (ParamSet.GlobalConfig & CFG_GPS_ACTIVE))
                {
                        GPS_Nick = FromNaviCtrl.GPS_Nick;
                        GPS_Roll  = FromNaviCtrl.GPS_Roll;
                }
                // update compass readings
                if(FromNaviCtrl.CompassHeading <= 360)

                }
        }
        else // no valid data from NaviCtrl
        {
                // disable GPS control
                GPS_Nick = 0;
                GPS_Roll = 0;
        }
}

Rev 903	Rev 916
Line 65...	Line 65...
65	// initialize data packet to NaviControl	65	// initialize data packet to NaviControl
66	ToNaviCtrl.Sync1 = SPI_TXSYNCBYTE1;	66	ToNaviCtrl.Sync1 = SPI_TXSYNCBYTE1;
67	ToNaviCtrl.Sync2 = SPI_TXSYNCBYTE2;	67	ToNaviCtrl.Sync2 = SPI_TXSYNCBYTE2;
Line 68...	Line 68...
68		68
69	ToNaviCtrl.Command = SPI_CMD_USER;	69	ToNaviCtrl.Command = SPI_CMD_USER;
70	ToNaviCtrl.IntegralPitch = 0;	70	ToNaviCtrl.IntegralNick = 0;
71	ToNaviCtrl.IntegralRoll = 0;	71	ToNaviCtrl.IntegralRoll = 0;
72	SPI_RxDataValid = 0;	72	SPI_RxDataValid = 0;
Line 80...	Line 80...
80	{	80	{
81	int16_t tmp;	81	int16_t tmp;
82	cli(); // stop all interrupts to avoid writing of new data during update of that packet.	82	cli(); // stop all interrupts to avoid writing of new data during update of that packet.
Line 83...	Line 83...
83		83
84	// update content of packet to NaviCtrl	84	// update content of packet to NaviCtrl
85	ToNaviCtrl.IntegralPitch = (int16_t) (IntegralPitch / 108);	85	ToNaviCtrl.IntegralNick = (int16_t) (IntegralNick / 108);
86	ToNaviCtrl.IntegralRoll = (int16_t) (IntegralRoll / 108);	86	ToNaviCtrl.IntegralRoll = (int16_t) (IntegralRoll / 108);
87	ToNaviCtrl.GyroHeading = YawGyroHeading / YAW_GYRO_DEG_FACTOR;	87	ToNaviCtrl.GyroHeading = YawGyroHeading / YAW_GYRO_DEG_FACTOR;
88	ToNaviCtrl.GyroPitch = Reading_GyroPitch;	88	ToNaviCtrl.GyroNick = Reading_GyroNick;
89	ToNaviCtrl.GyroRoll = Reading_GyroRoll;	89	ToNaviCtrl.GyroRoll = Reading_GyroRoll;
90	ToNaviCtrl.GyroYaw = Reading_GyroYaw;	90	ToNaviCtrl.GyroYaw = Reading_GyroYaw;
91	ToNaviCtrl.AccPitch = (int16_t) ACC_AMPLIFY * (NaviAccPitch / NaviCntAcc);	91	ToNaviCtrl.AccNick = (int16_t) ACC_AMPLIFY * (NaviAccNick / NaviCntAcc);
92	ToNaviCtrl.AccRoll = (int16_t) ACC_AMPLIFY * (NaviAccRoll / NaviCntAcc);	92	ToNaviCtrl.AccRoll = (int16_t) ACC_AMPLIFY * (NaviAccRoll / NaviCntAcc);
Line 93...	Line 93...
93	NaviCntAcc = 0; NaviAccPitch = 0; NaviAccRoll = 0;	93	NaviCntAcc = 0; NaviAccNick = 0; NaviAccRoll = 0;
94		94
95	switch(ToNaviCtrl.Command)	95	switch(ToNaviCtrl.Command)
96	{	96	{
Line 104...	Line 104...
104	ToNaviCtrl.Param.Byte[6] = FCParam.UserParam7;	104	ToNaviCtrl.Param.Byte[6] = FCParam.UserParam7;
105	ToNaviCtrl.Param.Byte[7] = FCParam.UserParam8;	105	ToNaviCtrl.Param.Byte[7] = FCParam.UserParam8;
106	break;	106	break;
Line 107...	Line 107...
107		107
108	case SPI_CMD_STICK:	108	case SPI_CMD_STICK:
109	tmp = PPM_in[ParamSet.ChannelAssignment[CH_THRUST]]; if(tmp > 127) tmp = 127; else if(tmp < -128) tmp = -128;	109	tmp = PPM_in[ParamSet.ChannelAssignment[CH_GAS]]; if(tmp > 127) tmp = 127; else if(tmp < -128) tmp = -128;
110	ToNaviCtrl.Param.Byte[0] = (int8_t) tmp;	110	ToNaviCtrl.Param.Byte[0] = (int8_t) tmp;
111	tmp = PPM_in[ParamSet.ChannelAssignment[CH_YAW]]; if(tmp > 127) tmp = 127; else if(tmp < -128) tmp = -128;	111	tmp = PPM_in[ParamSet.ChannelAssignment[CH_YAW]]; if(tmp > 127) tmp = 127; else if(tmp < -128) tmp = -128;
112	ToNaviCtrl.Param.Byte[1] = (int8_t) tmp;	112	ToNaviCtrl.Param.Byte[1] = (int8_t) tmp;
113	tmp = PPM_in[ParamSet.ChannelAssignment[CH_ROLL]]; if(tmp > 127) tmp = 127; else if(tmp < -128) tmp = -128;	113	tmp = PPM_in[ParamSet.ChannelAssignment[CH_ROLL]]; if(tmp > 127) tmp = 127; else if(tmp < -128) tmp = -128;
114	ToNaviCtrl.Param.Byte[2] = (int8_t) tmp;	114	ToNaviCtrl.Param.Byte[2] = (int8_t) tmp;
115	tmp = PPM_in[ParamSet.ChannelAssignment[CH_PITCH]]; if(tmp > 127) tmp = 127; else if(tmp < -128) tmp = -128;	115	tmp = PPM_in[ParamSet.ChannelAssignment[CH_NICK]]; if(tmp > 127) tmp = 127; else if(tmp < -128) tmp = -128;
116	ToNaviCtrl.Param.Byte[3] = (int8_t) tmp;	116	ToNaviCtrl.Param.Byte[3] = (int8_t) tmp;
117	ToNaviCtrl.Param.Byte[4] = (uint8_t) Poti1;	117	ToNaviCtrl.Param.Byte[4] = (uint8_t) Poti1;
118	ToNaviCtrl.Param.Byte[5] = (uint8_t) Poti2;	118	ToNaviCtrl.Param.Byte[5] = (uint8_t) Poti2;
119	ToNaviCtrl.Param.Byte[6] = (uint8_t) Poti3;	119	ToNaviCtrl.Param.Byte[6] = (uint8_t) Poti3;
120	ToNaviCtrl.Param.Byte[7] = (uint8_t) Poti4;	120	ToNaviCtrl.Param.Byte[7] = (uint8_t) Poti4;
-		121	ToNaviCtrl.Param.Byte[8] = (uint8_t) RC_Quality;
121	ToNaviCtrl.Param.Byte[8] = (uint8_t) RC_Quality;	122	ToNaviCtrl.Param.Byte[9] = (uint8_t) MotorsOn;
Line 122...	Line 123...
122	break;	123	break;
123		124
124	case SPI_CMD_CAL_COMPASS:	125	case SPI_CMD_CAL_COMPASS:
Line 139...	Line 140...
139		140
140	// analyze content of packet from NaviCtrl if valid	141	// analyze content of packet from NaviCtrl if valid
141	if (SPI_RxDataValid)	142	if (SPI_RxDataValid)
142	{	143	{
143	// update gps controls	144	// update gps controls
144	if(abs(FromNaviCtrl.GPS_Pitch) < 512 && abs(FromNaviCtrl.GPS_Roll) < 512 && (ParamSet.GlobalConfig & CFG_GPS_ACTIVE))	145	if(abs(FromNaviCtrl.GPS_Nick) < 512 && abs(FromNaviCtrl.GPS_Roll) < 512 && (ParamSet.GlobalConfig & CFG_GPS_ACTIVE))
145	{	146	{
146	GPS_Pitch = FromNaviCtrl.GPS_Pitch;	147	GPS_Nick = FromNaviCtrl.GPS_Nick;
147	GPS_Roll = FromNaviCtrl.GPS_Roll;	148	GPS_Roll = FromNaviCtrl.GPS_Roll;
148	}	149	}
149	// update compass readings	150	// update compass readings
150	if(FromNaviCtrl.CompassHeading <= 360)	151	if(FromNaviCtrl.CompassHeading <= 360)
Line 178...	Line 179...
178	}	179	}
179	}	180	}
180	else // no valid data from NaviCtrl	181	else // no valid data from NaviCtrl
181	{	182	{
182	// disable GPS control	183	// disable GPS control
183	GPS_Pitch = 0;	184	GPS_Nick = 0;
184	GPS_Roll = 0;	185	GPS_Roll = 0;
185	}	186	}
186	}	187	}

Subversion Repositories FlightCtrl

(root)/branches/V0.69k CRK HexaLotte/spi.c - Rev 903 → 916