merge latest lqg and apm-handler, fix speed-mode wpt bug

autopilot_handler/apm_handler/src/mavros_translate.cpp

@@ -97,59 +97,77 @@ void sendToMavros( const double &p, const double &r, const double &y, const doub
   atti_pub.publish( atti_tgt );
 }
 
-ros::ServiceClient rtkmap_lock;
-ros::ServiceClient iscompsrv;
+ros::ServiceClient map_lock;
+ros::ServiceClient bias_comp;
 bool vehicle_armed_ = false;
-bool in_cmode_ = false;
+bool in_comp_mode_ = false;
 void mavrosStateCallback( const mavros_msgs::State::ConstPtr &msg )
 {
+  /* call map lock/unlock service when arming/disarming */
+  std_srvs::SetBool lockreq;
   if( msg->armed == true && !vehicle_armed_ )
   {
     vehicle_armed_ = true;
-    std_srvs::SetBool lockrtk;
-    lockrtk.request.data = true;
-    rtkmap_lock.call( lockrtk );
-    ROS_WARN( "Locking rtk map.. " );
+    lockreq.request.data = true;
+    map_lock.call( lockreq );
   }
   else if( msg->armed == false && vehicle_armed_ )
   {
     vehicle_armed_ = false;
-    std_srvs::SetBool lockrtk;
-    lockrtk.request.data = false;
-    rtkmap_lock.call( lockrtk );
-    ROS_WARN( "Unlocking rtk map .." );
+    lockreq.request.data = false;
+    map_lock.call( lockreq );
+  }
+
+  /* call bias compensation service when switching in/out of computer */
+  std_srvs::SetBool biasreq;
+  if( msg->mode == "CMODE(25)" && !in_comp_mode_ )
+  {
+    in_comp_mode_ = true;
+    biasreq.request.data = true;
+    bias_comp.call( biasreq );
+  }
+  else if( msg->mode != "CMODE(25)" && in_comp_mode_ )
+  {
+    in_comp_mode_ = false;
+    biasreq.request.data = false;
+    bias_comp.call( biasreq );
   }
 }
-void rc_callback( const mavros_msgs::RCIn::ConstPtr &msg )
+
+
+void rcdataCallback( const mavros_msgs::RCIn::ConstPtr &msg )
 {
-  if( msg -> channels[5] < 1500 && !in_cmode_ )
+  /*  !!NOTE: mavros can publish a zero length array if no rc.
+    Check array length first, or wrap in try-catch */
+  /*
+  try
   {
-      //if( msg->mode == "CMODE(25)" && !in_cmode_ )
-    in_cmode_ = true;
-    std_srvs::SetBool iscomp;
-    iscomp.request.data = true;
-    iscompsrv.call( iscomp );
+    if( msg -> channels[5] < 1500 )
+    {
+      // do something
+    }
+    else if( msg -> channels[5] >=1500 )
+    {
+      // do something
+    }
   }
-  else if( msg -> channels[5] >=1500 && in_cmode_ )
+  catch( ... )
   {
-    in_cmode_ = false;
-    std_srvs::SetBool iscomp;
-    iscomp.request.data = false;
-    iscompsrv.call( iscomp );
+    // skip
   }
+  
+  */
 }
 
 int main( int argc, char **argv )
 {
   ros::init( argc, argv, ROS_NODE_NAME );
-
-  ros::NodeHandle nh;
+  ros::NodeHandle nh, priv_nh("~");
 
   /* Load parameters */
-  nh.param( "thrust_scaler", THRUST_SCALER, double(200.0) );
-  nh.param( "min_thrust_clip", THRUST_MIN, double(0.04) );
-  nh.param( "max_thrust_clip", THRUST_MAX, double(1.0) );
-
+  priv_nh.param( "thrust_scaler", THRUST_SCALER, double(200.0) );
+  priv_nh.param( "min_thrust_clip", THRUST_MIN, double(0.04) );
+  priv_nh.param( "max_thrust_clip", THRUST_MAX, double(1.0) );
 
   atti_pub = nh.advertise <AttiTarget>
                             ( "/mavros/setpoint_raw/attitude", 1, true );
@@ -159,11 +177,11 @@ int main( int argc, char **argv )
   ros::Subscriber mavstate_sub = nh.subscribe
                             ( "/mavros/state", 1, mavrosStateCallback );
   ros::Subscriber mavrc_sub = nh.subscribe
-                            ( "/mavros/rc/in", 1, rc_callback );
+                            ( "/mavros/rc/in", 1, rcdataCallback );
 
-  rtkmap_lock = nh.serviceClient<std_srvs::SetBool>("/set_rtk_mapcorrections");
-  iscompsrv = nh.serviceClient<std_srvs::SetBool>( "/set_bias_compensation");
+  map_lock = nh.serviceClient<std_srvs::SetBool>("/lock_arming_mapframe");
+  bias_comp = nh.serviceClient<std_srvs::SetBool>( "/set_bias_compensation");
 
-  ros::spin();
+  ros::MultiThreadedSpinner(2).spin();
   return 0;
 }

freyja_examples/src/waypoint_provider.py

@@ -41,7 +41,7 @@ def __init__(self):
           self._quit = True
 
         # Init all the publishers and subscribers
-        self.attitude_pub = rospy.Publisher("/waypoint_state", WaypointTarget, queue_size=10)
+        self.attitude_pub = rospy.Publisher("/discrete_waypoint_target", WaypointTarget, queue_size=10)
 
         time.sleep(2)
         self.start()

lqg_controller/include/bias_estimator.h

@@ -19,6 +19,8 @@
 #include <freyja_msgs/CurrentStateBiasEst.h>
 
 typedef std::chrono::microseconds uSeconds;
+typedef std::chrono::high_resolution_clock ClockTime;
+typedef std::chrono::time_point<ClockTime> ClockTimePoint;
 
 const int nStates = 9;
 const int nCtrl = 3;
@@ -33,7 +35,7 @@ class BiasEstimator
   Eigen::Matrix<double, nStates, nCtrl> sys_B_;
 
   Eigen::Matrix<double, nStates, 1> best_estimate_;
-  Eigen::Matrix<double, nCtrl, 1> ctrl_input_u_, prev_ctrl_input_;
+  Eigen::Matrix<double, nCtrl, 1> ctrl_input_u_;
   Eigen::Matrix<double, nMeas, 1> measurement_z_;
 
   /* KF matrices */
@@ -52,19 +54,25 @@ class BiasEstimator
 
   /* parameters */
   int estimator_rate_, estimator_period_us_;
+  bool estimator_off_;
 
   /* prevent state propagation when other callbacks are happening */
   std::mutex state_prop_mutex_;
   std::thread state_prop_thread_;
   volatile bool state_propagation_alive_;
   volatile int n_stprops_since_update_;
-  int n_stprops_allowed_;
 
   std::thread st_upd_thread_;
 
   /* timing related objects */
-  std::chrono::time_point<std::chrono::high_resolution_clock> ts, te, last_prop_t_;
+  ClockTimePoint ts, te, last_prop_t_;
   std::chrono::duration<double> prop_interval_;
+
+  /* smooth-in the estimator using a time-constant factor */
+  double estimator_tc_;
+  double estimator_output_shaping_;
+  ClockTimePoint t_estimator_on_;
+  std::chrono::duration<double> tc_interval_;
 
   /* final ros data published */
   freyja_msgs::CurrentStateBiasEst state_msg_;
@@ -87,6 +95,29 @@ class BiasEstimator
     void setMeasurement( const Eigen::Matrix<double, 6, 1> & );
     void setControlInput( const Eigen::Matrix<double, 4, 1> & );
     void getEstimatedBiases( Eigen::Matrix<double, 3, 1> & );
+
+    /* set and reset accessors */
+    inline void enable()
+    {
+      t_estimator_on_ = ClockTime::now();
+      estimator_off_ = false;
+    }
+    inline void disable()
+    {
+      best_estimate_.tail<3>() << 0.0, 0.0, 0.0;
+      estimator_output_shaping_ = 0.0;
+      estimator_off_ = true;
+    }
+
+    /* Time constant calc */
+    inline void updateOutputShapingFactor()
+    {
+      /* Output shaping as a cubic polynomial of time since ON */
+      auto tnow = ClockTime::now();
+      tc_interval_ = tnow - t_estimator_on_;
+      double t_ratio = tc_interval_.count() / estimator_tc_;
+      estimator_output_shaping_ = std::min( 1.0, t_ratio * t_ratio * t_ratio );
+    }
 };
 
 /*

lqg_controller/include/lqr_control_bias.h

@@ -17,6 +17,7 @@
 #include <ros/ros.h>
 #include <geometry_msgs/TransformStamped.h>
 #include <std_srvs/SetBool.h>
+#include <std_msgs/Float32.h>
 
 #include <freyja_msgs/CurrentState.h>
 #include <freyja_msgs/CtrlCommand.h>
@@ -30,6 +31,7 @@
 typedef freyja_msgs::ReferenceState TrajRef;
 typedef std_srvs::SetBool::Request BoolServReq;
 typedef std_srvs::SetBool::Response BoolServRsp;
+typedef Eigen::Matrix<double, 6, 1> PosVelNED;
 
 class LQRController
 {
@@ -66,9 +68,14 @@ class LQRController
   /* Bias estimator reference object */
   BiasEstimator &bias_est_;
   bool bias_compensation_req_;
+  bool bias_compensation_off_;
   Eigen::Matrix<double, 3, 1> f_biases_;
   std::thread bias_handler_thread_;
 
+  /*Dynamic mass estimation and compensation */
+  bool enable_dyn_mass_correction_;
+  bool enable_dyn_mass_estimation_;
+
   public:
     LQRController( BiasEstimator & );
     void initLqrSystem();
@@ -90,4 +97,8 @@ class LQRController
 
     /* helper function to calculate yaw error */
     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 & );
+    ros::Publisher est_mass_pub_;
 };