59,7 → 59,7 |
#include "timer0.h" |
#include "analog.h" |
#include "attitude.h" |
#include "GPScontrol.h" |
#include "GPSControl.h" |
#include "flight.h" |
|
//----------------------------------------- |
126,11 → 126,11 |
#define SPI_RXSYNCBYTE2 0x55 |
|
typedef enum |
{ |
SPI_SYNC1, |
SPI_SYNC2, |
SPI_DATA |
} SPI_RXState_t; |
{ |
SPI_SYNC1, |
SPI_SYNC2, |
SPI_DATA |
} SPI_RXState_t; |
|
|
// data exchange packets to and From NaviCtrl |
200,15 → 200,15 |
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)((10 * pitchAngle) / GYRO_DEG_FACTOR_PITCHROLL); // convert to multiple of 0.1° |
ToNaviCtrl.IntegralRoll = (int16_t)((10 * rollAngle) / GYRO_DEG_FACTOR_PITCHROLL); // convert to multiple of 0.1° |
ToNaviCtrl.IntegralNick = (int16_t)((10 * angle[PITCH]) / GYRO_DEG_FACTOR_PITCHROLL); // convert to multiple of 0.1° |
ToNaviCtrl.IntegralRoll = (int16_t)((10 * angle[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL); // convert to multiple of 0.1° |
ToNaviCtrl.GyroHeading = (int16_t)((10 * yawGyroHeading) / GYRO_DEG_FACTOR_YAW); // convert to multiple of 0.1° |
ToNaviCtrl.GyroNick = pitchRate_PID; // TODO: Which one should it be?? |
ToNaviCtrl.GyroRoll = rollRate_PID; |
ToNaviCtrl.GyroNick = rate_PID[PITCH]; // TODO: Which one should it be?? |
ToNaviCtrl.GyroRoll = rate_PID[ROLL]; |
ToNaviCtrl.GyroYaw = yawRate; |
ToNaviCtrl.AccNick = 0; // ((int16_t) 10 * ACC_AMPLIFY * (NaviAccNick / NaviCntAcc)) / ACC_DEG_FACTOR; // convert to multiple of 0.1° |
ToNaviCtrl.AccRoll = 0; // ((int16_t) 10 * ACC_AMPLIFY * (NaviAccRoll / NaviCntAcc)) / ACC_DEG_FACTOR; // convert to multiple of 0.1° |
naviCntAcc = 0; naviAccPitch = 0; naviAccRoll = 0; |
// naviCntAcc = 0; naviAccPitch = 0; naviAccRoll = 0; |
|
switch(ToNaviCtrl.Command) { |
case SPI_CMD_USER: |
260,7 → 260,7 |
compassCalState = 0; |
} |
ToNaviCtrl.Param.Byte[1] = staticParams.NaviPHLoginTime; |
ToNaviCtrl.Param.Int[1] = readingHeight; // at address of Byte 2 and 3 |
ToNaviCtrl.Param.Int[1] = 0; //readingHeight; // at address of Byte 2 and 3 |
ToNaviCtrl.Param.Byte[4] = staticParams.NaviGpsPLimit; |
ToNaviCtrl.Param.Byte[5] = staticParams.NaviGpsILimit; |
ToNaviCtrl.Param.Byte[6] = staticParams.NaviGpsDLimit; |