Diff: BaseStation/BaseStation.cpp

Revision:

3:4823d6750629

Parent:

2:969dfa4f2436

Child:

4:9ffbf9101992

--- a/BaseStation/BaseStation.cpp	Wed Apr 01 11:19:21 2015 +0000
+++ b/BaseStation/BaseStation.cpp	Fri Apr 24 16:50:20 2015 +0000
@@ -139,16 +139,22 @@
         }
         else if (mode == Status::VELOCITY)
         {
-            //Sensors::Velocity velocity = _sensors.getVelocity();
-            //_wireless->printf("<AVelX=%1.2f:AVelY=%1.2f:AVelZ=%1.2f:LVel=%1.2f:CVelX=%1.2f:CVelY=%1.2f:CVelZ=%1.2f>",
-            //velocity.accelX, velocity.accelY, velocity.accelZ, velocity.lidar, velocity.computedX, velocity.computedY, velocity.computedZ);      
+            Imu::Velocity velocity = _sensors.getImuVelocity();
+            _wireless->printf("<XVEL=%1.2f:YVEL=%1.2f:ZVEL=%1.2f>",
+            velocity.x, velocity.y, velocity.z);      
         }
         else if (mode == Status::ALTITUDE_STATUS)
         {
             NavigationController::SetPoint setPoints = _navigationController.getSetPoint();
             Sensors::Altitude altitude = _sensors.getAltitude();
-            _wireless->printf("<ACR=%1.2f:AT=%1.2f:ATHR=%1.2f:LAlt=%1.2f>",
-            setPoints.climbRate, setPoints.targetAltitude, (setPoints.throttle * 100), altitude.computed);      
+            _wireless->printf("<ACR=%1.2f:ATA=%1.2f:ATHR=%1.2f:CAlt=%1.2f:ZVEL=%1.2f>",
+            setPoints.climbRate, setPoints.targetAltitude, (setPoints.throttle * 100), altitude.computed, _sensors.getImuVelocity().z);      
+        }
+        else if (mode == Status::LIDAR)
+        {
+            Sensors::Altitude altitude = _sensors.getAltitude();
+            Imu::Angle angle = _sensors.getAngle();         
+            _wireless->printf("<SP=%1.2f:SR=%1.2f:LAlt=%1.2f>", angle.pitch, angle.roll, altitude.lidar);   
         }
         
         //Check for wireless serial command