Rev 1179 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 1179 | Rev 1180 | ||
|---|---|---|---|
| Line -... | Line 1... | ||
| - | 1 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
|
| - | 2 | // + Copyright (c) 04.2007 Holger Buss |
|
| - | 3 | // + Nur für den privaten Gebrauch |
|
| - | 4 | // + www.MikroKopter.com |
|
| - | 5 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
|
| - | 6 | // + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation), |
|
| - | 7 | // + dass eine Nutzung (auch auszugsweise) nur für den privaten (nicht-kommerziellen) Gebrauch zulässig ist. |
|
| - | 8 | // + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt |
|
| - | 9 | // + bzgl. der Nutzungsbedingungen aufzunehmen. |
|
| - | 10 | // + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen, |
|
| - | 11 | // + Verkauf von Luftbildaufnahmen, usw. |
|
| - | 12 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
|
| - | 13 | // + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht, |
|
| - | 14 | // + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen |
|
| - | 15 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
|
| - | 16 | // + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts |
|
| - | 17 | // + auf anderen Webseiten oder sonstigen Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de" |
|
| - | 18 | // + eindeutig als Ursprung verlinkt werden |
|
| - | 19 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
|
| - | 20 | // + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion |
|
| - | 21 | // + Benutzung auf eigene Gefahr |
|
| - | 22 | // + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden |
|
| - | 23 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
|
| - | 24 | // + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur |
|
| - | 25 | // + mit unserer Zustimmung zulässig |
|
| - | 26 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
|
| - | 27 | // + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen |
|
| - | 28 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
|
| - | 29 | // + Redistributions of source code (with or without modifications) must retain the above copyright notice, |
|
| - | 30 | // + this list of conditions and the following disclaimer. |
|
| - | 31 | // + * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived |
|
| 1 | // ######################## SPI - FlightCtrl ################### |
32 | // + from this software without specific prior written permission. |
| - | 33 | // + * The use of this project (hardware, software, binary files, sources and documentation) is only permittet |
|
| - | 34 | // + for non-commercial use (directly or indirectly) |
|
| - | 35 | // + Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted |
|
| - | 36 | // + with our written permission |
|
| - | 37 | // + * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be |
|
| - | 38 | // + clearly linked as origin |
|
| - | 39 | // + * porting to systems other than hardware from www.mikrokopter.de is not allowed |
|
| - | 40 | // + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" |
|
| - | 41 | // + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE |
|
| - | 42 | // + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE |
|
| - | 43 | // + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE |
|
| - | 44 | // + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR |
|
| - | 45 | // + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF |
|
| - | 46 | // + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS |
|
| - | 47 | // + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN// + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) |
|
| - | 48 | // + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
|
| - | 49 | // + POSSIBILITY OF SUCH DAMAGE. |
|
| - | 50 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
|
| 2 | #include <avr/io.h> |
51 | #include <avr/io.h> |
| 3 | #include <avr/interrupt.h> |
52 | #include <avr/interrupt.h> |
| 4 | #include <string.h> |
53 | #include <string.h> |
| 5 | #include <stdlib.h> |
54 | #include <stdlib.h> |
| 6 | #include "_Settings.h" |
55 | #include "main.h" |
| 7 | #include "spi.h" |
56 | #include "spi.h" |
| 8 | #include "fc.h" |
57 | #include "fc.h" |
| 9 | #include "rc.h" |
58 | #include "rc.h" |
| 10 | #include "eeprom.h" |
59 | #include "eeprom.h" |
| 11 | #include "uart.h" |
60 | #include "uart0.h" |
| 12 | #include "timer0.h" |
61 | #include "timer0.h" |
| 13 | #include "analog.h" |
62 | #include "analog.h" |
| Line 14... | Line 63... | ||
| 14 | 63 | ||
| 15 | #define SPI_TXSYNCBYTE1 0xAA |
64 | #define SPI_TXSYNCBYTE1 0xAA |
| Line 87... | Line 136... | ||
| 87 | { |
136 | { |
| 88 | int16_t tmp; |
137 | int16_t tmp; |
| 89 | cli(); // stop all interrupts to avoid writing of new data during update of that packet. |
138 | cli(); // stop all interrupts to avoid writing of new data during update of that packet. |
| Line 90... | Line 139... | ||
| 90 | 139 | ||
| 91 | // update content of packet to NaviCtrl |
140 | // update content of packet to NaviCtrl |
| 92 | ToNaviCtrl.IntegralNick = (int16_t) (IntegralNick / 130L); // convert to multiple of 0.1° |
141 | ToNaviCtrl.IntegralNick = (int16_t)((10 * IntegralGyroNick) / GYRO_DEG_FACTOR); // convert to multiple of 0.1° |
| 93 | ToNaviCtrl.IntegralRoll = (int16_t) (IntegralRoll / 130L); // convert to multiple of 0.1° |
142 | ToNaviCtrl.IntegralRoll = (int16_t)((10 * IntegralGyroRoll) / GYRO_DEG_FACTOR); // convert to multiple of 0.1° |
| 94 | ToNaviCtrl.GyroHeading = YawGyroHeading / YAW_GYRO_DEG_FACTOR; |
143 | ToNaviCtrl.GyroHeading = (int16_t)((10 * YawGyroHeading) / GYRO_DEG_FACTOR); // convert to multiple of 0.1° |
| 95 | ToNaviCtrl.GyroNick = Reading_GyroNick; |
144 | ToNaviCtrl.GyroNick = GyroNick; |
| 96 | ToNaviCtrl.GyroRoll = Reading_GyroRoll; |
145 | ToNaviCtrl.GyroRoll = GyroRoll; |
| 97 | ToNaviCtrl.GyroYaw = Reading_GyroYaw; |
146 | ToNaviCtrl.GyroYaw = GyroYaw; |
| 98 | ToNaviCtrl.AccNick = (int16_t) ACC_AMPLIFY * (NaviAccNick / NaviCntAcc); |
147 | ToNaviCtrl.AccNick = ((int16_t) 10 * ACC_AMPLIFY * (NaviAccNick / NaviCntAcc)) / ACC_DEG_FACTOR; // convert to multiple of 0.1° |
| 99 | ToNaviCtrl.AccRoll = (int16_t) ACC_AMPLIFY * (NaviAccRoll / NaviCntAcc); |
148 | ToNaviCtrl.AccRoll = ((int16_t) 10 * ACC_AMPLIFY * (NaviAccRoll / NaviCntAcc)) / ACC_DEG_FACTOR; // convert to multiple of 0.1° |
| Line 100... | Line 149... | ||
| 100 | NaviCntAcc = 0; NaviAccNick = 0; NaviAccRoll = 0; |
149 | NaviCntAcc = 0; NaviAccNick = 0; NaviAccRoll = 0; |
| 101 | 150 | ||
| 102 | switch(ToNaviCtrl.Command) |
151 | switch(ToNaviCtrl.Command) |
| Line 153... | Line 202... | ||
| 153 | ToNaviCtrl.Param.Byte[0] = CompassCalState; |
202 | ToNaviCtrl.Param.Byte[0] = CompassCalState; |
| 154 | if(CompassCalState > 4) |
203 | if(CompassCalState > 4) |
| 155 | { // jump from 5 to 0 |
204 | { // jump from 5 to 0 |
| 156 | CompassCalState = 0; |
205 | CompassCalState = 0; |
| 157 | } |
206 | } |
| - | 207 | ToNaviCtrl.Param.Byte[1] = ParamSet.NaviPHLoginTime; |
|
| - | 208 | ToNaviCtrl.Param.Int[1] = ReadingHeight; // at address of Byte 2 and 3 |
|
| - | 209 | ToNaviCtrl.Param.Byte[4] = ParamSet.NaviGpsPLimit; |
|
| 158 | ToNaviCtrl.Param.Int[1] = ReadingHeight; |
210 | ToNaviCtrl.Param.Byte[5] = ParamSet.NaviGpsILimit; |
| - | 211 | ToNaviCtrl.Param.Byte[6] = ParamSet.NaviGpsDLimit; |
|
| 159 | break; |
212 | break; |
| Line 160... | Line 213... | ||
| 160 | 213 | ||
| 161 | case SPI_CMD_VERSION: |
214 | case SPI_CMD_VERSION: |
| 162 | ToNaviCtrl.Param.Byte[0] = SPI_VersionInfo.Major; |
215 | ToNaviCtrl.Param.Byte[0] = SPI_VersionInfo.Major; |
| 163 | ToNaviCtrl.Param.Byte[1] = SPI_VersionInfo.Minor; |
216 | ToNaviCtrl.Param.Byte[1] = SPI_VersionInfo.Minor; |
| 164 | ToNaviCtrl.Param.Byte[2] = SPI_VersionInfo.Patch; |
217 | ToNaviCtrl.Param.Byte[2] = SPI_VersionInfo.Patch; |
| - | 218 | ToNaviCtrl.Param.Byte[3] = SPI_VersionInfo.Compatible; |
|
| 165 | ToNaviCtrl.Param.Byte[3] = SPI_VersionInfo.Compatible; |
219 | ToNaviCtrl.Param.Byte[4] = BoardRelease; |
| Line 166... | Line 220... | ||
| 166 | break; |
220 | break; |
| 167 | 221 | ||
| 168 | default: |
222 | default: |
| Line 174... | Line 228... | ||
| 174 | 228 | ||
| 175 | // analyze content of packet from NaviCtrl if valid |
229 | // analyze content of packet from NaviCtrl if valid |
| 176 | if (SPI_RxDataValid) |
230 | if (SPI_RxDataValid) |
| 177 | { |
231 | { |
| 178 | // update gps controls |
232 | // update gps controls |
| 179 | if(abs(FromNaviCtrl.GPS_Nick) < 512 && abs(FromNaviCtrl.GPS_Roll) < 512 && (ParamSet.GlobalConfig & CFG_GPS_ACTIVE)) |
233 | if(abs(FromNaviCtrl.GPSStickNick) < 512 && abs(FromNaviCtrl.GPSStickRoll) < 512 && (ParamSet.GlobalConfig & CFG_GPS_ACTIVE)) |
| 180 | { |
234 | { |
| 181 | GPS_Nick = FromNaviCtrl.GPS_Nick; |
235 | GPSStickNick = FromNaviCtrl.GPSStickNick; |
| 182 | GPS_Roll = FromNaviCtrl.GPS_Roll; |
236 | GPSStickRoll = FromNaviCtrl.GPSStickRoll; |
| 183 | } |
237 | } |
| 184 | // update compass readings |
238 | // update compass readings |
| 185 | if(FromNaviCtrl.CompassHeading <= 360) |
239 | if(FromNaviCtrl.CompassHeading <= 360) |
| 186 | { |
240 | { |
| Line 192... | Line 246... | ||
| 192 | if (FromNaviCtrl.BeepTime > BeepTime && !CompassCalState) BeepTime = FromNaviCtrl.BeepTime; |
246 | if (FromNaviCtrl.BeepTime > BeepTime && !CompassCalState) BeepTime = FromNaviCtrl.BeepTime; |
| Line 193... | Line 247... | ||
| 193 | 247 | ||
| 194 | switch (FromNaviCtrl.Command) |
248 | switch (FromNaviCtrl.Command) |
| 195 | { |
249 | { |
| 196 | case SPI_KALMAN: |
250 | case SPI_KALMAN: |
| 197 | FCParam.Kalman_K = FromNaviCtrl.Param.Byte[0]; |
251 | FCParam.KalmanK = FromNaviCtrl.Param.Byte[0]; |
| 198 | FCParam.Kalman_MaxFusion = FromNaviCtrl.Param.Byte[1]; |
252 | FCParam.KalmanMaxFusion = FromNaviCtrl.Param.Byte[1]; |
| 199 | FCParam.Kalman_MaxDrift = FromNaviCtrl.Param.Byte[2]; |
253 | FCParam.KalmanMaxDrift = FromNaviCtrl.Param.Byte[2]; |
| Line 200... | Line 254... | ||
| 200 | break; |
254 | break; |
| 201 | 255 | ||
| 202 | default: |
256 | default: |
| 203 | break; |
257 | break; |
| 204 | } |
258 | } |
| 205 | } |
259 | } |
| 206 | else // no valid data from NaviCtrl |
260 | else // no valid data from NaviCtrl |
| 207 | { |
261 | { |
| 208 | // disable GPS control |
262 | // disable GPS control |
| 209 | GPS_Nick = 0; |
263 | GPSStickNick = 0; |
| 210 | GPS_Roll = 0; |
264 | GPSStickRoll = 0; |