Merge branch debugging-devel to master (#10)

* estimator-update sync, state & ref needed for controller

*remove thread-spawn for state update, make syncd
*fix launch arg string type

* force move ctor in statev passing, increase debug verbosity

*avoid race condition with forced move ctor
*big update to controller debug, increase logging

Author: ajshank

freyja_controller.launch

@@ -21,8 +21,8 @@
 
   <!-- advanced settings -->
   <arg name="use_velctrl_only"          default="false" />
-  <arg name="bias_compensation"         default="false" />
   <arg name="enable_flatness_ff"        default="false" />
+  <arg name="bias_compensation"         default="auto" />  <!--auto, on, off-->
 
   <!-- Used for examples -->
   <arg name="use_examples"              default="false"/>

freyja_msgs/msg/lqr_ctrl/ControllerDebug.msg

@@ -2,10 +2,14 @@
 # Often useful for observers, lqr_u is the 4-axis acceleration vector.
 # State can be invalid due to time delay, or a dead source.
 
+# flags:
+uint8   CTRL_OK = 1
+uint8   BIAS_EN = 2
+uint8   FLAT_FF = 4
+uint8   MASS_CR = 8
+
 Header header
-float64[4] lqr_u
-float64 thrust
-float64 roll
-float64 pitch
-float64 yaw
-uint8 state_valid
+float32[4]  lqr_u
+float32[4]  biasv
+float32[7]  errv
+uint8       flags

lqg_controller/CMakeLists.txt

@@ -1,7 +1,7 @@
 cmake_minimum_required(VERSION 2.8.3)
 #cmake_minimum_required(VERSION 3.1.0)
 project(lqg_control)
-set( CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -O3 -Wall" )
+set( CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14 -O3 -ffast-math -Wall" )
 
 
 find_package(catkin REQUIRED COMPONENTS

lqg_controller/include/lqr_control_bias.h

@@ -83,7 +83,7 @@ class LQRController
     void initLqrSystem();
 
     ros::Subscriber state_sub_;
-    void stateCallback( const freyja_msgs::CurrentState::ConstPtr & );
+    void stateCallback( const freyja_msgs::CurrentState::ConstPtr & ) __attribute__((hot));
 
     ros::ServiceServer bias_enable_serv_;
     bool biasEnableServer( BoolServReq&, BoolServRsp& );
@@ -92,13 +92,13 @@ class LQRController
     ros::Publisher controller_debug_pub_;
 
     ros::Timer controller_timer_;
-    void computeFeedback( const ros::TimerEvent & );
+    void computeFeedback( const ros::TimerEvent & ) __attribute__((optimize("unroll-loops")));
 
     ros::Subscriber reference_sub_;
     void trajectoryReferenceCallback( const TrajRef::ConstPtr & );
 
     /* helper function to calculate yaw error */
-    inline double calcYawError( const double&, const double& ) __attribute__((always_inline));
+    static constexpr inline double calcYawError( const double&, const double& ) __attribute__((always_inline));
 
     /* estimate actual mass in flight */
     void estimateMass( const Eigen::Matrix<double, 4, 1> &, ros::Time & );

lqg_controller/src/bias_estimator.cpp

@@ -195,8 +195,7 @@ void BiasEstimator::setMeasurement( const Eigen::Matrix<double, 6, 1> &m )
 {
   measurement_z_ = m;
   n_stprops_since_update_ = 0;
-  st_upd_thread_ = std::thread( &BiasEstimator::state_updation, this );
-  st_upd_thread_.detach();
+  state_updation();
 }
 
 void BiasEstimator::setControlInput( const Eigen::Matrix<double, 4, 1> &c )

lqg_controller/src/lqr_control_bias.cpp

@@ -138,7 +138,7 @@ bool LQRController::biasEnableServer( BoolServReq& rq, BoolServRsp& rp )
   return true;  // service successful
 }
 
-double LQRController::calcYawError( const double &a, const double &b )
+constexpr double LQRController::calcYawError( const double &a, const double &b )
 {
   double yd = fmod( a - b + pi, 2*pi );
   yd = yd < 0 ? yd+2*pi : yd;
@@ -149,6 +149,7 @@ void LQRController::stateCallback( const freyja_msgs::CurrentState::ConstPtr &ms
 {
   /* Parse message to obtain state and reduced state information */
   static Eigen::Matrix<double, 7, 1> current_state;
+  static Eigen::Matrix<double, 7, 1> state_err;
 
   float yaw = msg->state_vector[8];
   rot_yaw_ << std::cos(yaw), std::sin(yaw), 0,
@@ -168,15 +169,15 @@ void LQRController::stateCallback( const freyja_msgs::CurrentState::ConstPtr &ms
   if( have_reference_update_ )
   {
     rsmtx.lock();
-    reduced_state_.head<6>() = current_state.head<6>() - reference_state_.head<6>();
+    state_err.head<6>() = current_state.head<6>() - reference_state_.head<6>();
     /* yaw-error is done differently */
-    reduced_state_(6) = calcYawError( current_state(6), reference_state_(6) );
+    state_err(6) = calcYawError( current_state(6), reference_state_(6) );
     rsmtx.unlock();
+    reduced_state_ = std::move( state_err );
+    have_state_update_ = true;
   }
 
-  have_state_update_ = true;
   last_state_update_t_ = ros::Time::now();
-  
 }
 
 void LQRController::trajectoryReferenceCallback( const TrajRef::ConstPtr &msg )
@@ -195,6 +196,7 @@ void LQRController::trajectoryReferenceCallback( const TrajRef::ConstPtr &msg )
   have_reference_update_ = true;
 }
 
+__attribute__((optimize("unroll-loops")))
 void LQRController::computeFeedback( const ros::TimerEvent &event )
 {
   /* Wait for atleast one update, or architect the code better */
@@ -203,7 +205,8 @@ void LQRController::computeFeedback( const ros::TimerEvent &event )
 
   float roll, pitch, yaw;
   double T;
-  Eigen::Matrix<double, 4, 1> control_input;
+  static Eigen::Matrix<double, 4, 1> control_input;
+  static Eigen::Matrix<double, 7, 1> state_err;
 
   bool state_valid = true;
   auto tnow = ros::Time::now();
@@ -221,7 +224,8 @@ void LQRController::computeFeedback( const ros::TimerEvent &event )
   else
   {
     /* Compute control inputs (accelerations, in this case) */
-    control_input = -1 * lqr_K_ * reduced_state_ 
+    state_err = std::move( reduced_state_ );
+    control_input = -1 * lqr_K_ * state_err 
                     + static_cast<double>(enable_flatness_ff_) * reference_ff_;
 
     /* Force saturation on downward acceleration */
@@ -245,20 +249,6 @@ void LQRController::computeFeedback( const ros::TimerEvent &event )
     yaw = control_input(3);
   }
 
-  /* Debug information */
-  freyja_msgs::ControllerDebug debug_msg;
-  debug_msg.header.stamp = tnow;
-  debug_msg.lqr_u[0] = control_input(0);
-  debug_msg.lqr_u[1] = control_input(1);
-  debug_msg.lqr_u[2] = control_input(2);
-  debug_msg.lqr_u[3] = control_input(3);
-  debug_msg.thrust = T;
-  debug_msg.roll = roll;
-  debug_msg.pitch = pitch;
-  debug_msg.yaw = yaw;
-  debug_msg.state_valid = state_valid;
-  controller_debug_pub_.publish( debug_msg );
-  
   /* Actual commanded input */
   freyja_msgs::CtrlCommand ctrl_cmd;
   ctrl_cmd.roll = roll;
@@ -268,13 +258,30 @@ void LQRController::computeFeedback( const ros::TimerEvent &event )
   ctrl_cmd.ctrl_mode = 0b00001111;
   atti_cmd_pub_.publish( ctrl_cmd );
 
+
   /* Tell bias estimator about new control input */
   if( bias_compensation_req_ )
     bias_est_.setControlInput( control_input );
 
   /* Update the total flying mass if requested */
   if( enable_dyn_mass_estimation_ )
     estimateMass( control_input, tnow );
+    
+  /* Debug information */
+  static freyja_msgs::ControllerDebug debug_msg;
+  debug_msg.header.stamp = tnow;
+  for( uint8_t idx=0; idx<4; idx++ )
+    debug_msg.lqr_u[idx] = static_cast<float>(control_input(idx));
+  for( uint8_t idx=0; idx<3; idx++ )
+    debug_msg.biasv[idx] = static_cast<float>(f_biases_(idx));
+  for( uint8_t idx=0; idx<7; idx++ )
+    debug_msg.errv[idx] = static_cast<float>(state_err(idx));
+
+  debug_msg.flags = (debug_msg.BIAS_EN * bias_compensation_req_) |
+                    (debug_msg.MASS_CR * enable_dyn_mass_correction_ ) |
+                    (debug_msg.FLAT_FF * enable_flatness_ff_ ) |
+                    (debug_msg.CTRL_OK * state_valid );
+  controller_debug_pub_.publish( debug_msg ); 
 }
 
 void LQRController::estimateMass( const Eigen::Matrix<double, 4, 1> &c, ros::Time &t )

lqg_controller/src/lqr_vel_control.cpp

@@ -166,11 +166,7 @@ void LQRController::computeFeedback( const ros::TimerEvent &event )
   debug_msg.lqr_u[1] = control_input(1);
   debug_msg.lqr_u[2] = control_input(2);
   debug_msg.lqr_u[3] = control_input(3);
-  debug_msg.thrust = T;
-  debug_msg.roll = roll;
-  debug_msg.pitch = pitch;
-  debug_msg.yaw = yaw;
-  debug_msg.state_valid = state_valid;
+  debug_msg.flags = state_valid;
   controller_debug_pub_.publish( debug_msg );
 
   /* Actual commanded input */