diff --git a/src/segway_RMP400.cpp b/src/segway_RMP400.cpp
index e33cc09ed9556d121c1b8b94846d2d81ee0a2df1..002f68823f9c9c236820a19ed86a6aaa3d256320 100644
--- a/src/segway_RMP400.cpp
+++ b/src/segway_RMP400.cpp
@@ -155,6 +155,16 @@ CSegwayRMP400::set_wheel_radius(const float radius)
     wheel_radius_ = radius;
 }
 
+TSegwayRMP400Status
+CSegwayRMP400::get_status()
+{
+    TSegwayRMP400Status status;
+    status.rmp200[0] = segways_[0]->get_status();
+    status.rmp200[1] = segways_[1]->get_status();
+
+    return status;
+}
+
 float
 CSegwayRMP400::get_forward_displacement()
 {
diff --git a/src/segway_RMP400.h b/src/segway_RMP400.h
index 595f51b01eed759e39dc6e524ac06683c5f141c6..a7ddf7ec7231dcc6247e75ec3b25e014b35a4ec3 100644
--- a/src/segway_RMP400.h
+++ b/src/segway_RMP400.h
@@ -16,6 +16,11 @@ const int NUM_SEGWAY_200 = 2;
 
 enum SegwayRMP400_status { rmp400_off, rmp400_connected };
 
+struct TSegwayRMP400Status
+{
+    TSegwayRMP200Status rmp200[2];
+};
+
 class CSegwayRMP400
 {
   private:
@@ -137,6 +142,13 @@ class CSegwayRMP400
      */
     bool is_connected() const;
 
+    /**
+     * \brief function to return whole platform status
+     *
+     * It will return the two segway internal status
+     */
+    TSegwayRMP400Status get_status(void);
+
     /**
      * \brief function to return the total forward displacement
      *