Subversion Repositories FlightCtrl

Rev

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

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