Altitude Control PID

[mohradza]

Hello!

I have been doing some development on your roscopter controller, and I am encountering a weird response from the PID_w_.computePID() function in line 225 of the controller.cpp script. It is always returning a value of zero. I doubt this is an "issue" on your end, but anyone has any ideas on what I am missing or have misconfigured, I would appreciate any and all feedback!

A little background: We have a good height and vertical velocity estimator using only onboard sensors and no gps, which I would like to plug into an altitude controller. I am not concerned with controlling x or y motion, so what I have done is the following:

• I have a script which publishes the high level command which has the ignore bit mask set to ignore x, y, and z, and F is set to our desired agl height of -1.5m. The command mode is set to XVEL_YVEL_YAWRATE_ALTITUDE mode, which should cause the XPOS_YPOS_YAW_ALTITUDE if statement (lines 190-215) to be skipped over.
• I edited the stateCallback function to a set xhat_.pdand xhat_.w as our height and vertical velocity estimates respectively. I have zeroed out all of the other states and completely commented out the entire pose / angular velocity section. I thought this was justifiable, as none of these states are used in the altitude controller.
• I changed the line which sets pddot (line 225) such that it is directly set to our incoming velocity estimate. (double pddot = xhat_.w)
• I commented out lines 219 - 222 because I am not concerned with controlling motion in the x or y direction.
• I temporarily removed the saturate function to see if it was computing wild values inside the PID controller, and it still outputs 0.

With all of these changes made, I would expect the computeControl function to skip straight to line 225 to set pddot = xhat_.w, and then move into the PID_w_.computePID() in line 226. After all these changes, the value pddot_c is always zero, and so is xc_.az. I have verified from the output of ROS_INFO("pddot = %f, pddot_c = %f, az_c = %f", pddot, pddot_c, xc_.az); that pddot is nonzero, and I added in xhat_.pd as well to confirm that it was also reading the correct values.

I know I have made some major changes to the code, but I was hoping someone might be able to point me in the right direction for debugging this issue. My next step is to modify the computePID function in the controller class to print out some of the intermediate values before the final output "u", but if anyone thinks the bug is coming from the changes I made, I would sincerely appreciate some feedback!!

Thank you! -Mohradza

[mohradza]

I figured it out. I was not loading the parameter list, which was causing all the PID gains to default to 0.