Revision 02975ec6
| ID | 02975ec6a7e42ce76c44acd0de4c89954a7bc914 |
Changed goal in altitude_node to be current height when enabled.
| quad2/altitude_node/src/altitude_node.cpp | ||
|---|---|---|
| 18 | 18 |
|
| 19 | 19 |
private: |
| 20 | 20 |
bool enabled; |
| 21 |
float last_height; |
|
| 21 | 22 |
PID_control pid; |
| 22 | 23 |
ros::NodeHandle n; |
| 23 | 24 |
ros::Publisher pub; |
| ... | ... | |
| 29 | 30 |
ros::Subscriber enable_sub; |
| 30 | 31 |
}; |
| 31 | 32 |
|
| 32 |
AltitudeNode::AltitudeNode() : pid(0, 0, 0, 0), enabled(false) |
|
| 33 |
AltitudeNode::AltitudeNode() : pid(0, 0, 0, 0), enabled(false), last_height(0)
|
|
| 33 | 34 |
{
|
| 34 | 35 |
height_sub = n.subscribe<mikrokopter::FcDebug>("/mikrokopter/fc_debug", 100,
|
| 35 | 36 |
&AltitudeNode::height_cb, this); |
| ... | ... | |
| 45 | 46 |
&AltitudeNode::enable_cb, this); |
| 46 | 47 |
pub = n.advertise<std_msgs::Float64>("/mikrokopter/thrust", 100);
|
| 47 | 48 |
|
| 48 |
double default_goal; |
|
| 49 |
/*double default_goal;
|
|
| 49 | 50 |
ros::param::param<double>("~default_goal", default_goal, 0);
|
| 50 |
pid.change_goal(default_goal); |
|
| 51 |
pid.change_goal(default_goal);*/
|
|
| 51 | 52 |
} |
| 52 | 53 |
|
| 53 | 54 |
void AltitudeNode::height_cb(const mikrokopter::FcDebug::ConstPtr& fc) |
| 54 | 55 |
{
|
| 56 |
last_height = fc->heightValue; |
|
| 55 | 57 |
if (enabled) |
| 56 | 58 |
{
|
| 57 | 59 |
std_msgs::Float64 msg; |
| ... | ... | |
| 89 | 91 |
enabled = msg->data; |
| 90 | 92 |
if (enabled) |
| 91 | 93 |
{
|
| 94 |
pid.change_goal(last_height); |
|
| 92 | 95 |
ROS_INFO("Altitude Hold: Enabled.");
|
| 96 |
ROS_INFO("Altitude: %f", last_height);
|
|
| 93 | 97 |
} |
| 94 | 98 |
else |
| 95 | 99 |
{
|
Also available in: Unified diff