Subversion Repositories FlightCtrl

Rev

Rev 1805 | Rev 1844 | Go to most recent revision | Blame | Compare with Previous | Last modification | View Log | RSS feed

// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Copyright (c) 04.2007 Holger Buss
// + Nur für den privaten Gebrauch
// + www.MikroKopter.com
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation),
// + dass eine Nutzung (auch auszugsweise) nur für den privaten und nicht-kommerziellen Gebrauch zulässig ist.
// + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt
// + bzgl. der Nutzungsbedingungen aufzunehmen.
// + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen,
// + Verkauf von Luftbildaufnahmen, usw.
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht,
// + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts
// + auf anderen Webseiten oder Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de"
// + eindeutig als Ursprung verlinkt und genannt werden
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion
// + Benutzung auf eigene Gefahr
// + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur
// + mit unserer Zustimmung zulässig
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Redistributions of source code (with or without modifications) must retain the above copyright notice,
// + this list of conditions and the following disclaimer.
// +   * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived
// +     from this software without specific prior written permission.
// +   * The use of this project (hardware, software, binary files, sources and documentation) is only permittet
// +     for non-commercial use (directly or indirectly)
// +     Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted
// +     with our written permission
// +   * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be
// +     clearly linked as origin
// +   * porting to systems other than hardware from www.mikrokopter.de is not allowed
// +  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// +  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// +  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// +  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
// +  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// +  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// +  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// +  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// +  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// +  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// +  POSSIBILITY OF SUCH DAMAGE.
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

#include <stdlib.h>
#include <avr/io.h>
#include "eeprom.h"
#include "flight.h"

// Only for debug. Remove.
//#include "analog.h"
//#include "rc.h"

// Necessary for external control and motor test
#include "uart0.h"

// for scope debugging
// #include "rc.h"

#include "twimaster.h"
#include "attitude.h"
#include "controlMixer.h"
#include "commands.h"
#ifdef USE_MK3MAG
#include "gps.h"
#endif

#define CHECK_MIN_MAX(value, min, max) {if(value < min) value = min; else if(value > max) value = max;}

/*
 * These are no longer maintained, just left at 0. The original implementation just summed the acc.
 * value to them every 2 ms. No filtering or anything. Just a case for an eventual overflow?? Hey???
 */

// int16_t naviAccPitch = 0, naviAccRoll = 0, naviCntAcc = 0;

uint8_t gyroPFactor, gyroIFactor; // the PD factors for the attitude control
uint8_t yawPFactor, yawIFactor; // the PD factors for the yaw control

// Some integral weight constant...
uint16_t Ki = 10300 / 33;
uint8_t RequiredMotors = 0;

// No support for altitude control right now.
// int16_t SetPointHeight = 0;

/************************************************************************/
/*  Filter for motor value smoothing (necessary???)                     */
/************************************************************************/
int16_t motorFilter(int16_t newvalue, int16_t oldvalue) {
        switch (dynamicParams.UserParams[5]) {
        case 0:
                return newvalue;
        case 1:
                return (oldvalue + newvalue) / 2;
        case 2:
                if (newvalue > oldvalue)
                        return (1 * (int16_t) oldvalue + newvalue) / 2; //mean of old and new
                else
                        return newvalue - (oldvalue - newvalue) * 1; // 2 * new - old
        case 3:
                if (newvalue < oldvalue)
                        return (1 * (int16_t) oldvalue + newvalue) / 2; //mean of old and new
                else
                        return newvalue - (oldvalue - newvalue) * 1; // 2 * new - old
        default:
                return newvalue;
        }
}

/************************************************************************/
/*  Neutral Readings                                                    */
/************************************************************************/
void flight_setNeutral() {
        MKFlags |= MKFLAG_CALIBRATE;

        // not really used here any more.
        dynamicParams.KalmanK = -1;
        dynamicParams.KalmanMaxDrift = 0;
        dynamicParams.KalmanMaxFusion = 32;

        controlMixer_initVariables();
}

void setFlightParameters(uint8_t _Ki, uint8_t _gyroPFactor,
                uint8_t _gyroIFactor, uint8_t _yawPFactor, uint8_t _yawIFactor) {
        Ki = 10300 / _Ki;
        gyroPFactor = _gyroPFactor;
        gyroIFactor = _gyroIFactor;
        yawPFactor = _yawPFactor;
        yawIFactor = _yawIFactor;
}

void setNormalFlightParameters(void) {
        setFlightParameters(dynamicParams.IFactor + 1, dynamicParams.GyroP + 10,
                        staticParams.GlobalConfig & CFG_HEADING_HOLD ? 0 : dynamicParams.GyroI,
                        dynamicParams.GyroP + 10, dynamicParams.UserParams[6]);
}

void setStableFlightParameters(void) {
        setFlightParameters(33, 90, 120, 90, 120);
}

/************************************************************************/
/*  Main Flight Control                                                 */
/************************************************************************/
void flight_control(void) {
        int16_t tmp_int;
        // Mixer Fractions that are combined for Motor Control
        int16_t yawTerm, throttleTerm, term[2];

        // PID controller variables
        int16_t PDPart[2], PDPartYaw, PPart[2];
        static int32_t IPart[2] = { 0, 0 };
        //  static int32_t yawControlRate = 0;

        // Removed. Too complicated, and apparently not necessary with MEMS gyros anyway.
        // static int32_t IntegralGyroPitchError = 0, IntegralGyroRollError = 0;
        // static int32_t CorrectionPitch, CorrectionRoll;

        static uint16_t emergencyFlightTime;
        static int8_t debugDataTimer = 1;

        // High resolution motor values for smoothing of PID motor outputs
        static int16_t motorFilters[MAX_MOTORS];

        uint8_t i, axis;

        controlMixer_update();

        // Fire the main flight attitude calculation, including integration of angles.
        calculateFlightAttitude();

        throttleTerm = controlThrottle;
        // This check removed. Is done on a per-motor basis, after output matrix multiplication.
        // if(throttleTerm < staticParams.MinThrottle + 10) throttleTerm = staticParams.MinThrottle + 10;
        // else if(throttleTerm > staticParams.MaxThrottle - 20) throttleTerm = (staticParams.MaxThrottle - 20);

        /************************************************************************/
        /* RC-signal is bad                                                     */
        /* This part could be abstracted, as having yet another control input   */
        /* to the control mixer: An emergency autopilot control.                */
        /************************************************************************/

        if (controlMixer_getSignalQuality() <= SIGNAL_BAD) { // the rc-frame signal is not reveived or noisy
                RED_ON;
                beepRCAlarm();

                if (emergencyFlightTime) {
                        // continue emergency flight
                        emergencyFlightTime--;
                        if (isFlying > 256) {
                                // We're probably still flying. Descend slowly.
                                throttleTerm = staticParams.EmergencyGas; // Set emergency throttle
                                MKFlags |= (MKFLAG_EMERGENCY_LANDING); // Set flag for emergency landing
                                setStableFlightParameters();
                        } else {
                                MKFlags &= ~(MKFLAG_MOTOR_RUN); // Probably not flying, and bad R/C signal. Kill motors.
                        }
                } else {
                        // end emergency flight (just cut the motors???)
                        MKFlags &= ~(MKFLAG_MOTOR_RUN | MKFLAG_EMERGENCY_LANDING);
                }
        } else {
                // signal is acceptable
                if (controlMixer_getSignalQuality() > SIGNAL_BAD) {
                        // Reset emergency landing control variables.
                        MKFlags &= ~(MKFLAG_EMERGENCY_LANDING); // clear flag for emergency landing
                        // The time is in whole seconds.
                        emergencyFlightTime = (uint16_t) staticParams.EmergencyGasDuration * 488;
                }

                // If some throttle is given, and the motor-run flag is on, increase the probability that we are flying.
                if (throttleTerm > 40 && (MKFlags & MKFLAG_MOTOR_RUN)) {
                        // increment flight-time counter until overflow.
                        if (isFlying != 0xFFFF)
                                isFlying++;
                } else
                /*
                 * When standing on the ground, do not apply I controls and zero the yaw stick.
                 * Probably to avoid integration effects that will cause the copter to spin
                 * or flip when taking off.
                 */

                if (isFlying < 256) {
                        IPart[PITCH] = IPart[ROLL] = 0;
                        // TODO: Don't stomp on other modules' variables!!!
                        // controlYaw = 0;
                        PDPartYaw = 0; // instead.
                        if (isFlying == 250) {
                                // HC_setGround();
                                updateCompassCourse = 1;
                                yawAngleDiff = 0;
                        }
                } else {
                        // Set fly flag. TODO: Hmmm what can we trust - the isFlying counter or the flag?
                        // Answer: The counter. The flag is not read from anywhere anyway... except the NC maybe.
                        MKFlags |= (MKFLAG_FLY);
                }

                commands_handleCommands();

                // if(controlMixer_getSignalQuality() >= SIGNAL_GOOD) {
                setNormalFlightParameters();
                // }
        } // end else (not bad signal case)
        // end part1a: 750-800 usec.
        /*
         * Looping the H&I way basically is just a matter of turning off attitude angle measurement
         * by integration (because 300 deg/s gyros are too slow) and turning down the throttle.
         * This is the throttle part.
         */

        if (looping) {
                if (throttleTerm > staticParams.LoopGasLimit)
                        throttleTerm = staticParams.LoopGasLimit;
        }

        /************************************************************************/
        /*  Yawing                                                              */
        /************************************************************************/
        if (abs(controlYaw) > 4 * staticParams.StickYawP) { // yaw stick is activated
                ignoreCompassTimer = 1000;
                if (!(staticParams.GlobalConfig & CFG_COMPASS_FIX)) {
                        updateCompassCourse = 1;
                }
        }

        //  yawControlRate = controlYaw;

        // Trim drift of yawAngleDiff with controlYaw.
        // TODO: We want NO feedback of control related stuff to the attitude related stuff.
        // This seems to be used as: Difference desired <--> real heading.
        yawAngleDiff -= controlYaw;

        // limit the effect
        CHECK_MIN_MAX(yawAngleDiff, -50000, 50000);

        /************************************************************************/
        /* Compass is currently not supported.                                  */
        /************************************************************************/
        if (staticParams.GlobalConfig & (CFG_COMPASS_ACTIVE | CFG_GPS_ACTIVE)) {
                updateCompass();
        }

#if defined (USE_NAVICTRL)
        /************************************************************************/
        /* GPS is currently not supported.                                      */
        /************************************************************************/
        if(staticParams.GlobalConfig & CFG_GPS_ACTIVE) {
                GPS_Main();
                MKFlags &= ~(MKFLAG_CALIBRATE | MKFLAG_START);
        } else {
                // GPSStickPitch = 0;
                // GPSStickRoll = 0;
        }
#endif
        // end part 1: 750-800 usec.
        // start part 3: 350 - 400 usec.
#define SENSOR_LIMIT  (4096 * 4)
        /************************************************************************/

        /* Calculate control feedback from angle (gyro integral)                */
        /* and angular velocity (gyro signal)                                   */
        /************************************************************************/
        // The P-part is the P of the PID controller. That's the angle integrals (not rates).
        for (axis = PITCH; axis <= ROLL; axis++) {
                if (looping & ((1 << 4) << axis)) {
                        PPart[axis] = 0;
                } else { // TODO: Where do the 44000 come from???
                        PPart[axis] = angle[axis] * gyroIFactor / (44000 / CONTROL_SCALING); // P-Part - Proportional to Integral
                }

                /*
                 * Now blend in the D-part - proportional to the Differential of the integral = the rate.
                 * Read this as: PDPart = PPart + rate_PID * pfactor * CONTROL_SCALING
                 * where pfactor is in [0..1].
                 */

                PDPart[axis] = PPart[axis] + (int32_t) ((int32_t) rate_PID[axis]
                                * gyroPFactor / (256L / CONTROL_SCALING)) + (differential[axis]
                                * (int16_t) dynamicParams.GyroD) / 16;

                CHECK_MIN_MAX(PDPart[axis], -SENSOR_LIMIT, SENSOR_LIMIT);
        }

        PDPartYaw = (int32_t) (yawRate * 2 * (int32_t) yawPFactor) / (256L
                        / CONTROL_SCALING) + (int32_t) (yawAngleDiff * yawIFactor) / (2 * (44000
                        / CONTROL_SCALING));

        // limit control feedback
        CHECK_MIN_MAX(PDPartYaw, -SENSOR_LIMIT, SENSOR_LIMIT);

        /*
         * Compose throttle term.
         * If a Bl-Ctrl is missing, prevent takeoff.
         */

        if (missingMotor) {
                // if we are in the lift off condition. Hmmmmmm when is throttleTerm == 0 anyway???
                if (isFlying > 1 && isFlying < 50 && throttleTerm > 0)
                        isFlying = 1; // keep within lift off condition
                throttleTerm = staticParams.MinThrottle; // reduce gas to min to avoid lift of
        }

        // Scale up to higher resolution. Hmm why is it not (from controlMixer and down) scaled already?
        throttleTerm *= CONTROL_SCALING;

        /*
         * Compose yaw term.
         * The yaw term is limited: Absolute value is max. = the throttle term / 2.
         * However, at low throttle the yaw term is limited to a fixed value,
         * and at high throttle it is limited by the throttle reserve (the difference
         * between current throttle and maximum throttle).
         */

#define MIN_YAWGAS (40 * CONTROL_SCALING)  // yaw also below this gas value
        yawTerm = PDPartYaw - controlYaw * CONTROL_SCALING;
        // Limit yawTerm
        if (throttleTerm > MIN_YAWGAS) {
                CHECK_MIN_MAX(yawTerm, - (throttleTerm / 2), (throttleTerm / 2));
        } else {
                CHECK_MIN_MAX(yawTerm, - (MIN_YAWGAS / 2), (MIN_YAWGAS / 2));
        }

        tmp_int = staticParams.MaxThrottle * CONTROL_SCALING;
        CHECK_MIN_MAX(yawTerm, -(tmp_int - throttleTerm), (tmp_int - throttleTerm));

        tmp_int = (int32_t) ((int32_t) dynamicParams.DynamicStability
                        * (int32_t) (throttleTerm + abs(yawTerm) / 2)) / 64;

        for (axis = PITCH; axis <= ROLL; axis++) {
                /*
                 * Compose pitch and roll terms. This is finally where the sticks come into play.
                 */

                if (gyroIFactor) {
                        // Integration mode: Integrate (angle - stick) = the difference between angle and stick pos.
                        // That means: Holding the stick a little forward will, at constant flight attitude, cause this to grow (decline??) over time.
                        // TODO: Find out why this seems to be proportional to stick position - not integrating it at all.
                        IPart[axis] += PPart[axis] - control[axis]; // Integrate difference between P part (the angle) and the stick pos.
                } else {
                        // "HH" mode: Integrate (rate - stick) = the difference between rotation rate and stick pos.
                        // To keep up with a full stick PDPart should be about 156...
                        IPart[axis] += PDPart[axis] - control[axis]; // With gyroIFactor == 0, PDPart is really just a D-part. Integrate D-part (the rot. rate) and the stick pos.
                }

                // TODO: From which planet comes the 16000?
                CHECK_MIN_MAX(IPart[axis], -(CONTROL_SCALING * 16000L), (CONTROL_SCALING * 16000L));
                // Add (P, D) parts minus stick pos. to the scaled-down I part.
                term[axis] = PDPart[axis] - control[axis] + IPart[axis] / Ki; // PID-controller for pitch

                /*
                 * Apply "dynamic stability" - that is: Limit pitch and roll terms to a growing function of throttle and yaw(!).
                 * The higher the dynamic stability parameter, the wider the bounds. 64 seems to be a kind of unity
                 * (max. pitch or roll term is the throttle value).
                 * TODO: Why a growing function of yaw?
                 */

                CHECK_MIN_MAX(term[axis], -tmp_int, tmp_int);
        }
        // end part 3: 350 - 400 usec.

        // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
        // Universal Mixer
        // Each (pitch, roll, throttle, yaw) term is in the range [0..255 * CONTROL_SCALING].
        // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

        DebugOut.Analog[12] = term[PITCH];
        DebugOut.Analog[13] = term[ROLL];
        DebugOut.Analog[14] = yawTerm;
        DebugOut.Analog[15] = throttleTerm;

        for (i = 0; i < MAX_MOTORS; i++) {
                int16_t tmp;
                if (MKFlags & MKFLAG_MOTOR_RUN && Mixer.Motor[i][MIX_THROTTLE] > 0) {
                        tmp = ((int32_t) throttleTerm * Mixer.Motor[i][MIX_THROTTLE]) / 64L;
                        tmp += ((int32_t) term[PITCH] * Mixer.Motor[i][MIX_PITCH]) / 64L;
                        tmp += ((int32_t) term[ROLL] * Mixer.Motor[i][MIX_ROLL]) / 64L;
                        tmp += ((int32_t) yawTerm * Mixer.Motor[i][MIX_YAW]) / 64L;
                        motorFilters[i] = motorFilter(tmp, motorFilters[i]);
                        // Now we scale back down to a 0..255 range.
                        tmp = motorFilters[i] / CONTROL_SCALING;
                        // So this was the THIRD time a throttle was limited. But should the limitation
                        // apply to the common throttle signal (the one used for setting the "power" of
                        // all motors together) or should it limit the throttle set for each motor,
                        // including mix components of pitch, roll and yaw? I think only the common
                        // throttle should be limited.
                        // --> WRONG. This caused motors to stall completely in tight maneuvers.
                        // Apply to individual signals instead.
                        CHECK_MIN_MAX(tmp, staticParams.MinThrottle, staticParams.MaxThrottle);
                        CHECK_MIN_MAX(tmp, 1, 255);
                        motor[i].SetPoint = tmp;
                } else if (motorTestActive) {
                        motor[i].SetPoint = motorTest[i];
                } else {
                        motor[i].SetPoint = 0;
                }
                if (i < 4)
                        DebugOut.Analog[22 + i] = motor[i].SetPoint;
        }
        I2C_Start(TWI_STATE_MOTOR_TX);

        // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
        // Debugging
        // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
        if (!(--debugDataTimer)) {
                debugDataTimer = 24; // update debug outputs at 488 / 24 = 20.3 Hz.
                DebugOut.Analog[0] = (10 * angle[PITCH]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg
                DebugOut.Analog[1] = (10 * angle[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg
                DebugOut.Analog[2] = yawGyroHeading / GYRO_DEG_FACTOR_YAW;

                /*
                 DebugOut.Analog[23] = (yawRate * 2 * (int32_t)yawPFactor) / (256L / CONTROL_SCALING);
                 DebugOut.Analog[24] = controlYaw;
                 DebugOut.Analog[25] = yawAngleDiff / 100L;
                 DebugOut.Analog[26] = accNoisePeak[PITCH];
                 DebugOut.Analog[27] = accNoisePeak[ROLL];
                 DebugOut.Analog[30] = gyroNoisePeak[PITCH];
                 DebugOut.Analog[31] = gyroNoisePeak[ROLL];
                 */

        }
}