| 1,14 → 1,63 |
|---|
| // ######################## SPI - FlightCtrl ################### |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Copyright (c) 04.2007 Holger Buss |
| // + Nur für den privaten Gebrauch |
| // + www.MikroKopter.com |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation), |
| // + dass eine Nutzung (auch auszugsweise) nur für den privaten (nicht-kommerziellen) Gebrauch zulässig ist. |
| // + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt |
| // + bzgl. der Nutzungsbedingungen aufzunehmen. |
| // + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen, |
| // + Verkauf von Luftbildaufnahmen, usw. |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht, |
| // + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts |
| // + auf anderen Webseiten oder sonstigen Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de" |
| // + eindeutig als Ursprung verlinkt werden |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion |
| // + Benutzung auf eigene Gefahr |
| // + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur |
| // + mit unserer Zustimmung zulässig |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Redistributions of source code (with or without modifications) must retain the above copyright notice, |
| // + this list of conditions and the following disclaimer. |
| // + * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived |
| // + from this software without specific prior written permission. |
| // + * The use of this project (hardware, software, binary files, sources and documentation) is only permittet |
| // + for non-commercial use (directly or indirectly) |
| // + Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted |
| // + with our written permission |
| // + * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be |
| // + clearly linked as origin |
| // + * porting to systems other than hardware from www.mikrokopter.de is not allowed |
| // + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" |
| // + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE |
| // + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE |
| // + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE |
| // + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR |
| // + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF |
| // + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS |
| // + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN// + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) |
| // + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
| // + POSSIBILITY OF SUCH DAMAGE. |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| #include <avr/io.h> |
| #include <avr/interrupt.h> |
| #include <string.h> |
| #include <stdlib.h> |
| #include "_Settings.h" |
| #include "main.h" |
| #include "spi.h" |
| #include "fc.h" |
| #include "rc.h" |
| #include "eeprom.h" |
| #include "uart.h" |
| #include "uart0.h" |
| #include "timer0.h" |
| #include "analog.h" |
| |
| 89,14 → 138,14 |
|---|
| 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 / 130L); // convert to multiple of 0.1° |
| ToNaviCtrl.IntegralRoll = (int16_t) (IntegralRoll / 130L); // convert to multiple of 0.1° |
| 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); |
| ToNaviCtrl.IntegralNick = (int16_t)((10 * IntegralGyroNick) / GYRO_DEG_FACTOR); // convert to multiple of 0.1° |
| ToNaviCtrl.IntegralRoll = (int16_t)((10 * IntegralGyroRoll) / GYRO_DEG_FACTOR); // convert to multiple of 0.1° |
| ToNaviCtrl.GyroHeading = (int16_t)((10 * YawGyroHeading) / GYRO_DEG_FACTOR); // convert to multiple of 0.1° |
| ToNaviCtrl.GyroNick = GyroNick; |
| ToNaviCtrl.GyroRoll = GyroRoll; |
| ToNaviCtrl.GyroYaw = GyroYaw; |
| ToNaviCtrl.AccNick = ((int16_t) 10 * ACC_AMPLIFY * (NaviAccNick / NaviCntAcc)) / ACC_DEG_FACTOR; // convert to multiple of 0.1° |
| ToNaviCtrl.AccRoll = ((int16_t) 10 * ACC_AMPLIFY * (NaviAccRoll / NaviCntAcc)) / ACC_DEG_FACTOR; // convert to multiple of 0.1° |
| NaviCntAcc = 0; NaviAccNick = 0; NaviAccRoll = 0; |
| |
| switch(ToNaviCtrl.Command) |
| 155,7 → 204,11 |
|---|
| { // jump from 5 to 0 |
| CompassCalState = 0; |
| } |
| ToNaviCtrl.Param.Int[1] = ReadingHeight; |
| ToNaviCtrl.Param.Byte[1] = ParamSet.NaviPHLoginTime; |
| ToNaviCtrl.Param.Int[1] = ReadingHeight; // at address of Byte 2 and 3 |
| ToNaviCtrl.Param.Byte[4] = ParamSet.NaviGpsPLimit; |
| ToNaviCtrl.Param.Byte[5] = ParamSet.NaviGpsILimit; |
| ToNaviCtrl.Param.Byte[6] = ParamSet.NaviGpsDLimit; |
| break; |
| |
| case SPI_CMD_VERSION: |
| 163,6 → 216,7 |
|---|
| ToNaviCtrl.Param.Byte[1] = SPI_VersionInfo.Minor; |
| ToNaviCtrl.Param.Byte[2] = SPI_VersionInfo.Patch; |
| ToNaviCtrl.Param.Byte[3] = SPI_VersionInfo.Compatible; |
| ToNaviCtrl.Param.Byte[4] = BoardRelease; |
| break; |
| |
| default: |
| 176,10 → 230,10 |
|---|
| if (SPI_RxDataValid) |
| { |
| // update gps controls |
| if(abs(FromNaviCtrl.GPS_Nick) < 512 && abs(FromNaviCtrl.GPS_Roll) < 512 && (ParamSet.GlobalConfig & CFG_GPS_ACTIVE)) |
| if(abs(FromNaviCtrl.GPSStickNick) < 512 && abs(FromNaviCtrl.GPSStickRoll) < 512 && (ParamSet.GlobalConfig & CFG_GPS_ACTIVE)) |
| { |
| GPS_Nick = FromNaviCtrl.GPS_Nick; |
| GPS_Roll = FromNaviCtrl.GPS_Roll; |
| GPSStickNick = FromNaviCtrl.GPSStickNick; |
| GPSStickRoll = FromNaviCtrl.GPSStickRoll; |
| } |
| // update compass readings |
| if(FromNaviCtrl.CompassHeading <= 360) |
| 194,9 → 248,9 |
|---|
| switch (FromNaviCtrl.Command) |
| { |
| case SPI_KALMAN: |
| FCParam.Kalman_K = FromNaviCtrl.Param.Byte[0]; |
| FCParam.Kalman_MaxFusion = FromNaviCtrl.Param.Byte[1]; |
| FCParam.Kalman_MaxDrift = FromNaviCtrl.Param.Byte[2]; |
| FCParam.KalmanK = FromNaviCtrl.Param.Byte[0]; |
| FCParam.KalmanMaxFusion = FromNaviCtrl.Param.Byte[1]; |
| FCParam.KalmanMaxDrift = FromNaviCtrl.Param.Byte[2]; |
| break; |
| |
| default: |
| 206,8 → 260,8 |
|---|
| else // no valid data from NaviCtrl |
| { |
| // disable GPS control |
| GPS_Nick = 0; |
| GPS_Roll = 0; |
| GPSStickNick = 0; |
| GPSStickRoll = 0; |
| } |
| } |
| |