/// <summary>Flight Computer constructor.</summary>
/// <param name="s">A signal processor (most probably a <see cref="ModuleSPU"/> instance.)</param>
public FlightComputer(ISignalProcessor s)
{
SignalProcessor = s;
Vessel = s.Vessel;
SanctionedPilots = new List <Action <FlightCtrlState> >();
LastTarget = TargetCommand.WithTarget(null);
var attitude = AttitudeCommand.Off();
_activeCommands[attitude.Priority] = attitude;
//Use http://www.ni.com/white-paper/3782/en/ to fine-tune
PIDController = new PIDController(PIDKp, PIDKi, PIDKd, 1.0, -1.0, true);
PIDController.SetVessel(Vessel);
GameEvents.onVesselChange.Add(OnVesselChange);
GameEvents.onVesselSwitching.Add(OnVesselSwitching);
GameEvents.onGameSceneSwitchRequested.Add(OnSceneSwitchRequested);
RoverComputer = new RoverComputer(this, RoverPIDKp, RoverPIDKi, RoverPIDKd);
RoverComputer.SetVessel(Vessel);
// Add RT listeners from KSP Autopilot
StockAutopilotCommand.UIreference = GameObject.FindObjectOfType <VesselAutopilotUI>();
for (var index = 0; index < StockAutopilotCommand.UIreference.modeButtons.Length; index++)
{
var buttonIndex = index; // prevent compiler optimisation from assigning static final index value
StockAutopilotCommand.UIreference.modeButtons[index].onClick.AddListener(delegate { StockAutopilotCommand.AutopilotButtonClick(buttonIndex, this); });
// bad idea to use RemoveAllListeners() since no easy way to re-add the original stock listener to onClick
}
}
void attitudeCommandCb (AttitudeCommand command)
{
lock ( command_mutex_ )
{
attitude_command_ = command;
if ( attitude_command_.header.Stamp.data.toSec () == 0.0 )
{
attitude_command_.header.Stamp = ROS.GetTime ();
}
}
}
public AttitudeSubscriberHelper (NodeHandle nh, object command_mutex, AttitudeCommand attitude_command, YawRateCommand yawrate_command, ThrustCommand thrust_command)
{
command_mutex_ = command_mutex;
attitude_command_ = attitude_command;
yawrate_command_ = yawrate_command;
thrust_command_ = thrust_command;
attitude_subscriber_ = nh.subscribe<AttitudeCommand> ( "attitude", 1, attitudeCommandCb );
yawrate_subscriber_ = nh.subscribe<YawRateCommand> ( "yawrate", 1, yawrateCommandCb );
thrust_subscriber_ = nh.subscribe<ThrustCommand> ( "thrust", 1, thrustCommandCb );
}
private void Confirm()
{
ICommand newCommand;
switch (mMode)
{
default:
case ComputerMode.Off:
mAttitude = FlightAttitude.Null;
newCommand = AttitudeCommand.Off();
break;
case ComputerMode.Kill:
mAttitude = FlightAttitude.Null;
newCommand = AttitudeCommand.KillRot();
break;
case ComputerMode.Node:
mAttitude = FlightAttitude.Null;
newCommand = AttitudeCommand.ManeuverNode();
break;
case ComputerMode.TargetPos:
mAttitude = (mAttitude == FlightAttitude.Null) ? FlightAttitude.Prograde : mAttitude;
newCommand =
AttitudeCommand.WithAttitude(Attitude, ReferenceFrame.TargetParallel);
break;
case ComputerMode.Orbital:
mAttitude = (mAttitude == FlightAttitude.Null) ? FlightAttitude.Prograde : mAttitude;
newCommand =
AttitudeCommand.WithAttitude(Attitude, ReferenceFrame.Orbit);
break;
case ComputerMode.Surface:
mAttitude = (mAttitude == FlightAttitude.Null) ? FlightAttitude.Prograde : mAttitude;
newCommand =
AttitudeCommand.WithAttitude(Attitude, ReferenceFrame.Surface);
break;
case ComputerMode.TargetVel:
mAttitude = (mAttitude == FlightAttitude.Null) ? FlightAttitude.Prograde : mAttitude;
newCommand =
AttitudeCommand.WithAttitude(Attitude, ReferenceFrame.TargetVelocity);
break;
case ComputerMode.Custom:
mAttitude = FlightAttitude.Null;
newCommand = AttitudeCommand.WithSurface(Pitch, Heading, Roll);
break;
}
mFlightComputer.Enqueue(newCommand);
}
public AttitudeCommand limit (AttitudeCommand input)
{
AttitudeCommand output = new AttitudeCommand ();
output.header = input.header;
output.roll = (float) roll.limit ( input.roll );
output.pitch = (float) pitch.limit ( input.pitch );
double absolute_value = System.Math.Sqrt ( output.roll * output.roll + output.pitch * output.pitch );
if ( absolute_value > absolute_max )
{
output.roll *= (float) ( absolute_max / absolute_value );
output.pitch *= (float) ( absolute_max / absolute_value );
}
return output;
}
public FlightComputer(ISignalProcessor s)
{
SignalProcessor = s;
Vessel = s.Vessel;
SanctionedPilots = new List <Action <FlightCtrlState> >();
pid = new PIDControllerV2(0, 0, 0, 1, -1);
initPIDParameters();
lastAct = Vector3d.zero;
var attitude = AttitudeCommand.Off();
mActiveCommands[attitude.Priority] = attitude;
GameEvents.onVesselChange.Add(OnVesselChange);
mRoverComputer = new RoverComputer();
mRoverComputer.SetVessel(Vessel);
}
/// <summary>Flight Computer constructor.</summary>
/// <param name="s">A signal processor (most probably a <see cref="ModuleSPU"/> instance.)</param>
public FlightComputer(ISignalProcessor s)
{
SignalProcessor = s;
Vessel = s.Vessel;
SanctionedPilots = new List <Action <FlightCtrlState> >();
pid = new PIDControllerV3(Vector3d.zero, Vector3d.zero, Vector3d.zero, 1, -1);
setPIDParameters();
lastAct = Vector3d.zero;
LastTarget = TargetCommand.WithTarget(null);
var attitude = AttitudeCommand.Off();
_activeCommands[attitude.Priority] = attitude;
GameEvents.onVesselChange.Add(OnVesselChange);
GameEvents.onVesselSwitching.Add(OnVesselSwitching);
GameEvents.onGameSceneSwitchRequested.Add(OnSceneSwitchRequested);
RoverComputer = new RoverComputer();
RoverComputer.SetVessel(Vessel);
}
public void joyAttitudeCallback(Joy joy)
{
AttitudeCommand attitude = new AttitudeCommand();
ThrustCommand thrust = new ThrustCommand();
YawRateCommand yawrate = new YawRateCommand();
attitude.header.Stamp = thrust.header.Stamp = yawrate.header.Stamp = ROS.GetTime();
attitude.header.Frame_id = yawrate.header.Frame_id = baseStabilizedFrame;
thrust.header.Frame_id = baseLinkFrame;
attitude.roll = (float)(-getAxis(joy, sAxes.y) * Math.PI / 180.0);
attitude.pitch = (float)(getAxis(joy, sAxes.x) * Math.PI / 180.0);
if (getButton(joy, sButtons.slow))
{
attitude.roll *= (float)slowFactor;
attitude.pitch *= (float)slowFactor;
}
attitudePublisher.publish(attitude);
thrust.thrust = (float)getAxis(joy, sAxes.thrust);
thrustPublisher.publish(thrust);
yawrate.turnrate = (float)(getAxis(joy, sAxes.yaw) * Math.PI / 180.0);
if (getButton(joy, sButtons.slow))
{
yawrate.turnrate *= (float)slowFactor;
}
yawRatePublisher.publish(yawrate);
if (getButton(joy, sButtons.stop))
{
enableMotors(false);
}
else if (getButton(joy, sButtons.go))
{
enableMotors(true);
}
}
