Subversion Repositories FlightCtrl

Rev

Rev 2097 | 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 
 }