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