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 |
|
| - | 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 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
|
| 1 | #include <inttypes.h> |
51 | #include <inttypes.h> |
| 2 | #include <stdlib.h> |
52 | #include <stdlib.h> |
| 3 | #include "fc.h" |
53 | #include "fc.h" |
| 4 | #include "ubx.h" |
54 | #include "ubx.h" |
| 5 | #include "mymath.h" |
55 | #include "mymath.h" |
| 6 | #include "timer0.h" |
56 | #include "timer0.h" |
| 7 | #include "uart.h" |
57 | #include "uart0.h" |
| 8 | #include "rc.h" |
58 | #include "rc.h" |
| 9 | #include "eeprom.h" |
59 | #include "eeprom.h" |
| Line 10... | Line 60... | ||
| 10 | 60 | ||
| 11 | typedef enum |
61 | typedef enum |
| Line 138... | Line 188... | ||
| 138 | } |
188 | } |
| Line 139... | Line 189... | ||
| 139 | 189 | ||
| 140 | // disable GPS control sticks |
190 | // disable GPS control sticks |
| 141 | void GPS_Neutral(void) |
191 | void GPS_Neutral(void) |
| 142 | { |
192 | { |
| 143 | GPS_Nick = 0; |
193 | GPSStickNick = 0; |
| 144 | GPS_Roll = 0; |
194 | GPSStickRoll = 0; |
| Line 145... | Line 195... | ||
| 145 | } |
195 | } |
| 146 | 196 | ||
| 147 | // calculates the GPS control stick values from the deviation to target position |
197 | // calculates the GPS control stick values from the deviation to target position |
| Line 248... | Line 298... | ||
| 248 | // A positive nick angle will fly to north and a positive roll angle will fly to west. |
298 | // A positive nick angle will fly to north and a positive roll angle will fly to west. |
| 249 | // In case of a positive north deviation/velocity the |
299 | // In case of a positive north deviation/velocity the |
| 250 | // copter should fly to south (negative nick). |
300 | // copter should fly to south (negative nick). |
| 251 | // In case of a positive east position deviation and a positive east velocity the |
301 | // In case of a positive east position deviation and a positive east velocity the |
| 252 | // copter should fly to west (positive roll). |
302 | // copter should fly to west (positive roll). |
| 253 | // The influence of the GPS_Nick and GPS_Roll variable is contrarily to the stick values |
303 | // The influence of the GPSStickNick and GPSStickRoll variable is contrarily to the stick values |
| 254 | // in the fc.c. Therefore a positive north deviation/velocity should result in a positive |
304 | // in the fc.c. Therefore a positive north deviation/velocity should result in a positive |
| 255 | // GPS_Nick and a positive east deviation/velocity should result in a negative GPS_Roll. |
305 | // GPSStickNick and a positive east deviation/velocity should result in a negative GPSStickRoll. |
| Line 256... | Line 306... | ||
| 256 | 306 | ||
| 257 | coscompass = (int32_t)c_cos_8192(YawGyroHeading / YAW_GYRO_DEG_FACTOR); |
307 | coscompass = (int32_t)c_cos_8192(YawGyroHeading / YAW_GyroDegFactor); |
| 258 | sincompass = (int32_t)c_sin_8192(YawGyroHeading / YAW_GYRO_DEG_FACTOR); |
308 | sincompass = (int32_t)c_sin_8192(YawGyroHeading / YAW_GyroDegFactor); |
| 259 | PID_Nick = (coscompass * PID_North + sincompass * PID_East) / 8192; |
309 | PID_Nick = (coscompass * PID_North + sincompass * PID_East) / 8192; |
| Line 260... | Line 310... | ||
| 260 | PID_Roll = (sincompass * PID_North - coscompass * PID_East) / 8192; |
310 | PID_Roll = (sincompass * PID_North - coscompass * PID_East) / 8192; |
| 261 | 311 | ||
| Line 262... | Line 312... | ||
| 262 | 312 | ||
| 263 | // limit resulting GPS control vector |
313 | // limit resulting GPS control vector |
| 264 | GPS_LimitXY(&PID_Nick, &PID_Roll, GPS_STICK_LIMIT); |
314 | GPS_LimitXY(&PID_Nick, &PID_Roll, GPS_STICK_LIMIT); |
| 265 | 315 | ||
| 266 | GPS_Nick = (int16_t)PID_Nick; |
316 | GPSStickNick = (int16_t)PID_Nick; |
| 267 | GPS_Roll = (int16_t)PID_Roll; |
317 | GPSStickRoll = (int16_t)PID_Roll; |
| 268 | } |
318 | } |