Diff: Control/ControllerManager.cpp

Revision:

12:8e39bb45c61c

Parent:

10:0a4bf8c82493

Child:

20:6fbf1f50abda

--- a/Control/ControllerManager.cpp	Sat Nov 28 15:08:59 2015 +0000
+++ b/Control/ControllerManager.cpp	Sat Nov 28 18:47:26 2015 +0000
@@ -11,8 +11,8 @@
  */
 
 
-ControllerManager::ControllerManager(PinName b, PinName h, PinName v)
-    : controller(b, h, v){
+ControllerManager::ControllerManager(PinName h, PinName v, PinName hTrimup, PinName hTrimdown)
+    : controller(NC, h, v), pitchtrim(hTrimup, hTrimdown){
     maxpitch = Global::getmaxpitch();
     minpitch = Global::getminpitch();
     maxyaw = Global::getmaxyaw();
@@ -62,6 +62,7 @@
     Global::setyaw(getyaw());
     Global::setpitchdegree(controller.getV());//生データであることに注意
     Global::setyawdegree(controller.getH());//生データであることに注意
+    Global::setinttrimpitch(pitchtrim.getint());
 }
 
 
@@ -147,12 +148,10 @@
 
 //ここでtrim値を加える
 double ControllerManager::getpitch(){
-    //printf("pitchratio=%f doublepitch=%f gettrimpitch=%f\n", pitchratio(), doublepitch(pitchratio()), Global::gettrimpitch());
-    return clamp(doublepitch(pitchratio())+Global::gettrimpitch(), minpitch, maxpitch);
+    return clamp(doublepitch(pitchratio())+pitchtrim.get(Global::gettrimpitchrate()), minpitch, maxpitch);
 }
 double ControllerManager::getyaw(){
-    //printf("yawratio=%f doubleyaw=%f gettrimyaw=%f\n", yawratio(), doubleyaw(yawratio()), Global::gettrimyaw());
-    return clamp(doubleyaw(yawratio())+Global::gettrimyaw(), minyaw, maxyaw);
+    return clamp(doubleyaw(yawratio()), minyaw, maxyaw);
 }
 
 double ControllerManager::getpitch(double _pitchratio){