Rev 1612 | Go to most recent revision | Show entire file | Regard whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 1612 | Rev 1645 | ||
|---|---|---|---|
| Line 57... | Line 57... | ||
| 57 | #include "eeprom.h" |
57 | #include "eeprom.h" |
| 58 | #include "uart0.h" |
58 | #include "uart0.h" |
| 59 | #include "timer0.h" |
59 | #include "timer0.h" |
| 60 | #include "analog.h" |
60 | #include "analog.h" |
| 61 | #include "attitude.h" |
61 | #include "attitude.h" |
| 62 | #include "GPScontrol.h" |
62 | #include "GPSControl.h" |
| 63 | #include "flight.h" |
63 | #include "flight.h" |
| Line 64... | Line 64... | ||
| 64 | 64 | ||
| 65 | //----------------------------------------- |
65 | //----------------------------------------- |
| 66 | #define DDR_SPI DDRB |
66 | #define DDR_SPI DDRB |
| Line 198... | Line 198... | ||
| 198 | uint8_t i; |
198 | uint8_t i; |
| 199 | int16_t tmp; |
199 | int16_t tmp; |
| 200 | cli(); // stop all interrupts to avoid writing of new data during update of that packet. |
200 | cli(); // stop all interrupts to avoid writing of new data during update of that packet. |
| Line 201... | Line 201... | ||
| 201 | 201 | ||
| 202 | // update content of packet to NaviCtrl |
202 | // update content of packet to NaviCtrl |
| 203 | ToNaviCtrl.IntegralNick = (int16_t)((10 * pitchAngle) / GYRO_DEG_FACTOR_PITCHROLL); // convert to multiple of 0.1° |
203 | ToNaviCtrl.IntegralNick = (int16_t)((10 * angle[PITCH]) / GYRO_DEG_FACTOR_PITCHROLL); // convert to multiple of 0.1° |
| 204 | ToNaviCtrl.IntegralRoll = (int16_t)((10 * rollAngle) / GYRO_DEG_FACTOR_PITCHROLL); // convert to multiple of 0.1° |
204 | ToNaviCtrl.IntegralRoll = (int16_t)((10 * angle[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL); // convert to multiple of 0.1° |
| 205 | ToNaviCtrl.GyroHeading = (int16_t)((10 * yawGyroHeading) / GYRO_DEG_FACTOR_YAW); // convert to multiple of 0.1° |
205 | ToNaviCtrl.GyroHeading = (int16_t)((10 * yawGyroHeading) / GYRO_DEG_FACTOR_YAW); // convert to multiple of 0.1° |
| 206 | ToNaviCtrl.GyroNick = pitchRate_PID; // TODO: Which one should it be?? |
206 | ToNaviCtrl.GyroNick = rate_PID[PITCH]; // TODO: Which one should it be?? |
| 207 | ToNaviCtrl.GyroRoll = rollRate_PID; |
207 | ToNaviCtrl.GyroRoll = rate_PID[ROLL]; |
| 208 | ToNaviCtrl.GyroYaw = yawRate; |
208 | ToNaviCtrl.GyroYaw = yawRate; |
| 209 | ToNaviCtrl.AccNick = 0; // ((int16_t) 10 * ACC_AMPLIFY * (NaviAccNick / NaviCntAcc)) / ACC_DEG_FACTOR; // convert to multiple of 0.1° |
209 | ToNaviCtrl.AccNick = 0; // ((int16_t) 10 * ACC_AMPLIFY * (NaviAccNick / NaviCntAcc)) / ACC_DEG_FACTOR; // convert to multiple of 0.1° |
| 210 | ToNaviCtrl.AccRoll = 0; // ((int16_t) 10 * ACC_AMPLIFY * (NaviAccRoll / NaviCntAcc)) / ACC_DEG_FACTOR; // convert to multiple of 0.1° |
210 | ToNaviCtrl.AccRoll = 0; // ((int16_t) 10 * ACC_AMPLIFY * (NaviAccRoll / NaviCntAcc)) / ACC_DEG_FACTOR; // convert to multiple of 0.1° |
| Line 211... | Line 211... | ||
| 211 | naviCntAcc = 0; naviAccPitch = 0; naviAccRoll = 0; |
211 | // naviCntAcc = 0; naviAccPitch = 0; naviAccRoll = 0; |
| 212 | 212 | ||
| 213 | switch(ToNaviCtrl.Command) { |
213 | switch(ToNaviCtrl.Command) { |
| 214 | case SPI_CMD_USER: |
214 | case SPI_CMD_USER: |
| Line 258... | Line 258... | ||
| 258 | if(compassCalState > 4) |
258 | if(compassCalState > 4) |
| 259 | { // jump from 5 to 0 |
259 | { // jump from 5 to 0 |
| 260 | compassCalState = 0; |
260 | compassCalState = 0; |
| 261 | } |
261 | } |
| 262 | ToNaviCtrl.Param.Byte[1] = staticParams.NaviPHLoginTime; |
262 | ToNaviCtrl.Param.Byte[1] = staticParams.NaviPHLoginTime; |
| 263 | ToNaviCtrl.Param.Int[1] = readingHeight; // at address of Byte 2 and 3 |
263 | ToNaviCtrl.Param.Int[1] = 0; //readingHeight; // at address of Byte 2 and 3 |
| 264 | ToNaviCtrl.Param.Byte[4] = staticParams.NaviGpsPLimit; |
264 | ToNaviCtrl.Param.Byte[4] = staticParams.NaviGpsPLimit; |
| 265 | ToNaviCtrl.Param.Byte[5] = staticParams.NaviGpsILimit; |
265 | ToNaviCtrl.Param.Byte[5] = staticParams.NaviGpsILimit; |
| 266 | ToNaviCtrl.Param.Byte[6] = staticParams.NaviGpsDLimit; |
266 | ToNaviCtrl.Param.Byte[6] = staticParams.NaviGpsDLimit; |
| 267 | break; |
267 | break; |