public void addCommandToQueue(MotorCommand command)
{
MotorPWM pwm = new MotorPWM();
pwm.header = command.header;
pwm.pwm = new byte[command.voltage.Length];
for (int i = 0; i < command.voltage.Length; ++i)
{
byte temp = (byte)Math.Round(command.voltage [i] / supply_.voltage [0] * 255.0);
if (temp < 0)
{
pwm.pwm [i] = 0;
}
else
if (temp > 255)
{
pwm.pwm [i] = 255;
}
else
{
pwm.pwm [i] = temp;
}
}
addPWMToQueue(pwm);
}
public void addPWMToQueue(MotorPWM pwm)
{
lock ( command_queue_mutex_ )
{
if (!motor_status_.on)
{
ROS.Warn("quadrotor_propulsion", "Received new motor command. Enabled motors.");
engage();
}
ROS.Debug("quadrotor_propulsion", "Received motor command valid at " + pwm.header.Stamp.ToString());
command_queue_.Enqueue(pwm);
command_condition_.Set();
}
}
public void setVoltage(MotorPWM voltage)
{
lock ( mutex_ )
{
last_command_time_ = voltage.header.Stamp;
if (motor_status_.on && voltage.pwm.Length >= 4)
{
propulsion_model_.u [6] = voltage.pwm [0] * supply_.voltage [0] / 255.0;
propulsion_model_.u [7] = voltage.pwm [1] * supply_.voltage [0] / 255.0;
propulsion_model_.u [8] = voltage.pwm [2] * supply_.voltage [0] / 255.0;
propulsion_model_.u [9] = voltage.pwm [3] * supply_.voltage [0] / 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;
}
}
}
public bool processQueue(Time timestamp, Duration tolerance, Duration delay, Duration wait, CallbackQueue callback_queue)
{
bool new_command = false;
lock ( command_queue_mutex_ )
{
Time min = new Time(timestamp.data);
Time max = new Time(timestamp.data);
try {
min = timestamp - delay - tolerance /* - ros::Duration(dt) */;
} catch (Exception e) {}
try {
max = timestamp - delay + tolerance;
} catch (Exception e) {}
do
{
while (command_queue_.Count > 0)
{
MotorPWM new_motor_voltage = command_queue_.Peek();
Time new_time = new_motor_voltage.header.Stamp;
if (new_time.data.toSec() == 0.0 || (new_time >= min && new_time <= max))
{
setVoltage(new_motor_voltage);
command_queue_.Dequeue();
new_command = true;
// new motor command is too old:
}
else if (new_time < min)
{
ROS.Debug("quadrotor_propulsion", "Command received was %fs too old. Discarding.", (new_time - timestamp).data.toSec());
command_queue_.Dequeue();
// new motor command is too new:
}
else
{
break;
}
}
if (command_queue_.Count == 0 && !new_command)
{
if (!motor_status_.on || wait.data.toSec() == 0.0)
{
break;
}
ROS.Debug("quadrotor_propulsion", "Waiting for command at simulation step t = %fs... last update was %fs ago", timestamp.data.toSec(), (timestamp - last_command_time_).data.toSec());
if (callback_queue == null)
{
if (command_condition_.WaitOne((int)(wait.data.toSec() * 1000)))
{
continue;
}
// if (command_condition_.timed_wait(lock, wait.toBoost())) continue;
}
else
{
command_condition_.Set();
callback_queue.callAvailable((int)(wait.toSec() * 1000));
command_condition_.Reset();
if (command_queue_.Count > 0)
{
continue;
}
}
ROS.Error("quadrotor_propulsion", "Command timed out. Disabled motors.");
shutdown();
}
} while (false);
if (new_command)
{
// ROS.Debug ("quadrotor_propulsion", "Using motor command valid at t = " << last_command_time_.toSec() << "s for simulation step at t = " << timestamp.toSec() << "s (dt = " << (timestamp - last_command_time_).toSec() << "s)");
}
}
return(new_command);
}
