Subversion Repositories FlightCtrl

Rev

Rev 1612 | Show entire file | Ignore 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 124... Line 124...
124 
#define SPI_TXSYNCBYTE2 0x83
 124 
#define SPI_TXSYNCBYTE2 0x83
125 
#define SPI_RXSYNCBYTE1 0x81
 125 
#define SPI_RXSYNCBYTE1 0x81
126 
#define SPI_RXSYNCBYTE2 0x55
 126 
#define SPI_RXSYNCBYTE2 0x55
Line 127... Line 127...
127 
 
 127 
 
128 
typedef enum
 128 
typedef enum
129 
{
 129 
  {
130 
        SPI_SYNC1,
 130 
    SPI_SYNC1,
131 
        SPI_SYNC2,
 131 
    SPI_SYNC2,
132 
        SPI_DATA
 132 
    SPI_DATA
Line 133... Line 133...
133 
} SPI_RXState_t;
 133 
  } SPI_RXState_t;
134 
 
 134 
 
135 
 
 135 
 
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;