Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 1774 → Rev 1775

/branches/dongfang_FC_rewrite/flight.c
61,9 → 61,14
 
// 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
76,10 → 81,6
*/
// int16_t naviAccPitch = 0, naviAccRoll = 0, naviCntAcc = 0;
 
// MK flags
uint16_t isFlying = 0;
volatile uint8_t MKFlags = 0;
 
uint8_t gyroPFactor, gyroIFactor; // the PD factors for the attitude control
uint8_t yawPFactor, yawIFactor; // the PD factors for the yaw control
 
117,9 → 118,6
/* Neutral Readings */
/************************************************************************/
void flight_setNeutral() {
// GPSStickPitch = 0;
// GPSStickRoll = 0;
 
MKFlags |= MKFLAG_CALIBRATE;
 
// not really used here any more.
130,32 → 128,6
controlMixer_initVariables();
}
 
/************************************************************************/
/* Transmit Motor Data via I2C */
/************************************************************************/
void sendMotorData(void) {
uint8_t i;
if(!(MKFlags & MKFLAG_MOTOR_RUN)) {
// If pilot has not started the engines....
MKFlags &= ~(MKFLAG_FLY | MKFLAG_START); // clear flag FLY and START if motors are off
for(i = 0; i < MAX_MOTORS; i++) {
// and if we are not in motor test mode, cut throttle on all motors.
if(!motorTestActive) Motor[i].SetPoint = 0;
else Motor[i].SetPoint = motorTest[i];
}
if(motorTestActive) motorTestActive--;
}
/*
DebugOut.Analog[] = Motor[0].SetPoint; // Front
DebugOut.Analog[] = Motor[1].SetPoint; // Rear
DebugOut.Analog[] = Motor[3].SetPoint; // Left
DebugOut.Analog[] = Motor[2].SetPoint; // Right
*/
// Start I2C Interrupt Mode
I2C_Start(TWI_STATE_MOTOR_TX);
}
 
void setFlightParameters(uint8_t _Ki, uint8_t _gyroPFactor, uint8_t _gyroIFactor, uint8_t _yawPFactor, uint8_t _yawIFactor) {
Ki = 10300 / _Ki;
gyroPFactor = _gyroPFactor;
177,67 → 149,6
setFlightParameters(33, 90, 120, 90, 120);
}
 
void handleCommands(uint8_t command, uint8_t argument, uint8_t isCommandRepeated) {
if(!(MKFlags & MKFLAG_MOTOR_RUN)) {
if (command == COMMAND_GYROCAL && !isCommandRepeated) {
// Run gyro calibration but do not repeat it.
GRN_OFF;
// 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());
attitude_setNeutral();
flight_setNeutral();
// Right stick is centered; calibrate it to zero (hmm strictly does not belong here).
// If heading hold is active, do not do it. TODO: We also want to re-set old calibration.
controlMixer_setNeutral(!argument);
beepNumber(getActiveParamSet());
} else if(staticParams.GlobalConfig & (CFG_COMPASS_ACTIVE | CFG_GPS_ACTIVE) && argument == 7) {
// If right stick is centered and down
compassCalState = 1;
beep(1000);
}
}
// save the ACC neutral setting to eeprom
else {
if(command == COMMAND_ACCCAL && !isCommandRepeated) {
// Run gyro and acc. meter calibration but do not repeat it.
GRN_OFF;
analog_calibrateAcc();
attitude_setNeutral();
flight_setNeutral();
controlMixer_setNeutral(1); // Calibrate right stick neutral position.
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);
}
}
 
/************************************************************************/
/* Main Flight Control */
250,7 → 161,7
// PID controller variables
int16_t PDPart[2], PDPartYaw, PPart[2];
static int32_t IPart[2] = {0,0};
static int32_t setPointYaw = 0;
// static int32_t yawControlRate = 0;
 
// Removed. Too complicated, and apparently not necessary with MEMS gyros anyway.
// static int32_t IntegralGyroPitchError = 0, IntegralGyroRollError = 0;
265,23 → 176,32
uint8_t i, axis;
 
// Fire the main flight attitude calculation, including integration of angles.
 
calculateFlightAttitude();
GRN_ON;
 
/*
* TODO: update should: Set the stick variables if good signal, set them to zero if bad.
* Set variables also.
*/
// start part 1: 750-800 usec.
// start part 1a: 750-800 usec.
// start part1b: 700 usec
// start part1c: 700 usec!!!!!!!!! WAY too slow.
controlMixer_update();
// end part1c
 
throttleTerm = controlThrottle;
if(throttleTerm < staticParams.MinThrottle + 10) throttleTerm = staticParams.MinThrottle + 10;
// 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);
 
// end part1b: 700 usec.
/************************************************************************/
/* 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();
289,7 → 209,7
if(emergencyFlightTime) {
// continue emergency flight
emergencyFlightTime--;
if(isFlying > 1000) {
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
307,7 → 227,7
// Reset emergency landing control variables.
MKFlags &= ~(MKFLAG_EMERGENCY_LANDING); // clear flag for emergency landing
// The time is in whole seconds.
emergencyFlightTime = staticParams.EmergencyGasDuration * 488;
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.
321,35 → 241,28
* or flip when taking off.
*/
if(isFlying < 256) {
IPart[PITCH] = IPart[ROLL] = 0;
// TODO: Don't stomp on other modules' variables!!!
controlYaw = 0;
if(isFlying == 250) {
updateCompassCourse = 1;
yawAngle = 0;
setPointYaw = 0;
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 {
// DebugOut.Digital[1] = 0;
// 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);
}
/*
* 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();
 
handleCommands(command, argument, repeated);
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.
369,49 → 282,47
}
}
setPointYaw = controlYaw;
// yawControlRate = controlYaw;
 
// Trim drift of yawAngle with controlYaw.
// Trim drift of yawAngleDiff with controlYaw.
// TODO: We want NO feedback of control related stuff to the attitude related stuff.
yawAngle -= controlYaw;
// This seems to be used as: Difference desired <--> real heading.
yawAngleDiff -= controlYaw;
// limit the effect
CHECK_MIN_MAX(yawAngle, -50000, 50000)
 
/************************************************************************/
/* Compass is currently not supported. */
/************************************************************************/
/*
if(staticParams.GlobalConfig & (CFG_COMPASS_ACTIVE|CFG_GPS_ACTIVE)) {
updateCompass();
}
*/
 
CHECK_MIN_MAX(yawAngleDiff, -50000, 50000);
/************************************************************************/
/* Compass is currently not supported. */
/************************************************************************/
if(staticParams.GlobalConfig & (CFG_COMPASS_ACTIVE|CFG_GPS_ACTIVE)) {
updateCompass();
}
#if defined (USE_MK3MAG)
/************************************************************************/
/* GPS is currently not supported. */
/************************************************************************/
/*
if(staticParams.GlobalConfig & CFG_GPS_ACTIVE) {
GPS_Main();
MKFlags &= ~(MKFLAG_CALIBRATE | MKFLAG_START);
}
else {
// GPSStickPitch = 0;
// GPSStickRoll = 0;
}
*/
/************************************************************************/
/* 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))) {
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
427,12 → 338,13
 
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));
PDPartYaw = (int32_t)(yawRate * 2 * (int32_t)yawPFactor) / (256L / CONTROL_SCALING)
+ (int32_t)(yawAngle * yawIFactor) / (2 * (44000 / CONTROL_SCALING));
// limit control feedback
CHECK_MIN_MAX(PDPartYaw, -SENSOR_LIMIT, SENSOR_LIMIT);
CHECK_MIN_MAX(PDPartYaw, -SENSOR_LIMIT, SENSOR_LIMIT);
/*
* Compose throttle term.
440,15 → 352,12
*/
if(missingMotor) {
// if we are in the lift off condition. Hmmmmmm when is throttleTerm == 0 anyway???
if((isFlying > 1) && (isFlying < 50) && (throttleTerm > 0))
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
}
 
/*
* Height control was here.
*/
if(throttleTerm > staticParams.MaxThrottle - 20) throttleTerm = (staticParams.MaxThrottle - 20);
// Scale up to higher resolution. Hmm why is it not (from controlMixer and down) scaled already?
throttleTerm *= CONTROL_SCALING;
 
/*
459,14 → 368,14
* between current throttle and maximum throttle).
*/
#define MIN_YAWGAS (40 * CONTROL_SCALING) // yaw also below this gas value
yawTerm = PDPartYaw - setPointYaw * CONTROL_SCALING;
// limit yawTerm
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));
 
486,12 → 395,12
// 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
500,26 → 409,49
*/
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(Mixer.Motor[i][MIX_THROTTLE] > 0) { // If a motor has a zero throttle mix, it is not considered.
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);
Motor[i].SetPoint = tmp;
CHECK_MIN_MAX(tmp, 1, 255);
motor[i].SetPoint = tmp;
}
else Motor[i].SetPoint = 0;
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
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
529,31 → 461,13
DebugOut.Analog[1] = (10 * angle[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg
DebugOut.Analog[2] = yawGyroHeading / GYRO_DEG_FACTOR_YAW;
 
// DebugOut.Analog[9] = setPointYaw;
// DebugOut.Analog[10] = yawIFactor;
// DebugOut.Analog[11] = gyroIFactor;
// DebugOut.Analog[12] = RC_getVariable(0);
// DebugOut.Analog[13] = dynamicParams.UserParams[0];
// DebugOut.Analog[14] = RC_getVariable(4);
// DebugOut.Analog[15] = dynamicParams.UserParams[4];
/* DebugOut.Analog[11] = yawGyroHeading / GYRO_DEG_FACTOR_YAW; */
 
// 12..15 are the controls.
// DebugOut.Analog[16] = pitchAxisAcc;
// DebugOut.Analog[17] = rollAxisAcc;
DebugOut.Analog[18] = HIRES_GYRO_INTEGRATION_FACTOR;
 
DebugOut.Analog[19] = throttleTerm;
DebugOut.Analog[20] = term[PITCH];
DebugOut.Analog[21] = term[ROLL];
DebugOut.Analog[22] = yawTerm;
 
DebugOut.Analog[23] = PPart[PITCH]; //
DebugOut.Analog[24] = IPart[PITCH] /Ki; // meget meget lille.
DebugOut.Analog[25] = PDPart[PITCH]; // omtrent lig ppart.
 
/*
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];