pos_est

PlutoPilot.cpp

@@ -1,6 +1,9 @@
 // Do not remove the include below
 #include "PlutoPilot.h"
-
+#include "src/main/flight/opticflow.h"
+#include "src/main/flight/posEstimate.h"
+#include "src/main/API/Debugging.h"
+#include <cstdio>
 /**
  * Configures Pluto's receiver to use PPM or default ESP mode; activate the line matching your setup.
  * AUX channel configurations is only for PPM recievers if no custom configureMode function is called this are the default setup
@@ -20,16 +23,26 @@ void plutoRxConfig ( void ) {
 
 // The setup function is called once at Pluto's hardware startup
 void plutoInit ( void ) {
+
   // Add your hardware initialization code here
 }
 
 // The function is called once before plutoLoop when you activate Developer Mode
 void onLoopStart ( void ) {
+ setUserLoopFrequency(150);
+
   // do your one time stuffs here
 }
 
 // The loop function is called in an endless loop
 void plutoLoop ( void ) {
+  char msg[100];
+  float pos_est_X = PositionX;
+  float pos_est_y = PositionY;
+  sprintf(msg,"\npositionX = %f ,positionY = %f \n",pos_est_X,pos_est_y);
+  Monitor_Print(msg);
+
+
   // Add your repeated code here
 }
 

src/main/target/PRIMUS_X2_v1/target.h

@@ -176,7 +176,7 @@ extern "C" {
 // #define BINDPLUG_PIN   Pin_12
 
 #define DCM
-// #define OPTIC_FLOW
+#define OPTIC_FLOW
 #define PAW3903_SPI
 // #define PAW3903_SC18