diff options
Diffstat (limited to 'ctrl/flight-control.vala')
| -rw-r--r-- | ctrl/flight-control.vala | 115 |
1 files changed, 44 insertions, 71 deletions
diff --git a/ctrl/flight-control.vala b/ctrl/flight-control.vala index 00e7b31..c2df730 100644 --- a/ctrl/flight-control.vala +++ b/ctrl/flight-control.vala @@ -18,15 +18,17 @@ */ public class FlightControl { - MotorController motor; FlightMode mode; PID[] pid; - Kinematics k; - public FlightControl() { - motor = new AtmostripeMotorController(); + int throttle; + + const double ATTITUDE_SCALING = 1.0; + + public FlightControl() throws Error { pid = new PID[PIDEntry.length]; - k = new Kinematics(0, 0); // TODO: read compass instead of using 0,0 + for(int i=0; i<PIDEntry.length; i++) + pid[i] = new PID(); } /** @@ -35,62 +37,34 @@ public class FlightControl { * Calculate roll/pitch axis error with gyro/accel data to * compute motor command thrust so used command are executed */ - void calculateFlightError(out double roll, out double pitch) { + void calculateFlightError(out int roll, out int pitch) throws Error { roll = 0; pitch = 0; if (mode == FlightMode.ATTITUDE_FLIGHT_MODE) { + double rollAttitudeCmd = pid[PIDEntry.ATTITUDE_XAXIS].update(receiver.get_value(AXIS.X) * ATTITUDE_SCALING, kinematics.kinematicsAngle[AXIS.X]); + double pitchAttitudeCmd = pid[PIDEntry.ATTITUDE_YAXIS].update(receiver.get_value(AXIS.Y) * ATTITUDE_SCALING, kinematics.kinematicsAngle[AXIS.Y]); + roll = (int) pid[PIDEntry.ATTITUDE_GYRO_XAXIS].update(rollAttitudeCmd, gyroscope.rate[AXIS.X]*1.2); + pitch = (int) pid[PIDEntry.ATTITUDE_GYRO_YAXIS].update(pitchAttitudeCmd, -gyroscope.rate[AXIS.Y]*1.2); + } else { + roll = (int) pid[PIDEntry.RATE_XAXIS].update(receiver.get_value(AXIS.X), gyroscope.rate[AXIS.X]*0.8); + pitch = (int) pid[PIDEntry.RATE_YAXIS].update(receiver.get_value(AXIS.Y), -gyroscope.rate[AXIS.Y]*0.8); } -#if 0 - if (mode == FlightMode.ATTITUDE_FLIGHT_MODE) { - // 0.0 -> (receiverCommand[AXIS.X] - receiverZero[AXIS.X]) * ATTITUDE_SCALING - double rollAttitudeCmd = pid[PIDEntry.ATTITUDE_XAXIS].update(0.1, k.kinematicsAngle[AXIS.X]); - // 0.0 -> (receiverCommand[AXIS.Y] - receiverZero[AXIS.Y]) * ATTITUDE_SCALING - double pitchAttitudeCmd = pid[PIDEntry.ATTITUDE_YAXIS].update(0.1, k.kinematicsAngle[AXIS.Y]); - - roll = pid[PIDEntry.ATTITUDE_GYRO_XAXIS].update(rollAttitudeCmd, gyroRate[AXIS.X]*1.2); - pitch = pid[PIDEntry.ATTITUDE_GYRO_YAXIS].update(pitchAttitudeCmd, -gyroRate[AXIS.Y]*1.2); - } -#endif -#if 0 - double pitchAttitudeCmd = updatePID((receiverCommand[AXIS.Y] - receiverZero[AXIS.Y]) * ATTITUDE_SCALING, -kinematicsAngle[AXIS.Y], &PID[ATTITUDE_YAXIS_PID_IDX]); - roll = updatePID(rollAttitudeCmd, gyroRate[XAXIS]*1.2, &PID[ATTITUDE_GYRO_XAXIS_PID_IDX]); - pitch = updatePID(pitchAttitudeCmd, -gyroRate[YAXIS]*1.2, &PID[ATTITUDE_GYRO_YAXIS_PID_IDX]); - } - else { - roll = updatePID(getReceiverSIData(XAXIS), gyroRate[XAXIS]*0.8, &PID[RATE_XAXIS_PID_IDX]); - pitch = updatePID(getReceiverSIData(YAXIS), -gyroRate[YAXIS]*0.8, &PID[RATE_YAXIS_PID_IDX]); - } -#endif } -#if 0 - /** - * processThrottleCorrection - * - * This function will add some throttle imput if the craft is angled - * this prevent the craft to loose altitude when angled. - * it also add the battery throttle correction in case - * of we are in auto-descent. - * - * Special thank to Ziojo for this. - */ void processThrottleCorrection() { - int throttleAsjust = throttle / (cos (radians (kinematicsAngle[XAXIS])) * cos (radians (kinematicsAngle[YAXIS]))); - throttleAsjust = constrain ((throttleAsjust - throttle), 0, 160); //compensate max +/- 25 deg XAXIS or YAXIS or +/- 18 ( 18(XAXIS) + 18(YAXIS)) - throttle = throttle + throttleAsjust + (int)batteyMonitorThrottleCorrection; + int throttleAdjust = (int) (throttle / (Math.cos(radians(kinematics.kinematicsAngle[AXIS.X])) * Math.cos(radians(kinematics.kinematicsAngle[AXIS.Y])))); + throttleAdjust = (int) constrain ((throttleAdjust - throttle), 0, 160); //compensate max +/- 25 deg XAXIS or YAXIS or +/- 18 ( 18(XAXIS) + 18(YAXIS)) + throttle = throttle + throttleAdjust; // + (int)batteyMonitorThrottleCorrection; - throttle = constrain(throttle,MINCOMMAND,MAXCOMMAND-150); // limmit throttle to leave some space for motor correction in max throttle manuever + /* limit throttle to leave some space for motor correction in max throttle manuever */ + throttle = constrain_int(throttle,motorCtrl.min, motorCtrl.max - 10); } -#endif - /** - * processHeading - * - * This function will calculate the craft heading correction depending - * of the users command. Heading correction is process with the gyro - * or a magnetometer - */ - private void processHeading() { + private void processHeadingHold() { + // TODO + } + + private void processAltitudeHold() { // TODO } @@ -99,44 +73,43 @@ public class FlightControl { * * Main flight control processos function */ - public void processFlightControl() { - double roll, pitch; + public void process() throws Error { + int throttle = 0, yaw = 0, roll, pitch; + int[] motor_data; + calculateFlightError(out roll, out pitch); - processHeading(); + processHeadingHold(); -#if 0 - /* 50Hz tasks */ + /* TODO: 50Hz tasks */ processAltitudeHold(); processThrottleCorrection(); - /* if running */ - applyMotorCommand(); - - processMinMaxCommand(); - - /* Allows quad to do acrobatics by lowering power to opposite motors during hard manuevers */ - //processHardManuevers(); + /* TODO: if running */ + motor_data = model.get_motor_data(throttle, yaw, roll, pitch); + //TODO: processMinMaxCommand(); +#if 0 /* If throttle in minimum position, don't apply yaw */ - if (receiverCommand[THROTTLE] < MINCHECK) { - for (byte motor = 0; motor < LASTMOTOR; motor++) { + if (receiver.get_value(THROTTLE) < MINCHECK) { + for (uint8 motor = 0; motor < LASTMOTOR; motor++) { motorMaxCommand[motor] = minArmedThrottle; } } /* Apply limits to motor commands */ - for (byte motor = 0; motor < LASTMOTOR; motor++) { - motorCommand[motor] = constrain(motorCommand[motor], motorMinCommand[motor], motorMaxCommand[motor]); + for (uint8 motor = 0; motor < LASTMOTOR; motor++) { + motor_data[motor] = constraint_int(motor_data[motor], motorMinCommand[motor], motorMaxCommand[motor]); } /* ESC Calibration */ if (motorArmed == OFF) { processCalibrateESC(); } - - if (motorArmed == ON && safetyCheck == ON) { - writeMotors(); - } #endif + + //if (motorArmed == ON && safetyCheck == ON) { + for(uint8 i=0; i<motor_data.length; i++) + motorCtrl.set_single(i, (uint8) motor_data[i]); + //} } } |