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]; |