diff options
| author | Sebastian Reichel <sre@ring0.de> | 2012-10-29 17:00:19 +0100 |
|---|---|---|
| committer | Sebastian Reichel <sre@ring0.de> | 2012-10-29 17:00:19 +0100 |
| commit | 640ed5b264bcefd345cf17edb645c39d243e0c12 (patch) | |
| tree | d9dbe1c880b058708f8b2673afd5f540e34ed0ff /ctrl | |
| parent | ee966044a12eecbb7e9249a719addd44e5c342a8 (diff) | |
| download | microcopterd-640ed5b264bcefd345cf17edb645c39d243e0c12.tar.bz2 | |
fix new kinematics code
Diffstat (limited to 'ctrl')
| -rw-r--r-- | ctrl/flight-control.vala | 6 | ||||
| -rw-r--r-- | ctrl/kinematics.vala | 2 |
2 files changed, 4 insertions, 4 deletions
diff --git a/ctrl/flight-control.vala b/ctrl/flight-control.vala index c2df730..f0e1345 100644 --- a/ctrl/flight-control.vala +++ b/ctrl/flight-control.vala @@ -41,8 +41,8 @@ public class FlightControl { 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]); + double rollAttitudeCmd = pid[PIDEntry.ATTITUDE_XAXIS].update(receiver.get_value(AXIS.X) * ATTITUDE_SCALING, kinematics.result[AXIS.X]); + double pitchAttitudeCmd = pid[PIDEntry.ATTITUDE_YAXIS].update(receiver.get_value(AXIS.Y) * ATTITUDE_SCALING, kinematics.result[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 { @@ -52,7 +52,7 @@ public class FlightControl { } void processThrottleCorrection() { - int throttleAdjust = (int) (throttle / (Math.cos(radians(kinematics.kinematicsAngle[AXIS.X])) * Math.cos(radians(kinematics.kinematicsAngle[AXIS.Y])))); + int throttleAdjust = (int) (throttle / (Math.cos(kinematics.result[AXIS.X]) * Math.cos(kinematics.result[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; diff --git a/ctrl/kinematics.vala b/ctrl/kinematics.vala index 47a41bc..0f07f62 100644 --- a/ctrl/kinematics.vala +++ b/ctrl/kinematics.vala @@ -22,7 +22,7 @@ */ public class Kinematics { - double[] result = new double[3]; + public double[] result = new double[3]; bool firstUpdate; /* Quaternion orientation of earth frame relative to auxiliary frame. */ |