////////////////////////////////////////////////////////////////////////////////
// Update the controller
void GazeboQuadrotorAerodynamics::Update()
{
Vector3 force, torque;
boost::mutex::scoped_lock lock(command_mutex_);
// Get simulator time
Time current_time = world->GetSimTime();
double dt = (current_time - last_time_).Double();
last_time_ = current_time;
if (dt <= 0.0) return;
// Get new commands/state
// callback_queue_.callAvailable();
while(1) {
if (!new_motor_voltages_.empty()) {
hector_uav_msgs::MotorPWMConstPtr new_motor_voltage = new_motor_voltages_.front();
Time new_time = Time(new_motor_voltage->header.stamp.sec, new_motor_voltage->header.stamp.nsec);
if (new_time == Time() || (new_time >= current_time - control_delay_ - control_tolerance_ && new_time <= current_time - control_delay_ + control_tolerance_)) {
motor_voltage_ = new_motor_voltage;
new_motor_voltages_.pop_front();
last_control_time_ = current_time - control_delay_;
ROS_DEBUG_STREAM("Using motor command valid at " << new_time << " for simulation step at " << current_time << " (dt = " << (current_time - new_time) << ")");
break;
} else if (new_time < current_time - control_delay_ - control_tolerance_) {
ROS_DEBUG("[quadrotor_aerodynamics] command received was too old: %f s", (new_time - current_time).Double());
new_motor_voltages_.pop_front();
continue;
}
}
if (new_motor_voltages_.empty() && motor_status_.on && control_period_ > 0 && current_time > last_control_time_ + control_period_ + control_tolerance_) {
ROS_WARN("[quadrotor_aerodynamics] waiting for command...");
if (command_condition_.timed_wait(lock, (ros::Duration(control_period_.sec, control_period_.nsec) * 100.0).toBoost())) continue;
ROS_ERROR("[quadrotor_aerodynamics] command timed out.");
motor_status_.on = false;
}
break;
}
// fill input vector u for propulsion model
velocity = model->GetRelativeLinearVel();
rate = model->GetRelativeAngularVel();
propulsion_model_->u[0] = velocity.x;
propulsion_model_->u[1] = -velocity.y;
propulsion_model_->u[2] = -velocity.z;
propulsion_model_->u[3] = rate.x;
propulsion_model_->u[4] = -rate.y;
propulsion_model_->u[5] = -rate.z;
if (motor_voltage_ && motor_voltage_->pwm.size() >= 4) {
propulsion_model_->u[6] = motor_voltage_->pwm[0] * 14.8 / 255.0;
propulsion_model_->u[7] = motor_voltage_->pwm[1] * 14.8 / 255.0;
propulsion_model_->u[8] = motor_voltage_->pwm[2] * 14.8 / 255.0;
propulsion_model_->u[9] = motor_voltage_->pwm[3] * 14.8 / 255.0;
} else {
propulsion_model_->u[6] = 0.0;
propulsion_model_->u[7] = 0.0;
propulsion_model_->u[8] = 0.0;
propulsion_model_->u[9] = 0.0;
}
// std::cout << "u = [ ";
// for(std::size_t i = 0; i < propulsion_model_->u.size(); ++i)
// std::cout << propulsion_model_->u[i] << " ";
// std::cout << "]" << std::endl;
checknan(propulsion_model_->u, "propulsion model input");
checknan(propulsion_model_->x, "propulsion model state");
// update propulsion model
quadrotorPropulsion(propulsion_model_->x.data(), propulsion_model_->u.data(), propulsion_model_->parameters_, dt, propulsion_model_->y.data(), propulsion_model_->x_pred.data());
propulsion_model_->x = propulsion_model_->x_pred;
force = force + checknan(Vector3(propulsion_model_->y[0], -propulsion_model_->y[1], propulsion_model_->y[2]), "propulsion model force");
torque = torque + checknan(Vector3(propulsion_model_->y[3], -propulsion_model_->y[4], -propulsion_model_->y[5]), "propulsion model torque");
// std::cout << "y = [ ";
// for(std::size_t i = 0; i < propulsion_model_->y.size(); ++i)
// std::cout << propulsion_model_->y[i] << " ";
// std::cout << "]" << std::endl;
// publish wrench
if (wrench_publisher_) {
wrench_.force.x = force.x;
wrench_.force.y = force.y;
wrench_.force.z = force.z;
wrench_.torque.x = torque.x;
wrench_.torque.y = torque.y;
wrench_.torque.z = torque.z;
wrench_publisher_.publish(wrench_);
}
// fill input vector u for drag model
drag_model_->u[0] = (velocity.x - wind_.x);
drag_model_->u[1] = -(velocity.y - wind_.y);
drag_model_->u[2] = -(velocity.z - wind_.z);
drag_model_->u[3] = rate.x;
drag_model_->u[4] = -rate.y;
drag_model_->u[5] = -rate.z;
// std::cout << "u = [ ";
// for(std::size_t i = 0; i < drag_model_->u.size(); ++i)
// std::cout << drag_model_->u[i] << " ";
// std::cout << "]" << std::endl;
checknan(drag_model_->u, "drag model input");
// update drag model
quadrotorDrag(drag_model_->u.data(), drag_model_->parameters_, dt, drag_model_->y.data());
force = force - checknan(Vector3(drag_model_->y[0], -drag_model_->y[1], -drag_model_->y[2]), "drag model force");
torque = torque - checknan(Vector3(drag_model_->y[3], -drag_model_->y[4], -drag_model_->y[5]), "drag model torque");
if (motor_status_publisher_ && current_time >= last_motor_status_time_ + control_period_) {
motor_status_.header.stamp = ros::Time(current_time.sec, current_time.nsec);
motor_status_.running = propulsion_model_->x[0] > 0.0 && propulsion_model_->x[1] > 0.0 && propulsion_model_->x[2] > 0.0 && propulsion_model_->x[3] > 0.0;
motor_status_.frequency.resize(4);
motor_status_.frequency[0] = propulsion_model_->x[0];
motor_status_.frequency[1] = propulsion_model_->x[1];
motor_status_.frequency[2] = propulsion_model_->x[2];
motor_status_.frequency[3] = propulsion_model_->x[3];
motor_status_publisher_.publish(motor_status_);
last_motor_status_time_ = current_time;
}
// set force and torque in gazebo
model->GetLink()->AddRelativeForce(force);
model->GetLink()->AddRelativeTorque(torque);
}
inline void QuadrotorPropulsion::f(const double xin[4], const double uin[10], double dt, double y[14], double xpred[4]) const
{
quadrotorPropulsion(xin, uin, propulsion_model_->parameters_, dt, y, xpred);
}
