/****************************************************************************/ /* Copyright (c) 2000 MBARI */ /* MBARI Proprietary Information. All rights reserved. */ /****************************************************************************/ /* Summary : */ /* Filename : OdysseyTailCone.cc */ /* Author : */ /* Project : */ /* Version : 1.0 */ /* Created : 02/07/2000 */ /* Modified : */ /* Archived : */ /****************************************************************************/ /* Modification History: */ /****************************************************************************/ #include #include "OdysseyTailCone.h" #include "VehicleConfigurationIF.h" #include "Math.h" #include "ourTypes.h" #include "Time.h" #include "Syslog.h" #include "SerialDevice.h" #include "FloatAttribute.h" #include "IntegerAttribute.h" #include "BooleanAttribute.h" #include "System.h" #include "AttributeParser.h" #define ThrusterTimeout 4 OdysseyTailCone::OdysseyTailCone(SerialParameters *serialParams) : _attributes("odysseyTailCone") { // This is not a true server so ... if (qnx_pflags(0, _PPF_PRIORITY_REC | _PPF_PRIORITY_FLOAT, 0, 0) == -1) { Syslog::write("OdysseyTailCone::OdysseyTailCone() - qnx_pflags(): %s", strerror(errno)); exit (1); } // Set task priority if (System::setTaskPriority(TailconeDriverPriority) == -1) exit (1); Boolean debug = False; _thruster = 0; _rudder = 0; _elevator = 0; _serialDevice = 0; _rudderCmd = _elevatorCmd = _speedCmd = 1.e9; Boolean rezeroFins; _attributes.add(new FloatAttribute("rudderCtsPerRad", "Rudder encoder counts per radian", &_rudderCtsPerRad, 1169.31)); _attributes.add(new FloatAttribute("elevatorCtsPerRad", "Elevator encoder counts per radian", &_elevatorCtsPerRad, -1169.31)); _attributes.add(new BooleanAttribute("rezeroFins", "Rezero fins using Hall sensors", &rezeroFins)); _attributes.add(new IntegerAttribute("thrusterTimeout", "Thruster serial timeoout" ,&_thrusterTimeout,2500)); _attributes.add(new BooleanAttribute("enableDeadman", "Thruster deadman enabled at init", &_enableThrusterDeadman, True)); AttributeParser::parse(System::configurationFile("odysseyTailCone.cfg"), &_attributes); dprintf("Create serial device"); _serialDevice = new SerialDevice(serialParams); _serialDevice->setLineFormat(9600, 8, 1, "NONE"); dprintf("done"); VehicleConfigurationIF config("config"); _effDragCoeff = config.effDragCoef(); _propPitch = config.propPitch(); dprintf("OdysseyTailCone::OdysseyTailCone() - create thruster object"); _thruster = new OdysseyThruster(_serialDevice,(int)_thrusterTimeout); dprintf("OdysseyTailCone::OdysseyTailCone() - create elevator object"); _elevator = new OdysseyFin("elevator", _serialDevice, OdysseyFin::ElevatorID, _elevatorCtsPerRad, rezeroFins); dprintf("OdysseyTailCone::OdysseyTailCone() - create rudder object"); _rudder = new OdysseyFin("rudder", _serialDevice, OdysseyFin::RudderID, _rudderCtsPerRad, rezeroFins); _motorCurrent = 0.; } OdysseyTailCone::~OdysseyTailCone() { setPropellerSpeed(0.); delete _thruster; delete _rudder; delete _elevator; delete _serialDevice; } void OdysseyTailCone::name(DeviceIF::Name name) { strcpy(name, "OdysseyTailCone"); } void OdysseyTailCone::serialNumber(DeviceIF::Name number) { strcpy(number, "XXX"); } DeviceIF::Status OdysseyTailCone::powerOn() { // Dummy implementation for now return DeviceIF::Ok; } DeviceIF::Status OdysseyTailCone::powerOff() { // Dummy implementation for now return DeviceIF::Ok; } DeviceIF::Status OdysseyTailCone::setPropellerSpeed(double propSpeedCmd) { Boolean debug = False; double vehicleSpeed; if (!_thruster->enabled() && propSpeedCmd != 0.) { // Thruster has been disabled and requested speed is non-zero setStatus(DeviceIF::Offline); return status(); } // // Convert from propeller speed to vehicle speed. This is temporary and // will be replaced with a calculation that doesn't involve vehicle speed. // vehicleSpeed = propSpeedCmd * _propPitch; _motorCurrent = speedToCurrent(vehicleSpeed); _speedCmd = vehicleSpeed; dprintf("OdysseyTailCone::setPropellerSpeed() - prop speed=%.5f, curr=%.5f", propSpeedCmd, _motorCurrent); DeviceIF::Status newStatus = _thruster->control(_motorCurrent); setStatus(newStatus); return status(); } DeviceIF::Status OdysseyTailCone::setElevator(double angle) { // if (angle == _elevatorCmd) // return DeviceIF::Ok; _elevatorCmd = angle; DeviceIF::Status newStatus = _elevator->control(angle); setStatus(newStatus); return status(); } DeviceIF::Status OdysseyTailCone::setRudder(double angle) { // if (angle == _rudderCmd) // return DeviceIF::Ok; // Store latest rudder command _rudderCmd = angle; DeviceIF::Status newStatus = _rudder->control(angle); setStatus(newStatus); return status(); } DeviceIF::Status OdysseyTailCone::command(double propSpeedCmd, double elevator, double rudder) { Boolean debug = False; setStatus(DeviceIF::Ok); DeviceIF::Status newStatus; dprintf("OdysseyTailCone::command() - setRudder(%f)", rudder); if ((newStatus = (DeviceIF::Status )setRudder(rudder)) != DeviceIF::Ok) { dprintf("OdysseyTailCone::command() - status %d from setRudder()", status); setStatus(newStatus); } dprintf("OdysseyTailCone::command() - setElevator(%f)", elevator); if ((newStatus = (DeviceIF::Status )setElevator(elevator)) != DeviceIF::Ok) { dprintf("OdysseyTailCone::command() - status %d from setElevator()", status); setStatus(newStatus); } dprintf("OdysseyTailCone::command() - setPropellerSpeed(%f)", propSpeedCmd); if ((newStatus = (DeviceIF::Status )setPropellerSpeed(propSpeedCmd)) != DeviceIF::Ok) { dprintf("OdysseyTailCone::command() - status %d from setPropellerSpeed()", status); setStatus(newStatus); } return status(); } DeviceIF::Status OdysseyTailCone::initialize() { Boolean debug = False; Boolean error = False; DeviceIF::Status newStatus; if ((newStatus = (DeviceIF::Status )initThruster()) != DeviceIF::Ok) { dprintf("OdysseyTailCone::initialize() - initThruster() status = %d", status); error = True; } dprintf("initRudder()"); if ((newStatus = (DeviceIF::Status )initRudder()) != DeviceIF::Ok) { dprintf("OdysseyTailCone::initialize() - initRudder() status = %d", status); error = True; } dprintf("initElevator()"); if ((newStatus = (DeviceIF::Status )initElevator()) != DeviceIF::Ok) { dprintf("OdysseyTailCone::initialize() - initElevator() status = %d", status); error = True; } if (error) setStatus(DeviceIF::Error); else setStatus(DeviceIF::Ok); dprintf("OdysseyTailCone::initialize() - status=%d", status); return status(); } DeviceIF::Status OdysseyTailCone::setDeadman(short milliseconds) { Syslog::write("OdysseyTailCone::setDeadman() - not implemented!!!"); return DeviceIF::Ok; } DeviceIF::Status OdysseyTailCone::deadman(short *milliseconds) { Syslog::write("OdysseyTailCone::deadman() - I think it's %d", ThrusterTimeout); *milliseconds = ThrusterTimeout * 1000; return DeviceIF::Ok; } DeviceIF::Status OdysseyTailCone::propellerSpeed(double *speed, TimeIF::TimeSpec *sampleTime) { // Convert rpm to radians per second // Note: Conversion moved to OdysseyThruster.cc *speed = _thruster->propSpeed(); Time::gettime(sampleTime); return DeviceIF::Ok; } DeviceIF::Status OdysseyTailCone::elevator(double *angle, TimeIF::TimeSpec *sampleTime) { *angle = _elevator->encoderAngle(); Time::gettime(sampleTime); return DeviceIF::Ok; } DeviceIF::Status OdysseyTailCone::rudder(double *angle, TimeIF::TimeSpec *sampleTime) { *angle = _rudder->encoderAngle(); Time::gettime(sampleTime); return DeviceIF::Ok; } DeviceIF::Status OdysseyTailCone::actual(double *speed, double *elevatorAngle, double *rudderAngle, TimeIF::TimeSpec *sampleTime) { propellerSpeed(speed, sampleTime); elevator(elevatorAngle, sampleTime); rudder(rudderAngle, sampleTime); Time::gettime(sampleTime); return DeviceIF::Ok; } DeviceIF::Status OdysseyTailCone::initThruster() { return _thruster->initialize(_enableThrusterDeadman); } DeviceIF::Status OdysseyTailCone::initElevator() { return _elevator->initialize(); } DeviceIF::Status OdysseyTailCone::initRudder() { return _rudder->initialize(); } void OdysseyTailCone::enableThruster() { _thruster->enable(); } void OdysseyTailCone::disableThruster() { _thruster->disable(); } Boolean OdysseyTailCone::thrusterEnabled() { return _thruster->enabled(); } double OdysseyTailCone::speedToCurrent(double vehicleSpeed) { // Hack from Odyssey's dynamic_control.c return (2.6978 * _effDragCoeff * pow(vehicleSpeed, 3.)); } int OdysseyTailCone::spawnAuxTasks() { // No auxillary tasks to spawn return 0; } DeviceIF::Status OdysseyTailCone::status() { return _status; } Boolean OdysseyTailCone::setStatus(DeviceIF::Status newStatus) { if (newStatus == _status) { // No change in state return False; } _status = newStatus; // Trigger event for subscribers triggerEvent(newStatus); // State changed return True; } TailConeIF::Type OdysseyTailCone::tailConeType() { return TailConeIF::Odyssey; }