Subversion Repositories FlightCtrl

Rev

Rev 2097 | Details | Compare with Previous | Last modification | View Log | RSS feed

Rev Author Line No. Line
1775 - 1
#include <stdlib.h>
2
#include "commands.h"
3
#include "controlMixer.h"
4
#include "flight.h"
5
#include "eeprom.h"
2189 - 6
#include "analog.h"
1775 - 7
#include "attitude.h"
2189 - 8
#include "configuration.h"
9
#include "beeper.h"
1775 - 10
 
2052 - 11
#ifdef USE_MK3MAG
12
// TODO: Kick that all outa here!
2189 - 13
uint8_t compassCalState;
2052 - 14
#endif
2048 - 15
 
1775 - 16
void commands_handleCommands(void) {
2189 - 17
  /*
18
   * Get the current command (start/stop motors, calibrate), if any.
19
   */
20
  uint8_t command = controlMixer_getCommand();
21
  uint8_t repeated = controlMixer_isCommandRepeated();
1775 - 22
 
2189 - 23
  if (!(MKFlags & MKFLAG_MOTOR_RUN)) {
24
    uint8_t argument = controlMixer_getArgument();
25
    if ((command==COMMAND_GYROCAL || command==COMMAND_GYRO_ACC_CAL) && !repeated) {
26
      // Run gyro calibration but do not repeat it.
27
      // TODO: out of here. Anyway, MKFLAG_MOTOR_RUN is cleared. Not enough?
28
      // isFlying = 0;
29
      // check roll/pitch stick position
30
      // if pitch stick is top or roll stick is left or right --> change parameter setting
31
      // according to roll/pitch stick position
1821 - 32
 
2189 - 33
      if (argument < 6) {
34
        // Gyro calinbration, with or without selecting a new parameter-set.
35
        if (argument > 0 && argument < 6) {
36
          // A valid parameter-set (1..5) was chosen - use it.
37
          setActiveParamSet(argument);
38
        }
39
        paramSet_readFromEEProm(getActiveParamSet());
40
        analog_calibrateGyros();
41
        attitude_setNeutral();
42
        controlMixer_setNeutral();
43
        flight_setGround();
44
        beepNumber(getActiveParamSet());
45
      }
2052 - 46
#ifdef USE_MK3MAG
2189 - 47
      else if ((staticParams.bitConfig & CFG_COMPASS_ENABLED) && argument == 7) {
48
        // If right stick is centered and down
49
        compassCalState = 1;
50
        beep(1000);
51
      }
2052 - 52
#endif
2189 - 53
    }
1821 - 54
 
2189 - 55
    // save the ACC neutral setting to eeprom
56
    if ((command==COMMAND_ACCCAL || command==COMMAND_GYRO_ACC_CAL)  && !repeated) {
57
      // Run gyro and acc. meter calibration but do not repeat it.
58
      analog_calibrateAcc();
59
      attitude_setNeutral();
60
      controlMixer_setNeutral();
61
      beepNumber(getActiveParamSet());
62
    }
63
  } // end !MOTOR_RUN condition.
64
  if (command == COMMAND_START) {
65
    isFlying = 1; // TODO: Really????
66
    // if (!controlMixer_isCommandRepeated()) {
67
    // attitude_startDynamicCalibration(); // Try sense the effect of the motors on sensors.
68
    MKFlags |= MKFLAG_MOTOR_RUN;
69
    // } else { // Pilot is holding stick, ever after motor start. Continue to sense the effect of the motors on sensors.
70
    // attitude_continueDynamicCalibration();
71
    // setPointYaw = 0;
72
    // IPartPitch = 0;
73
    // IPartRoll = 0;
74
    // }
75
  } else if (command == COMMAND_STOP) {
76
    isFlying = 0;
77
    MKFlags &= ~(MKFLAG_MOTOR_RUN);
78
  }
1775 - 79
}
2048 - 80
 
81
/*
82
 *     if (controlMixer_testCompassCalState()) {
2189 - 83
 compassCalState++;
84
 if (compassCalState < 5)
85
 beepNumber(compassCalState);
86
 else
87
 beep(1000);
88
 }
2048 - 89
 *
90
 */
91
 
2052 - 92
#ifdef USE_MK3MAG
2048 - 93
uint8_t commands_isCalibratingCompass(void) {
94
  return RC_testCompassCalState();
95
}
2052 - 96
#endif