| /branches/dongfang_FC_fixedwing/analog.c |
|---|
| 163,12 → 163,12 |
| AD_GYRO_ROLL, |
| AD_GYRO_YAW, |
| AD_AIRPRESSURE, |
| AD_GYRO_PITCH, |
| AD_GYRO_ROLL, |
| AD_GYRO_YAW, |
| AD_AIRPRESSURE, |
| AD_UBAT, |
| AD_GYRO_PITCH, |
| 175,11 → 175,11 |
| AD_GYRO_ROLL, |
| AD_GYRO_YAW, |
| AD_AIRPRESSURE, |
| AD_GYRO_PITCH, |
| AD_GYRO_ROLL, |
| AD_GYRO_YAW |
| AD_GYRO_YAW, |
| AD_AIRPRESSURE |
| }; |
| // Feature removed. Could be reintroduced later - but should work for all gyro types then. |
| 405,12 → 405,12 |
| } |
| void analog_calibrate(void) { |
| #define OFFSET_CYCLES 250 |
| #define OFFSET_CYCLES 120 |
| uint8_t i, axis; |
| int32_t offsets[4] = { 0, 0, 0, 0 }; |
| gyro_calibrate(); |
| // determine gyro bias by averaging (requires that the copter does not rotate around any axis!) |
| // determine gyro bias by averaging (requires that the aircraft does not rotate around any axis!) |
| for (i = 0; i < OFFSET_CYCLES; i++) { |
| delay_ms_with_adc_measurement(2, 1); |
| for (axis = PITCH; axis <= YAW; axis++) { |
| 420,7 → 420,7 |
| } |
| for (axis = PITCH; axis <= YAW; axis++) { |
| gyroOffset.offsets[axis] = (offsets[axis] /*+ OFFSET_CYCLES/2 */) / OFFSET_CYCLES; |
| gyroOffset.offsets[axis] = (offsets[axis] + OFFSET_CYCLES/2) / OFFSET_CYCLES; |
| int16_t min = (512-200) * GYRO_OVERSAMPLING; |
| int16_t max = (512+200) * GYRO_OVERSAMPLING; |
| if(gyroOffset.offsets[axis] < min || gyroOffset.offsets[axis] > max) |
| 427,7 → 427,7 |
| versionInfo.hardwareErrors[0] |= FC_ERROR0_GYRO_PITCH << axis; |
| } |
| airpressureOffset = (offsets[3] /* + OFFSET_CYCLES/2 */) / OFFSET_CYCLES; |
| airpressureOffset = (offsets[3] + OFFSET_CYCLES/2) / OFFSET_CYCLES; |
| int16_t min = 200; |
| int16_t max = 1024-200; |
| if(airpressureOffset < min || airpressureOffset > max) |
| /branches/dongfang_FC_fixedwing/arduino_atmega328/configuration.c |
|---|
| 153,7 → 153,7 |
| /* Default Values for R/C Channels */ |
| /***************************************************/ |
| void channelMap_default(void) { |
| channelMap.trim = 0; |
| channelMap.HWTrim = 0; |
| channelMap.channels[CH_ELEVATOR] = 1; |
| channelMap.channels[CH_AILERONS] = 0; |
| channelMap.channels[CH_THROTTLE] = 2; |
| /branches/dongfang_FC_fixedwing/arduino_atmega328/configuration.h |
|---|
| 111,7 → 111,7 |
| */ |
| typedef struct { |
| uint8_t trim; |
| uint8_t HWTrim; |
| uint8_t variableOffset; |
| uint8_t channels[MAX_CHANNELS]; |
| } ChannelMap_t; |
| /branches/dongfang_FC_fixedwing/commands.c |
|---|
| 26,6 → 26,10 |
| analog_calibrate(); |
| attitude_setNeutral(); |
| controlMixer_setNeutral(); |
| RC_calibrate(); |
| rcTrim_writeToEEProm(); |
| beepNumber(1); |
| } else if (command == COMMAND_CHMOD && !repeated) { |
| configuration_setFlightParameters(argument); |
| /branches/dongfang_FC_fixedwing/configuration.c |
|---|
| 10,6 → 10,7 |
| int16_t variables[VARIABLE_COUNT]; |
| ParamSet_t staticParams; |
| ChannelMap_t channelMap; |
| RCTrim_t rcTrim; |
| IMUConfig_t IMUConfig; |
| volatile DynamicParams_t dynamicParams; |
| 153,7 → 154,7 |
| staticParams.batteryWarningVoltage = 101; // 3.7 each for 3S |
| for (uint8_t i=0; i<3; i++) { |
| staticParams.gyroPID[i].P = 80; |
| staticParams.gyroPID[i].P = 200; |
| staticParams.gyroPID[i].I = 80; |
| staticParams.gyroPID[i].D = 40; |
| staticParams.gyroPID[i].iMax = 30; |
| 204,7 → 205,7 |
| /***************************************************/ |
| void channelMap_default(void) { |
| channelMap.RCPolarity = 1; |
| channelMap.trim = 192; |
| channelMap.HWTrim = 192; |
| channelMap.variableOffset = 128; |
| channelMap.channels[CH_ELEVATOR] = 1; |
| channelMap.channels[CH_AILERONS] = 0; |
| /branches/dongfang_FC_fixedwing/configuration.h |
|---|
| 112,13 → 112,20 |
| typedef struct { |
| uint8_t RCPolarity; // 1=positive, 0=negative. Use positive with Futaba receiver, negative with FrSky. |
| uint8_t trim; |
| uint8_t HWTrim; |
| uint8_t variableOffset; |
| uint8_t channels[MAX_CHANNELS]; |
| } ChannelMap_t; |
| extern ChannelMap_t channelMap; |
| // With fixed wing, we need some way to trim the plane. This is done during a trim flight without gyros activated, |
| // and then save in a succeeding gyro calibration command. |
| typedef struct { |
| int16_t trim[MAX_CHANNELS]; |
| } RCTrim_t; |
| extern RCTrim_t rcTrim; |
| typedef struct { |
| int16_t offsets[3]; |
| } sensorOffset_t; |
| 221,6 → 228,7 |
| void IMUConfig_default(void); |
| void channelMap_default(void); |
| void rcTrim_setZero(void); |
| void paramSet_default(uint8_t setnumber); |
| void configuration_setFlightParameters(uint8_t newFlightMode); |
| /branches/dongfang_FC_fixedwing/eeprom.c |
|---|
| 144,7 → 144,7 |
| } |
| /***************************************************/ |
| /* ChannelMap */ |
| /* ChannelMap and R/C trim */ |
| /***************************************************/ |
| void channelMap_writeToEEProm(void) { |
| writeChecksummedBlock(CHANNELMAP_REVISION, (uint8_t*)&channelMap, EEPROM_ADR_CHANNELMAP, sizeof(ChannelMap_t)); |
| 155,10 → 155,23 |
| printf("\n\rwriting default channel map"); |
| channelMap_default(); |
| channelMap_writeToEEProm(); |
| // For whatever stupid reason, the newly defaulted channel map is not used! Reset. |
| wdt_enable(WDTO_500MS); |
| } |
| } |
| void rcTrim_writeToEEProm(void) { |
| writeChecksummedBlock(0, (uint8_t*)&rcTrim, EEPROM_ADR_RCTRIM, sizeof(RCTrim_t)); |
| } |
| void rcTrim_readOrDefault(void) { |
| if (readChecksummedBlock(0, (uint8_t*)&rcTrim, EEPROM_ADR_RCTRIM, sizeof(RCTrim_t))) { |
| printf("\n\rwriting zero RC trim"); |
| RC_setZeroTrim(); |
| rcTrim_writeToEEProm(); |
| } |
| } |
| /***************************************************/ |
| /* Sensor offsets */ |
| /***************************************************/ |
| /branches/dongfang_FC_fixedwing/eeprom.h |
|---|
| 20,7 → 20,8 |
| #define EEPROM_ADR_GYROOFFSET 32 |
| #define EEPROM_ADR_GYROAMPLIFIER 48 |
| #define EEPROM_ADR_CHANNELMAP 64 |
| #define EEPROM_ADR_IMU_CONFIG 100 |
| #define EEPROM_ADR_RCTRIM 96 |
| #define EEPROM_ADR_IMU_CONFIG 128 |
| #define EEPROM_ADR_PARAMSET_BEGIN 256 |
| #define CHANNELMAP_REVISION 1 |
| 31,6 → 32,7 |
| void paramSet_readOrDefault(void); |
| void channelMap_readOrDefault(void); |
| void rcTrim_readOrDefault(void); |
| void IMUConfig_readOrDefault(void); |
| void IMUConfig_writeToEEprom(void); |
| /branches/dongfang_FC_fixedwing/flight.c |
|---|
| 112,10 → 112,17 |
| term[CONTROL_THROTTLE] = controls[CONTROL_THROTTLE]; |
| // These params are just left the same in all modes. In MANUAL and RATE the results are ignored anyway. |
| target[PITCH] += ((int32_t)controls[CONTROL_ELEVATOR] * staticParams.stickIElevator) >> LOG_STICK_SCALE; |
| target[ROLL] += ((int32_t)controls[CONTROL_AILERONS] * staticParams.stickIAilerons) >> LOG_STICK_SCALE; |
| target[YAW] += ((int32_t)controls[CONTROL_RUDDER] * staticParams.stickIRudder) >> LOG_STICK_SCALE; |
| int32_t tmp; |
| tmp = ((int32_t)controls[CONTROL_ELEVATOR] * staticParams.stickIElevator) >> LOG_STICK_SCALE; |
| if (reverse[PITCH]) target[PITCH] -= tmp; else target[PITCH] += tmp; |
| tmp = ((int32_t)controls[CONTROL_AILERONS] * staticParams.stickIAilerons) >> LOG_STICK_SCALE; |
| if (reverse[ROLL]) target[ROLL] -= tmp; else target[ROLL] += tmp; |
| tmp = ((int32_t)controls[CONTROL_RUDDER] * staticParams.stickIRudder) >> LOG_STICK_SCALE; |
| if (reverse[YAW]) target[YAW] -= tmp; else target[YAW] += tmp; |
| for (axis = PITCH; axis <= YAW; axis++) { |
| if (target[axis] > OVER180) { |
| target[axis] -= OVER360; |
| 123,10 → 130,10 |
| target[axis] += OVER360; |
| } |
| if (reverse[axis]) |
| error[axis] = target[axis] - attitude[axis]; |
| else |
| error[axis] = attitude[axis] - target[axis]; |
| //if (reverse[axis]) |
| error[axis] = attitude[axis] - target[axis]; |
| // else |
| // error[axis] = attitude[axis] - target[axis]; |
| if (error[axis] > maxError[axis]) { |
| error[axis] = maxError[axis]; |
| 141,10 → 148,10 |
| /* and angular velocity (gyro signal) */ |
| /************************************************************************/ |
| if (currentFlightMode == FLIGHT_MODE_ANGLES || currentFlightMode == FLIGHT_MODE_RATE) { |
| PDPart[axis] = (((int32_t) gyro_PID[axis] * (int32_t) airspeedPID[axis].P) >> LOG_P_SCALE) |
| PDPart[axis] = +(((int32_t) gyro_PID[axis] * (int32_t) airspeedPID[axis].P) >> LOG_P_SCALE) |
| + (((int16_t)gyroD[axis] * (int16_t) airspeedPID[axis].D) >> LOG_D_SCALE); |
| if (reverse[axis]) |
| PDPart[axis] = -PDPart[axis]; |
| //if (reverse[axis]) |
| // PDPart[axis] = -PDPart[axis]; |
| } else { |
| PDPart[axis] = 0; |
| } |
| 151,14 → 158,17 |
| if (currentFlightMode == FLIGHT_MODE_ANGLES) { |
| int16_t anglePart = (int32_t)(error[axis] * (int32_t) airspeedPID[axis].I) >> LOG_I_SCALE; |
| if (reverse[axis]) |
| PDPart[axis] += anglePart; |
| else |
| PDPart[axis] -= anglePart; |
| // if (reverse[axis]) |
| PDPart[axis] += anglePart; |
| // else |
| // PDPart[axis] -= anglePart; |
| } |
| // Add I parts here... these are integrated errors. |
| term[axis] = controls[axis] + PDPart[axis]; // + IPart[axis]; |
| if (reverse[axis]) |
| term[axis] = controls[axis] - PDPart[axis]; // + IPart[axis]; |
| else |
| term[axis] = controls[axis] + PDPart[axis]; // + IPart[axis]; |
| } |
| debugOut.analog[12] = term[PITCH]; |
| /branches/dongfang_FC_fixedwing/main.c |
|---|
| 34,7 → 34,7 |
| // This is strange: It should NOT be necessarty to do. But the call of the same, |
| // in channelMap_readOrDefault (if eeprom read fails) just sets all to 0,0,0,.... |
| channelMap_default(); |
| // channelMap_default(); |
| // initalize modules |
| output_init(); |
| 48,6 → 48,7 |
| // Parameter Set handling |
| IMUConfig_readOrDefault(); |
| channelMap_readOrDefault(); |
| rcTrim_readOrDefault(); |
| paramSet_readOrDefault(); |
| // enable interrupts global |
| 138,32 → 139,21 |
| usart0_processRxData(); |
| static uint8_t aboveWarningLimitVoltageSeen = 0; |
| if (checkDelay(timer)) { |
| if (UBat <= UBAT_AT_5V) { |
| // Do nothing. The voltage on the input side of the regulator is <5V; |
| // we must be running off USB power. Keep it quiet. |
| } else if (UBat < staticParams.batteryWarningVoltage) { |
| beepBatteryAlarm(); |
| if (UBat >= staticParams.batteryWarningVoltage) { |
| aboveWarningLimitVoltageSeen = 1; |
| } else { // If we are above USB voltage, or if we have once been above warning voltage |
| if (aboveWarningLimitVoltageSeen || UBat > UBAT_AT_5V) { |
| beepBatteryAlarm(); |
| } |
| } |
| #ifdef USE_NAVICTRL |
| SPI_StartTransmitPacket(); |
| SendSPI = 4; |
| #endif |
| timer = setDelay(20); // every 20 ms |
| } |
| output_update(); |
| } |
| #ifdef USE_NAVICTRL |
| if(!SendSPI) { |
| // SendSPI is decremented in timer0.c with a rate of 9.765 kHz. |
| // within the SPI_TransmitByte() routine the value is set to 4. |
| // I.e. the SPI_TransmitByte() is called at a rate of 9.765 kHz/4= 2441.25 Hz, |
| // and therefore the time of transmission of a complete spi-packet (32 bytes) is 32*4/9.765 kHz = 13.1 ms. |
| SPI_TransmitByte(); |
| } |
| #endif |
| calculateFeaturedServoValues(); |
| if (runFlightControl) { // Time for the next iteration was up before the current finished. Signal error. |
| /branches/dongfang_FC_fixedwing/rc.c |
|---|
| 119,7 → 119,7 |
| if ((signal >= TIME(0.8)) && (signal < TIME(2.2))) { |
| // shift signal to zero symmetric range -154 to 159 |
| //signal -= 3750; // theoretical value |
| signal -= (TIME(1.5) - 128 + channelMap.trim); // best value with my Futaba in zero trim. |
| signal -= (TIME(1.5) - 128 + channelMap.HWTrim); |
| // check for stable signal |
| if (abs(signal - PPM_in[index]) < TIME(0.05)) { |
| if (RCQuality < 200) |
| 198,16 → 198,16 |
| if (RCQuality) { |
| RCQuality--; |
| debugOut.analog[20] = RCChannel(CH_ELEVATOR); |
| debugOut.analog[21] = RCChannel(CH_AILERONS); |
| debugOut.analog[22] = RCChannel(CH_RUDDER); |
| debugOut.analog[23] = RCChannel(CH_THROTTLE); |
| PRYT[CONTROL_ELEVATOR] = RCChannel(CH_ELEVATOR) - rcTrim.trim[CH_ELEVATOR]; |
| PRYT[CONTROL_AILERONS] = RCChannel(CH_AILERONS) - rcTrim.trim[CH_AILERONS]; |
| PRYT[CONTROL_RUDDER] = RCChannel(CH_RUDDER) - rcTrim.trim[CH_RUDDER]; |
| PRYT[CONTROL_THROTTLE] = RCChannel(CH_THROTTLE); // no trim on throttle! |
| PRYT[CONTROL_ELEVATOR] = RCChannel(CH_ELEVATOR); |
| PRYT[CONTROL_AILERONS] = RCChannel(CH_AILERONS); |
| PRYT[CONTROL_RUDDER] = RCChannel(CH_RUDDER); |
| PRYT[CONTROL_THROTTLE] = RCChannel(CH_THROTTLE); |
| } // if RCQuality is no good, we just do nothing. |
| debugOut.analog[20] = PRYT[CONTROL_ELEVATOR]; |
| debugOut.analog[21] = PRYT[CONTROL_AILERONS]; |
| debugOut.analog[22] = PRYT[CONTROL_RUDDER]; |
| debugOut.analog[23] = PRYT[CONTROL_THROTTLE]; |
| } // if RCQuality is no good, we just do nothing. |
| } |
| /* |
| 235,9 → 235,18 |
| } |
| void RC_calibrate(void) { |
| // Do nothing. |
| rcTrim.trim[CH_ELEVATOR] = RCChannel(CH_ELEVATOR); |
| rcTrim.trim[CH_AILERONS] = RCChannel(CH_AILERONS); |
| rcTrim.trim[CH_RUDDER] = RCChannel(CH_RUDDER); |
| rcTrim.trim[CH_THROTTLE] = 0; |
| } |
| int16_t RC_getZeroThrottle(void) { |
| return TIME (-0.5); |
| } |
| void RC_setZeroTrim(void) { |
| for (uint8_t i=0; i<MAX_CHANNELS; i++) { |
| rcTrim.trim[i] = 0; |
| } |
| } |
| /branches/dongfang_FC_fixedwing/rc.h |
|---|
| 30,6 → 30,9 |
| void RC_calibrate(void); |
| uint8_t RC_getSignalQuality(void); |
| int16_t RC_getZeroThrottle(void); |
| void RC_setZeroTrim(void); |
| #ifdef USE_MK3MAG |
| uint8_t RC_testCompassCalState(void); |
| #endif |
| /branches/dongfang_FC_fixedwing/uart0.c |
|---|
| 103,8 → 103,8 |
| "airspeed ", |
| "RC P ", //20 |
| "RC R ", |
| "RC T ", |
| "RC Y ", |
| "RC T ", |
| "Servo P ", |
| "Servo R ", //25 |
| "Servo T ", |