//////////////////////////////////////////////////////////////////////////////// // 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); }