Merge pull request #3 from BYU-MAGICC/flight_testing

Flight testing

cfg/Controller.cfg

@@ -14,14 +14,14 @@ trim.add("TRIM_T", double_t, 0, "Throttle trim", 0.6, 0, 1)
 
 # course hold
 course = gen.add_group("Course")
-course.add("COURSE_KP", double_t, 0, "Course proportional gain", 0.7329, -10, 10)
-course.add("COURSE_KD", double_t, 0, "Course derivative gain", 0, -10, 10)
-course.add("COURSE_KI", double_t, 0, "Course integral gain", 0.07, -10, 10)
+course.add("COURSE_KP", double_t, 0, "Course proportional gain", 0.7329, 0, 5)
+course.add("COURSE_KD", double_t, 0, "Course derivative gain", 0, -5, 0)
+course.add("COURSE_KI", double_t, 0, "Course integral gain", 0.0, 0, 5)
 
 # roll hold
 roll = gen.add_group("Roll")
 roll.add("ROLL_KP", double_t, 0, "Roll proportional gain", 1.2855, -10, 10)
-roll.add("ROLL_KD", double_t, 0, "Roll derivative gain", -0.325, -10, 10)
+roll.add("ROLL_KD", double_t, 0, "Roll derivative gain", -0.1, -10, 10)
 roll.add("ROLL_KI", double_t, 0, "Roll integral gain", 0, -10, 10)
 
 # pitch hold

cfg/Follower.cfg

@@ -12,6 +12,6 @@ gen.add("CHI_INFTY", double_t, 0, "Chi Infinity", 1.0472, 0 , 1.5708)
 gen.add("K_PATH", double_t, 0, "K Path", 0.025, 0, 1)
 
 # K Orbit
-gen.add("K_ORBIT", double_t, 0, "K Orbit", 8.0, 0, 15)
+gen.add("K_ORBIT", double_t, 0, "K Orbit", 2.0, 0, 15)
 
 exit(gen.generate(PACKAGE, "ros_plane", "Follower"))

launch/fly.launch

@@ -0,0 +1,34 @@
+<launch>
+
+ <rosparam subst_value="True">
+  naze:           { imu_pub_rate: 100.0,
+                    rc_send_rate: 50.0,
+                    timeout: 20 }
+
+ </rosparam>  
+
+ <rosparam command="load" file="$(find fcu_io)/param/rc.yaml"/>
+
+ <node name="naze"  pkg="fcu_io"  type="fcu_io_node" output="screen">
+ 	<param name="serial_port" value="/dev/ttyUSB1"/>
+ </node>
+
+
+ <node name="estimator" pkg="ros_plane" type="ros_plane_estimator" output="screen"/>
+
+ <node name="controller" pkg="ros_plane" type="ros_plane_controller">
+	<param name="PWM_RAD_E" value="2.8"/>
+	<param name="PWM_RAD_A" value="-1.6"/>
+	<param name="PWM_RAD_R" value="1.8"/>
+ </node>
+ <node name="path_follower" pkg="ros_plane" type="ros_plane_path_follower"/>
+ <!-- GPS -->
+
+ <node name="gps_driver" pkg="fcu_common" type="gps">
+	<param name="port" value="/dev/ttyUSB0"/>
+	<param name="baud" value="57600"/>
+ </node>
+
+</launch>
+
+

src/controller_base.cpp

@@ -17,8 +17,8 @@ controller_base::controller_base():
     nh_private_.param<double>("TRIM_A", _params.trim_a, 0.0);
     nh_private_.param<double>("TRIM_R", _params.trim_r, 0.0);
     nh_private_.param<double>("TRIM_T", _params.trim_t, 0.6);
-    nh_private_.param<double>("PWM_RAD_E", _params.pwm_rad_e, 1.0);
-    nh_private_.param<double>("PWM_RAD_A", _params.pwm_rad_a, 1.0);
+    nh_private_.param<double>("PWM_RAD_E", _params.pwm_rad_e, 2.3);
+    nh_private_.param<double>("PWM_RAD_A", _params.pwm_rad_a, -1.6);
     nh_private_.param<double>("PWM_RAD_R", _params.pwm_rad_r, 1.0);
     nh_private_.param<double>("ALT_TOZ", _params.alt_toz, 20.0);
     nh_private_.param<double>("ALT_HZ", _params.alt_hz, 10.0);
@@ -142,7 +142,7 @@ void controller_base::actuator_controls_publish(const ros::TimerEvent&)
         /* publish actuator controls */
 
         actuators.normalized_roll = output.delta_a;//(isfinite(output.delta_a)) ? output.delta_a : 0.0f;
-        actuators.normalized_pitch = -1.0*output.delta_e;//(isfinite(output.delta_e)) ? output.delta_e : 0.0f;
+        actuators.normalized_pitch = output.delta_e;//(isfinite(output.delta_e)) ? output.delta_e : 0.0f;
         actuators.normalized_yaw = output.delta_r;//(isfinite(output.delta_r)) ? output.delta_r : 0.0f;
         actuators.normalized_throttle = output.delta_t;//(isfinite(output.delta_t)) ? output.delta_t : 0.0f;
 

src/estimator_base.cpp

@@ -9,7 +9,7 @@ estimator_base::estimator_base():
 {
     nh_private_.param<std::string>("gps_topic", gps_topic_, "/gps/data");
     nh_private_.param<std::string>("imu_topic", imu_topic_, "/imu/data");
-    nh_private_.param<std::string>("baro_topic", baro_topic_, "/baro/data");
+    nh_private_.param<std::string>("baro_topic", baro_topic_, "/baro/alt");
     nh_private_.param<std::string>("airspeed_topic", airspeed_topic_, "/airspeed/data");
     nh_private_.param<double>("update_rate", update_rate_, 100.0);
     params_.Ts = 1.0f/update_rate_;

src/estimator_example.cpp

@@ -66,7 +66,7 @@ void estimator_example::estimate(const params_s &params, const input_s &input, o
         R_p(5,5) = 0.001;
 
         float lpf_a = 50;
-        float lpf_a1 = 8;
+        float lpf_a1 = 2.0;
         alpha = exp(-lpf_a*params.Ts);
         alpha1 = exp(-lpf_a1*params.Ts);
     }
@@ -95,11 +95,11 @@ void estimator_example::estimate(const params_s &params, const input_s &input, o
 //        ROS_WARN("problem 20");
 //        hhat = 10;
 //    }
-//    if(!std::isfinite(Vahat) || Vahat < 0 || Vahat > 25)
-//    {
+    if(!std::isfinite(Vahat) || Vahat <= 0 || Vahat > 25)
+    {
 //        ROS_WARN("problem 21");
-//        Vahat = 9;
-//    }
+        Vahat = 9;
+    }
 
     // low pass filter accelerometers
     lpf_accel_x = alpha*lpf_accel_x + (1-alpha)*input.accel_x;
@@ -248,7 +248,9 @@ void estimator_example::estimate(const params_s &params, const input_s &input, o
         xhat_p = xhat_p + L_p*(input.gps_Vg - h_p);
 
         // gps course
-        //wrap course measurement
+        if(input.gps_Vg > 1)
+	{
+	//wrap course measurement
         float gps_course = fmodf(input.gps_course, radians(360.0f));
 
         while(gps_course - xhat_p(3) > radians(180.0f)) gps_course = gps_course - radians(360.0f);
@@ -259,7 +261,7 @@ void estimator_example::estimate(const params_s &params, const input_s &input, o
         L_p = (P_p*C_p) / (R_p(3,3) + (C_p.transpose()*P_p*C_p));
         P_p = (I_p - L_p*C_p.transpose())*P_p;
         xhat_p = xhat_p + L_p*(gps_course - h_p);
-
+	}
 //        // pseudo measurement #1 y_1 = Va*cos(psi)+wn-Vg*cos(chi)
 //        h_p = Vahat*cosf(xhat_p(6)) + xhat_p(4) - xhat_p(2)*cosf(xhat_p(3));  // pseudo measurement
 //        C_p = Eigen::VectorXf::Zero(7);