control ne test

zxf545168815 · Mar 8, 2021 · 16f2577 · 16f2577
1 parent 63d7aa9
commit 16f2577
Show file tree

Hide file tree

Showing 2 changed files with 10 additions and 1 deletion.
diff --git a/Modules/control/include/Position_Controller/pos_controller_cascade_PID.h b/Modules/control/include/Position_Controller/pos_controller_cascade_PID.h
@@ -193,6 +193,14 @@ void pos_controller_cascade_PID::_positionController
     {
         vel_setpoint[2] = _Reference_State.velocity_ref[2];
     }
+
+    // Only for controller test
+    if(_Reference_State.Move_mode == prometheus_msgs::PositionReference::TRAJECTORY)
+    {
+        vel_setpoint[0] =  Kp_xy * (_Reference_State.position_ref[0] - _DroneState.position[0]);
+        vel_setpoint[1] =  Kp_xy * (_Reference_State.position_ref[1] - _DroneState.position[1]);
+        vel_setpoint[2] =  Kp_z  * (_Reference_State.position_ref[2] - _DroneState.position[2]);
+    }
 
     // Limit the velocity setpoint
     vel_setpoint[0] = constrain_function(vel_setpoint[0], MPC_XY_VEL_MAX);

diff --git a/Modules/control/src/px4_pos_controller.cpp b/Modules/control/src/px4_pos_controller.cpp
@@ -414,7 +414,8 @@ int main(int argc, char **argv)
                     _ControlOutput = pos_controller_cascade_pid.pos_controller(_DroneState, Command_Now.Reference_State, dt);
                 }else
                 {
-                    _ControlOutput = pos_controller_pid.pos_controller(_DroneState, Command_Now.Reference_State, dt);
+                    _ControlOutput = pos_controller_cascade_pid.pos_controller(_DroneState, Command_Now.Reference_State, dt);
+                    // _ControlOutput = pos_controller_pid.pos_controller(_DroneState, Command_Now.Reference_State, dt);
                     pub_message(message_pub, prometheus_msgs::Message::WARN, NODE_NAME, "CPID NOT SUPPOORT TRAJECTORY TRACKING.");
                 }