| 67,7 → 67,7 |
|---|
| ToNaviCtrl.Sync2 = SPI_TXSYNCBYTE2; |
| |
| ToNaviCtrl.Command = SPI_CMD_USER; |
| ToNaviCtrl.IntegralPitch = 0; |
| ToNaviCtrl.IntegralNick = 0; |
| ToNaviCtrl.IntegralRoll = 0; |
| SPI_RxDataValid = 0; |
| } |
| 82,15 → 82,15 |
|---|
| cli(); // stop all interrupts to avoid writing of new data during update of that packet. |
| |
| // update content of packet to NaviCtrl |
| ToNaviCtrl.IntegralPitch = (int16_t) (IntegralPitch / 108); |
| ToNaviCtrl.IntegralNick = (int16_t) (IntegralNick / 108); |
| ToNaviCtrl.IntegralRoll = (int16_t) (IntegralRoll / 108); |
| ToNaviCtrl.GyroHeading = YawGyroHeading / YAW_GYRO_DEG_FACTOR; |
| ToNaviCtrl.GyroPitch = Reading_GyroPitch; |
| ToNaviCtrl.GyroNick = Reading_GyroNick; |
| ToNaviCtrl.GyroRoll = Reading_GyroRoll; |
| ToNaviCtrl.GyroYaw = Reading_GyroYaw; |
| ToNaviCtrl.AccPitch = (int16_t) ACC_AMPLIFY * (NaviAccPitch / NaviCntAcc); |
| ToNaviCtrl.AccNick = (int16_t) ACC_AMPLIFY * (NaviAccNick / NaviCntAcc); |
| ToNaviCtrl.AccRoll = (int16_t) ACC_AMPLIFY * (NaviAccRoll / NaviCntAcc); |
| NaviCntAcc = 0; NaviAccPitch = 0; NaviAccRoll = 0; |
| NaviCntAcc = 0; NaviAccNick = 0; NaviAccRoll = 0; |
| |
| switch(ToNaviCtrl.Command) |
| { |
| 106,13 → 106,13 |
|---|
| break; |
| |
| case SPI_CMD_STICK: |
| tmp = PPM_in[ParamSet.ChannelAssignment[CH_THRUST]]; if(tmp > 127) tmp = 127; else if(tmp < -128) tmp = -128; |
| tmp = PPM_in[ParamSet.ChannelAssignment[CH_GAS]]; if(tmp > 127) tmp = 127; else if(tmp < -128) tmp = -128; |
| ToNaviCtrl.Param.Byte[0] = (int8_t) tmp; |
| tmp = PPM_in[ParamSet.ChannelAssignment[CH_YAW]]; if(tmp > 127) tmp = 127; else if(tmp < -128) tmp = -128; |
| ToNaviCtrl.Param.Byte[1] = (int8_t) tmp; |
| tmp = PPM_in[ParamSet.ChannelAssignment[CH_ROLL]]; if(tmp > 127) tmp = 127; else if(tmp < -128) tmp = -128; |
| ToNaviCtrl.Param.Byte[2] = (int8_t) tmp; |
| tmp = PPM_in[ParamSet.ChannelAssignment[CH_PITCH]]; if(tmp > 127) tmp = 127; else if(tmp < -128) tmp = -128; |
| tmp = PPM_in[ParamSet.ChannelAssignment[CH_NICK]]; if(tmp > 127) tmp = 127; else if(tmp < -128) tmp = -128; |
| ToNaviCtrl.Param.Byte[3] = (int8_t) tmp; |
| ToNaviCtrl.Param.Byte[4] = (uint8_t) Poti1; |
| ToNaviCtrl.Param.Byte[5] = (uint8_t) Poti2; |
| 119,6 → 119,7 |
|---|
| ToNaviCtrl.Param.Byte[6] = (uint8_t) Poti3; |
| ToNaviCtrl.Param.Byte[7] = (uint8_t) Poti4; |
| ToNaviCtrl.Param.Byte[8] = (uint8_t) RC_Quality; |
| ToNaviCtrl.Param.Byte[9] = (uint8_t) MotorsOn; |
| break; |
| |
| case SPI_CMD_CAL_COMPASS: |
| 141,9 → 142,9 |
|---|
| if (SPI_RxDataValid) |
| { |
| // update gps controls |
| if(abs(FromNaviCtrl.GPS_Pitch) < 512 && abs(FromNaviCtrl.GPS_Roll) < 512 && (ParamSet.GlobalConfig & CFG_GPS_ACTIVE)) |
| if(abs(FromNaviCtrl.GPS_Nick) < 512 && abs(FromNaviCtrl.GPS_Roll) < 512 && (ParamSet.GlobalConfig & CFG_GPS_ACTIVE)) |
| { |
| GPS_Pitch = FromNaviCtrl.GPS_Pitch; |
| GPS_Nick = FromNaviCtrl.GPS_Nick; |
| GPS_Roll = FromNaviCtrl.GPS_Roll; |
| } |
| // update compass readings |
| 180,7 → 181,7 |
|---|
| else // no valid data from NaviCtrl |
| { |
| // disable GPS control |
| GPS_Pitch = 0; |
| GPS_Nick = 0; |
| GPS_Roll = 0; |
| } |
| } |