Rev 1077 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 1077 | Rev 1078 | ||
|---|---|---|---|
| Line 24... | Line 24... | ||
| 24 | SPI_DATA |
24 | SPI_DATA |
| 25 | } SPI_RXState_t; |
25 | } SPI_RXState_t; |
| Line 26... | Line 26... | ||
| 26 | 26 | ||
| 27 | 27 | ||
| 28 | // data exchange packets to and From NaviCtrl |
28 | // data exchange packets to and From NaviCtrl |
| - | 29 | ToNaviCtrl_t ToNaviCtrl; |
|
| - | 30 | FromNaviCtrl_t FromNaviCtrl; |
|
| - | 31 | FromNaviCtrl_Value_t FromNaviCtrl_Value; |
|
| Line 29... | Line 32... | ||
| 29 | ToNaviCtrl_t ToNaviCtrl; |
32 | |
| 30 | FromNaviCtrl_t FromNaviCtrl; |
33 | SPI_VersionInfo_t SPI_VersionInfo; |
| 31 | 34 | ||
| 32 | // rx packet buffer |
35 | // rx packet buffer |
| Line 41... | Line 44... | ||
| 41 | uint8_t SPI_TxBufferIndex = 0; |
44 | uint8_t SPI_TxBufferIndex = 0; |
| Line 42... | Line 45... | ||
| 42 | 45 | ||
| 43 | uint8_t SPITransferCompleted, SPI_ChkSum; |
46 | uint8_t SPITransferCompleted, SPI_ChkSum; |
| Line 44... | Line 47... | ||
| 44 | uint8_t SPI_RxDataValid; |
47 | uint8_t SPI_RxDataValid; |
| 45 | 48 | ||
| Line 46... | Line 49... | ||
| 46 | uint8_t SPI_CommandSequence[] = { SPI_CMD_USER, SPI_CMD_STICK, SPI_CMD_PARAMETER1, SPI_CMD_STICK, SPI_CMD_CAL_COMPASS }; |
49 | uint8_t SPI_CommandSequence[] = { SPI_CMD_USER, SPI_CMD_STICK, SPI_CMD_PARAMETER1, SPI_CMD_STICK, SPI_CMD_MISC, SPI_CMD_VERSION }; |
| 47 | uint8_t SPI_CommandCounter = 0; |
50 | uint8_t SPI_CommandCounter = 0; |
| 48 | 51 | ||
| Line 66... | Line 69... | ||
| 66 | ToNaviCtrl.Sync2 = SPI_TXSYNCBYTE2; |
69 | ToNaviCtrl.Sync2 = SPI_TXSYNCBYTE2; |
| Line 67... | Line 70... | ||
| 67 | 70 | ||
| 68 | ToNaviCtrl.Command = SPI_CMD_USER; |
71 | ToNaviCtrl.Command = SPI_CMD_USER; |
| 69 | ToNaviCtrl.IntegralNick = 0; |
72 | ToNaviCtrl.IntegralNick = 0; |
| - | 73 | ToNaviCtrl.IntegralRoll = 0; |
|
| 70 | ToNaviCtrl.IntegralRoll = 0; |
74 | |
| - | 75 | SPI_RxDataValid = 0; |
|
| - | 76 | ||
| - | 77 | SPI_VersionInfo.Major = VERSION_MAJOR; |
|
| - | 78 | SPI_VersionInfo.Minor = VERSION_MINOR; |
|
| - | 79 | SPI_VersionInfo.Patch = VERSION_PATCH; |
|
| 71 | SPI_RxDataValid = 0; |
80 | SPI_VersionInfo.Compatible = NC_SPI_COMPATIBLE; |
| Line 72... | Line 81... | ||
| 72 | } |
81 | } |
| 73 | 82 | ||
| Line 86... | Line 95... | ||
| 86 | ToNaviCtrl.GyroHeading = YawGyroHeading / YAW_GYRO_DEG_FACTOR; |
95 | ToNaviCtrl.GyroHeading = YawGyroHeading / YAW_GYRO_DEG_FACTOR; |
| 87 | ToNaviCtrl.GyroNick = Reading_GyroNick; |
96 | ToNaviCtrl.GyroNick = Reading_GyroNick; |
| 88 | ToNaviCtrl.GyroRoll = Reading_GyroRoll; |
97 | ToNaviCtrl.GyroRoll = Reading_GyroRoll; |
| 89 | ToNaviCtrl.GyroYaw = Reading_GyroYaw; |
98 | ToNaviCtrl.GyroYaw = Reading_GyroYaw; |
| 90 | ToNaviCtrl.AccNick = (int16_t) ACC_AMPLIFY * (NaviAccNick / NaviCntAcc); |
99 | ToNaviCtrl.AccNick = (int16_t) ACC_AMPLIFY * (NaviAccNick / NaviCntAcc); |
| 91 | ToNaviCtrl.AccRoll = (int16_t) ACC_AMPLIFY * (NaviAccRoll / NaviCntAcc); |
100 | ToNaviCtrl.AccRoll = (int16_t) ACC_AMPLIFY * (NaviAccRoll / NaviCntAcc); |
| 92 | NaviCntAcc = 0; NaviAccNick = 0; NaviAccRoll = 0; |
101 | NaviCntAcc = 0; NaviAccNick = 0; NaviAccRoll = 0; |
| Line 93... | Line 102... | ||
| 93 | 102 | ||
| 94 | switch(ToNaviCtrl.Command) |
103 | switch(ToNaviCtrl.Command) |
| 95 | { |
104 | { |
| Line 108... | Line 117... | ||
| 108 | ToNaviCtrl.Param.Byte[10] = ParamSet.LowVoltageWarning; |
117 | ToNaviCtrl.Param.Byte[10] = ParamSet.LowVoltageWarning; |
| 109 | ToNaviCtrl.Param.Byte[11] = GetActiveParamSet(); |
118 | ToNaviCtrl.Param.Byte[11] = GetActiveParamSet(); |
| 110 | break; |
119 | break; |
| Line 111... | Line 120... | ||
| 111 | 120 | ||
| 112 | case SPI_CMD_PARAMETER1: |
121 | case SPI_CMD_PARAMETER1: |
| 113 | ToNaviCtrl.Param.Byte[0] = FCParam.NaviGpsModeControl; // Parameters for the Naviboard |
122 | ToNaviCtrl.Param.Byte[0] = ParamSet.NaviGpsModeControl; // Parameters for the Naviboard |
| 114 | ToNaviCtrl.Param.Byte[1] = FCParam.NaviGpsGain; |
123 | ToNaviCtrl.Param.Byte[1] = ParamSet.NaviGpsGain; |
| 115 | ToNaviCtrl.Param.Byte[2] = FCParam.NaviGpsP; |
124 | ToNaviCtrl.Param.Byte[2] = ParamSet.NaviGpsP; |
| 116 | ToNaviCtrl.Param.Byte[3] = FCParam.NaviGpsI; |
125 | ToNaviCtrl.Param.Byte[3] = ParamSet.NaviGpsI; |
| 117 | ToNaviCtrl.Param.Byte[4] = FCParam.NaviGpsD; |
126 | ToNaviCtrl.Param.Byte[4] = ParamSet.NaviGpsD; |
| 118 | ToNaviCtrl.Param.Byte[5] = FCParam.NaviGpsACC; |
127 | ToNaviCtrl.Param.Byte[5] = ParamSet.NaviGpsACC; |
| 119 | ToNaviCtrl.Param.Byte[6] = ParamSet.NaviGpsMinSat; |
128 | ToNaviCtrl.Param.Byte[6] = ParamSet.NaviGpsMinSat; |
| 120 | ToNaviCtrl.Param.Byte[7] = ParamSet.NaviStickThreshold; |
129 | ToNaviCtrl.Param.Byte[7] = ParamSet.NaviStickThreshold; |
| - | 130 | ToNaviCtrl.Param.Byte[8] = ParamSet.NaviOperatingRadius; |
|
| - | 131 | ToNaviCtrl.Param.Byte[9] = ParamSet.NaviWindCorrection; |
|
| - | 132 | ToNaviCtrl.Param.Byte[10] = ParamSet.NaviSpeedCompensation; |
|
| 121 | ToNaviCtrl.Param.Byte[8] = 15; // MaxRadius |
133 | ToNaviCtrl.Param.Byte[11] = ParamSet.NaviAngleLimitation; |
| Line 122... | Line 134... | ||
| 122 | break; |
134 | break; |
| 123 | 135 | ||
| Line 136... | Line 148... | ||
| 136 | ToNaviCtrl.Param.Byte[6] = (uint8_t) Poti3; |
148 | ToNaviCtrl.Param.Byte[6] = (uint8_t) Poti3; |
| 137 | ToNaviCtrl.Param.Byte[7] = (uint8_t) Poti4; |
149 | ToNaviCtrl.Param.Byte[7] = (uint8_t) Poti4; |
| 138 | ToNaviCtrl.Param.Byte[8] = (uint8_t) RC_Quality; |
150 | ToNaviCtrl.Param.Byte[8] = (uint8_t) RC_Quality; |
| 139 | break; |
151 | break; |
| Line 140... | Line 152... | ||
| 140 | 152 | ||
| 141 | case SPI_CMD_CAL_COMPASS: |
153 | case SPI_CMD_MISC: |
| 142 | if(CompassCalState > 5) |
- | |
| 143 | { |
154 | ToNaviCtrl.Param.Byte[0] = CompassCalState; |
| 144 | CompassCalState = 0; |
155 | if(CompassCalState > 4) |
| 145 | ToNaviCtrl.Param.Byte[0] = 5; |
- | |
| 146 | } |
- | |
| 147 | else |
- | |
| 148 | { |
156 | { // jump from 5 to 0 |
| 149 | ToNaviCtrl.Param.Byte[0] = CompassCalState; |
157 | CompassCalState = 0; |
| - | 158 | } |
|
| - | 159 | ToNaviCtrl.Param.Int[1] = ReadingHeight; |
|
| - | 160 | break; |
|
| - | 161 | ||
| - | 162 | case SPI_CMD_VERSION: |
|
| - | 163 | ToNaviCtrl.Param.Byte[0] = SPI_VersionInfo.Major; |
|
| - | 164 | ToNaviCtrl.Param.Byte[1] = SPI_VersionInfo.Minor; |
|
| - | 165 | ToNaviCtrl.Param.Byte[2] = SPI_VersionInfo.Patch; |
|
| - | 166 | ToNaviCtrl.Param.Byte[3] = SPI_VersionInfo.Compatible; |
|
| - | 167 | break; |
|
| - | 168 | ||
| 150 | } |
169 | default: |
| 151 | break; |
170 | break; |
| Line 152... | Line 171... | ||
| 152 | } |
171 | } |
| Line 173... | Line 192... | ||
| 173 | // NaviCtrl wants to beep? |
192 | // NaviCtrl wants to beep? |
| 174 | if (FromNaviCtrl.BeepTime > BeepTime && !CompassCalState) BeepTime = FromNaviCtrl.BeepTime; |
193 | if (FromNaviCtrl.BeepTime > BeepTime && !CompassCalState) BeepTime = FromNaviCtrl.BeepTime; |
| Line 175... | Line 194... | ||
| 175 | 194 | ||
| 176 | switch (FromNaviCtrl.Command) |
195 | switch (FromNaviCtrl.Command) |
| - | 196 | { |
|
| - | 197 | case SPI_KALMAN: |
|
| - | 198 | FromNaviCtrl_Value.Kalman_K = FromNaviCtrl.Param.Byte[0]; |
|
| - | 199 | FromNaviCtrl_Value.Kalman_MaxFusion = FromNaviCtrl.Param.Byte[1]; |
|
| - | 200 | FromNaviCtrl_Value.Kalman_MaxDrift = FromNaviCtrl.Param.Byte[2]; |
|
| - | 201 | break; |
|
| - | 202 | ||
| 177 | { |
203 | |
| 178 | case SPI_CMD_OSD_DATA: |
204 | case SPI_CMD_OSD_DATA: |
| 179 | // FromFlightCtrl.Param.Byte[0] = OsdBar; |
205 | FromNaviCtrl_Value.OsdBar = FromNaviCtrl.Param.Byte[0]; |
| 180 | // FromFlightCtrl.Param.Int[1] = Distance; |
206 | FromNaviCtrl_Value.Distance = FromNaviCtrl.Param.Int[1]; |
| Line 181... | Line 207... | ||
| 181 | break; |
207 | break; |
| 182 | 208 | ||
| 183 | case SPI_CMD_GPS_POS: |
209 | case SPI_CMD_GPS_POS: |
| 184 | // FromFlightCtrl.Param.Long[0] = GPS_Data.Longitude; |
210 | //FromFlightCtrl.Param.Long[0] = GPS_Data.Longitude; |
| Line 185... | Line 211... | ||
| 185 | // FromFlightCtrl.Param.Long[1] = GPS_Data.Latitude; |
211 | //FromFlightCtrl.Param.Long[1] = GPS_Data.Latitude; |
| 186 | break; |
212 | break; |
| 187 | 213 | ||
| 188 | case SPI_CMD_GPS_TARGET: |
214 | case SPI_CMD_GPS_TARGET: |
| Line 189... | Line 215... | ||
| 189 | // FromFlightCtrl.Param.Long[0] = GPS_Data.TargetLongitude; |
215 | //FromFlightCtrl.Param.Long[0] = GPS_Data.TargetLongitude; |
| 190 | // FromFlightCtrl.Param.Long[1] = GPS_Data.TargetLatitude; |
216 | //FromFlightCtrl.Param.Long[1] = GPS_Data.TargetLatitude; |
| 191 | break; |
217 | break; |
| Line 287... | Line 313... | ||
| 287 | uint8_t *ptr = (uint8_t *)&FromNaviCtrl; |
313 | uint8_t *ptr = (uint8_t *)&FromNaviCtrl; |
| 288 | cli(); |
314 | cli(); |
| 289 | memcpy(ptr, (uint8_t *) SPI_RxBuffer, sizeof(FromNaviCtrl)); |
315 | memcpy(ptr, (uint8_t *) SPI_RxBuffer, sizeof(FromNaviCtrl)); |
| 290 | sei(); |
316 | sei(); |
| 291 | SPI_RxDataValid = 1; |
317 | SPI_RxDataValid = 1; |
| 292 | DebugOut.Analog[18]++; |
318 | //DebugOut.Analog[18]++; |
| 293 | } |
319 | } |
| 294 | else |
320 | else |
| 295 | { // checksum does not match |
321 | { // checksum does not match |
| 296 | DebugOut.Analog[17]++; |
322 | //DebugOut.Analog[17]++; |
| 297 | SPI_RxDataValid = 0; // reset valid flag |
323 | SPI_RxDataValid = 0; // reset valid flag |
| 298 | } |
324 | } |
| 299 | SPI_RXState = SPI_SYNC1; // reset state sync |
325 | SPI_RXState = SPI_SYNC1; // reset state sync |
| 300 | } |
326 | } |
| 301 | else // not all bytes transfered |
327 | else // not all bytes transfered |