summaryrefslogtreecommitdiffstats
diff options
context:
space:
mode:
authorSebastian Reichel <sre@ring0.de>2012-10-29 17:00:19 +0100
committerSebastian Reichel <sre@ring0.de>2012-10-29 17:00:19 +0100
commit640ed5b264bcefd345cf17edb645c39d243e0c12 (patch)
treed9dbe1c880b058708f8b2673afd5f540e34ed0ff
parentee966044a12eecbb7e9249a719addd44e5c342a8 (diff)
downloadmicrocopterd-640ed5b264bcefd345cf17edb645c39d243e0c12.tar.bz2
fix new kinematics code
-rw-r--r--ctrl/flight-control.vala6
-rw-r--r--ctrl/kinematics.vala2
-rw-r--r--main.vala2
3 files changed, 5 insertions, 5 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. */
diff --git a/main.vala b/main.vala
index c198d75..56aa404 100644
--- a/main.vala
+++ b/main.vala
@@ -66,7 +66,7 @@ int main(string[] args) {
motorCtrl.set_single(id, speed);
});
- kinematics = new Kinematics(0, 0); // TODO: Compass
+ kinematics = new Kinematics();
flightCtrl = new FlightControl();
Timeout.add(10, loop, Priority.HIGH);