Subversion Repositories FlightCtrl

Rev

Rev 1852 | Rev 1856 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 1852 Rev 1855
Line 121... Line 121...
121 
                   {
 121 
                   {
Line 122... Line 122...
122 
 
 122 
 
123 
                if (rxdata == rxchksum)
 123 
                if (rxdata == rxchksum)
124 
                        {
 124 
                        {
125 
                  unsigned char *ptr = (unsigned char *)&FromNaviCtrl;
 - 
 
126 
 
 125 
                  unsigned char *ptr = (unsigned char *)&FromNaviCtrl;
127 
                          memcpy(ptr, (unsigned char *) SPI_Buffer,  sizeof(SPI_Buffer));
 - 
 
128 
 
 126 
                          memcpy(ptr, (unsigned char *) SPI_Buffer,  sizeof(SPI_Buffer));
- 
 
 127 
                          SPI_RxDataValid = 1;
129 
                          SPI_RxDataValid = 1;
 128 
//                        DebugOut.Analog[26]--;
- 
 
 129 
                        }
- 
 
 130 
                        else
130 
                        }
 131 
                         {
- 
 
 132 
                          SPI_RxDataValid = 0;
- 
 
 133 
//                        DebugOut.Analog[26]++;
- 
 
 134 
                         }
Line 131... Line 135...
131 
                        else SPI_RxDataValid = 0;
 135 
                         
132 
 
 136 
 
133 
                        SPI_RXState  = 0;
 137 
                        SPI_RXState  = 0;
134 
                   }
 138 
                   }
Line 267... Line 271...
267 
                        ToNaviCtrl.Param.Byte[7] = EE_Parameter.ServoRollComp;
 271 
                        ToNaviCtrl.Param.Byte[7] = EE_Parameter.ServoRollComp;
268 
                        ToNaviCtrl.Param.Byte[8] = EE_Parameter.ServoRollMin;
 272 
                        ToNaviCtrl.Param.Byte[8] = EE_Parameter.ServoRollMin;
269 
                        ToNaviCtrl.Param.Byte[9] = EE_Parameter.ServoRollMax;
 273 
                        ToNaviCtrl.Param.Byte[9] = EE_Parameter.ServoRollMax;
270 
                break;
 274 
                break;
271 
        }
 275 
        }
272 
 
 276 
                         
273 
  if(SPI_RxDataValid)
 277 
  if(SPI_RxDataValid)
274 
  {
 278 
  {
275 
        NaviDataOkay = 250;
 279 
        NaviDataOkay = 250;
276 
        CalculateCompassTimer = 1;
 280 
        CalculateCompassTimer = 1;
277 
        if(abs(FromNaviCtrl.GPS_Nick) < 512 && abs(FromNaviCtrl.GPS_Roll) < 512 && (EE_Parameter.GlobalConfig & CFG_GPS_AKTIV))
 281 
        if(abs(FromNaviCtrl.GPS_Nick) < 512 && abs(FromNaviCtrl.GPS_Roll) < 512 && (EE_Parameter.GlobalConfig & CFG_GPS_AKTIV))
Line 279... Line 283...
279 
                GPS_Nick = FromNaviCtrl.GPS_Nick;
 283 
                GPS_Nick = FromNaviCtrl.GPS_Nick;
280 
                GPS_Roll = FromNaviCtrl.GPS_Roll;
 284 
                GPS_Roll = FromNaviCtrl.GPS_Roll;
281 
        }
 285 
        }
Line 282... Line 286...
282 
 
 286 
 
283 
        // update compass readings
 287 
        // update compass readings
284 
        MagVec.x = FromNaviCtrl.MagVecX;
 288 
//      MagVec.x = FromNaviCtrl.MagVecX;
285 
        MagVec.y = FromNaviCtrl.MagVecY;
 289 
//      MagVec.y = FromNaviCtrl.MagVecY;
Line 286... Line 290...
286 
        MagVec.z = FromNaviCtrl.MagVecZ;
 290 
//      MagVec.z = FromNaviCtrl.MagVecZ;
287 
 
 291 
 
Line 288... Line 292...
288 
        if(FromNaviCtrl.CompassValue <= 360)   KompassValue = FromNaviCtrl.CompassValue;
 292 
        if(FromNaviCtrl.CompassValue <= 360)   KompassValue = FromNaviCtrl.CompassValue;
Line 298... Line 302...
298 
                        FromNaviCtrl_Value.Kalman_MaxDrift = FromNaviCtrl.Param.sByte[2];
 302 
                        FromNaviCtrl_Value.Kalman_MaxDrift = FromNaviCtrl.Param.sByte[2];
299 
                        FromNaviCtrl_Value.SerialDataOkay = FromNaviCtrl.Param.Byte[3];
 303 
                        FromNaviCtrl_Value.SerialDataOkay = FromNaviCtrl.Param.Byte[3];
300 
                        FromNaviCtrl_Value.GpsZ = FromNaviCtrl.Param.Byte[4];
 304 
                        FromNaviCtrl_Value.GpsZ = FromNaviCtrl.Param.Byte[4];
301 
                        FromNC_Rotate_C = FromNaviCtrl.Param.Byte[5];
 305 
                        FromNC_Rotate_C = FromNaviCtrl.Param.Byte[5];
302 
                        FromNC_Rotate_S = FromNaviCtrl.Param.Byte[6];
 306 
                        FromNC_Rotate_S = FromNaviCtrl.Param.Byte[6];
303 
                        // = FromNaviCtrl.Param.Byte[7];  noch frei
 - 
 
304 
                    POI_KameraNick = (POI_KameraNick + FromNaviCtrl.Param.sInt[5]) / 2; // Nickwinkel
 - 
 
305 
            if(CareFree && FromNaviCtrl.Param.sInt[4] >= 0)
 307 
            if(CareFree && FromNaviCtrl.Param.sInt[4] >= 0)
306 
                         {
 308 
                         {
307 
                          KompassSollWert = FromNaviCtrl.Param.sInt[4]; // bei Carefree kann NC den Kompass-Sollwinkel vorgeben
 309 
                          KompassSollWert = FromNaviCtrl.Param.sInt[4]; // bei Carefree kann NC den Kompass-Sollwinkel vorgeben
308 
                         }
 310 
                         }
- 
 
 311 
                    if(CareFree)  POI_KameraNick = (POI_KameraNick + FromNaviCtrl.Param.sInt[5]) / 2; // Nickwinkel
- 
 
 312 
                        else  POI_KameraNick = 0;
- 
 
 313 
                       
309 
                        break;
 314 
                        break;
310 
                case SPI_NCCMD_VERSION:
 315 
                case SPI_NCCMD_VERSION:
311 
                        NC_Version.Major = FromNaviCtrl.Param.Byte[0];
 316 
                        NC_Version.Major = FromNaviCtrl.Param.Byte[0];
312 
                        NC_Version.Minor = FromNaviCtrl.Param.Byte[1];
 317 
                        NC_Version.Minor = FromNaviCtrl.Param.Byte[1];
313 
                        NC_Version.Patch = FromNaviCtrl.Param.Byte[2];
 318 
                        NC_Version.Patch = FromNaviCtrl.Param.Byte[2];
314 
                        NC_Version.Compatible = FromNaviCtrl.Param.Byte[3];
 319 
                        NC_Version.Compatible = FromNaviCtrl.Param.Byte[3];
315 
                        NC_Version.Hardware = FromNaviCtrl.Param.Byte[4];
 320 
                        NC_Version.Hardware = FromNaviCtrl.Param.Byte[4];
316 
                        DebugOut.Status[0] |= FromNaviCtrl.Param.Byte[5];
 321 
                        DebugOut.Status[0] |= FromNaviCtrl.Param.Byte[5];
317 
                        NC_ErrorCode = FromNaviCtrl.Param.Byte[6];
 322 
                        NC_ErrorCode = FromNaviCtrl.Param.Byte[6];
318 
                        DebugOut.Status[1] = (DebugOut.Status[1] & (0x01|0x02)) | (FromNaviCtrl.Param.Byte[6] & (0x04 | 0x08));
 323 
                        DebugOut.Status[1] = (DebugOut.Status[1] & (0x01|0x02)) | (FromNaviCtrl.Param.Byte[6] & (0x04 | 0x08));
319 
                        break;
 324 
                        break;          
Line 320... Line 325...
320 
 
 325 
 
321 
                case SPI_NCCMD_GPSINFO:
 326 
                case SPI_NCCMD_GPSINFO:
322 
                        GPSInfo.Flags = FromNaviCtrl.Param.Byte[0];
 327 
                        GPSInfo.Flags = FromNaviCtrl.Param.Byte[0];
323 
                        GPSInfo.NumOfSats = FromNaviCtrl.Param.Byte[1];
 328 
                        GPSInfo.NumOfSats = FromNaviCtrl.Param.Byte[1];
324 
                        GPSInfo.SatFix = FromNaviCtrl.Param.Byte[2];
 329 
                        GPSInfo.SatFix = FromNaviCtrl.Param.Byte[2];
325 
                        GPSInfo.Speed = FromNaviCtrl.Param.Byte[3];
 330 
                        GPSInfo.Speed = FromNaviCtrl.Param.Byte[3];
326 
                        GPSInfo.HomeDistance = FromNaviCtrl.Param.Int[2];
 331 
                        GPSInfo.HomeDistance = FromNaviCtrl.Param.Int[2];
327 
                        GPSInfo.HomeBearing = FromNaviCtrl.Param.sInt[3];
 332 
                        GPSInfo.HomeBearing = FromNaviCtrl.Param.sInt[3];
- 
 
 333 
                        PPM_in[25] = (signed char) FromNaviCtrl.Param.Byte[8]; // WP_EVENT-Channel-Value
- 
 
 334 
            FromNC_AltitudeSpeed = FromNaviCtrl.Param.Byte[9];
- 
 
 335 
                FromNC_AltitudeSetpoint = (long) FromNaviCtrl.Param.sInt[5] * 10;
- 
 
 336 
DebugOut.Analog[16] = FromNC_AltitudeSpeed;
- 
 
 337 
DebugOut.Analog[17] = FromNC_AltitudeSetpoint;
328 
                        PPM_in[25] = (signed char) FromNaviCtrl.Param.Byte[8]; // WP_EVENT-Channel-Value
 338 
DebugOut.Analog[24] = SollHoehe/5;
- 
 
 339 
                        break;
- 
 
 340 
// 0 = 0,1
- 
 
 341 
// 1 = 2,3
- 
 
 342 
// 2 = 4,5
329 
                        break;
 343 
// 3 = 6,7
- 
 
 344 
// 4 = 8,9
330 
 
 345 
// 5 = 10,11
331 
                default:
 346 
                default:
332 
                        break;
 347 
                        break;
333 
          }
 348 
          }
334 
  }
 349 
  }