Subversion Repositories FlightCtrl

#include <stdlib.h>
#include "commands.h"
#include "controlMixer.h"
#include "flight.h"
#include "eeprom.h"
#include "attitude.h"
#include "output.h"
#include "rc.h"

#ifdef USE_MK3MAG
// TODO: Kick that all outa here!
uint8_t compassCalState = 0;
#endif

void commands_handleCommands(void) {
        /*
         * Get the current command (start/stop motors, calibrate), if any.
         */
        uint8_t command = controlMixer_getCommand();
        uint8_t repeated = controlMixer_isCommandRepeated();
        uint8_t argument = controlMixer_getArgument();

        if (!(MKFlags & MKFLAG_MOTOR_RUN)) {
                if (command == COMMAND_GYROCAL && !repeated) {
                        // Run gyro calibration but do not repeat it.
                        // TODO: out of here. Anyway, MKFLAG_MOTOR_RUN is cleared. Not enough?
                        // isFlying = 0;
                        // check roll/pitch stick position
                        // if pitch stick is top or roll stick is left or right --> change parameter setting
                        // according to roll/pitch stick position

                        if (argument < 6) {
                                // Gyro calinbration, with or without selecting a new parameter-set.
                                if (argument > 0 && argument < 6) {
                                        // A valid parameter-set (1..5) was chosen - use it.
                                        setActiveParamSet(argument);
                                }
                                paramSet_readFromEEProm(getActiveParamSet());
                                analog_calibrateGyros();
                                attitude_setNeutral();
                                controlMixer_setNeutral();
                                beepNumber(getActiveParamSet());
                        }
#ifdef USE_MK3MAG
                        else if ((staticParams.bitConfig & CFG_COMPASS_ENABLED) && argument == 7) {
                                // If right stick is centered and down
                                compassCalState = 1;
                                beep(1000);
                        }
#endif
                }

                // save the ACC neutral setting to eeprom
                else {
                        if (command == COMMAND_ACCCAL && !repeated) {
                                // Run gyro and acc. meter calibration but do not repeat it.
                                analog_calibrateAcc();
                                attitude_setNeutral();
                                controlMixer_setNeutral();
                                beepNumber(getActiveParamSet());
                        }
                }
        } // end !MOTOR_RUN condition.
        if (command == COMMAND_START) {
                isFlying = 1; // TODO: Really????
                // if (!controlMixer_isCommandRepeated()) {
                // attitude_startDynamicCalibration(); // Try sense the effect of the motors on sensors.
                MKFlags |= (MKFLAG_MOTOR_RUN | MKFLAG_START); // set flag RUN and START. TODO: Is that START flag used at all???
                // } else { // Pilot is holding stick, ever after motor start. Continue to sense the effect of the motors on sensors.
                // attitude_continueDynamicCalibration();
                // setPointYaw = 0;
                // IPartPitch = 0;
                // IPartRoll = 0;
                // }
        } else if (command == COMMAND_STOP) {
                isFlying = 0;
                MKFlags &= ~(MKFLAG_MOTOR_RUN);
        }
}

/*
 *     if (controlMixer_testCompassCalState()) {
      compassCalState++;
      if (compassCalState < 5)
        beepNumber(compassCalState);
      else
        beep(1000);
    }
 *
 */

#ifdef USE_MK3MAG
uint8_t commands_isCalibratingCompass(void) {
  return RC_testCompassCalState();
}
#endif