BaseStation.cpp

00001 #include "BaseStation.h"
00002 
00003 BaseStation::BaseStation(Status& status, Rc& rc, Sensors& sensors, NavigationController& navigationController, FlightController& flightController, ConfigFileWrapper& configFileWrapper, PinName wirelessPinTx, PinName wirelessPinRx) : _status(status), _rc(rc), _sensors(sensors), _navigationController(navigationController), _flightController(flightController), _configFileWrapper(configFileWrapper)
00004 {
00005     _wireless = new MODSERIAL(wirelessPinTx, wirelessPinRx);
00006     _wireless->baud(57600);
00007     _wirelessSerialRxPos = 0;
00008     
00009     _thread = new Thread(&BaseStation::threadStarter, this, osPriorityNormal);
00010     DEBUG("Base Station initialised\r\n");
00011 }
00012 
00013 BaseStation::~BaseStation(){}
00014 l
00015 void BaseStation::threadStarter(void const *p)
00016 {
00017     BaseStation *instance = (BaseStation*)p;
00018     instance->threadWorker();
00019 }
00020 
00021 void BaseStation::threadWorker()
00022 {   
00023     while(true)
00024     {
00025         //Check comms mode and print correct data back to PC application
00026         Status::BaseStationMode mode = _status.getBaseStationMode();
00027         
00028         if(mode == Status::MOTOR_POWER)
00029         {
00030             MotorMixer::MotorPower motorPower = _flightController.getMotorPower();
00031             
00032             _wireless->printf("<M1=%1.2f:M2=%1.2f:M3=%1.2f:M4=%1.2f>",
00033             motorPower.motor1, motorPower.motor2, motorPower.motor3, motorPower.motor4);
00034         }
00035         else if (mode == Status::PID_OUTPUTS)
00036         {
00037             PidWrapper::PidOutput pidOutputs = _flightController.getPidOutputs();
00038             
00039             _wireless->printf("<YPID=%1.2f:PPID=%1.2f:RPID=%1.2f>",
00040             pidOutputs.yaw, pidOutputs.pitch, pidOutputs.roll);
00041         }
00042         else if (mode == Status::IMU_OUTPUTS)
00043         {
00044             Imu::Rate rate = _sensors.getRate();
00045             Imu::Angle angle = _sensors.getAngle();
00046             
00047             _wireless->printf("<SY=%1.2f:SP=%1.2f:SR=%1.2f:RY=%1.2f:RP=%1.2f:RR=%1.2f>",
00048             angle.yaw, angle.pitch, angle.roll, rate.yaw, rate.pitch, rate.roll);
00049         }
00050         else if (mode == Status::STATUS)
00051         {                        
00052             _wireless->printf("<Batt=%f:Armed=%d:Init=%d:FlightMode=%d:State=%d:NavMode=%d>",
00053             _status.getBatteryLevel(), _status.getArmed(), _status.getInitialised(), _status.getFlightMode(), _status.getState(), _status.getNavigationMode());
00054         }
00055         else if (mode == Status::RC)
00056         {
00057             Rc::MappedRc mappedRc = _rc.getMappedRc();
00058             Rc::RawRc rawRc = _rc.getRawRc();
00059             
00060             _wireless->printf("<MRCY=%1.2f:MRCP=%1.2f:MRCR=%1.2f:MRCT=%1.2f:RRC1=%1.2f:RRC2=%1.2f:RRC3=%1.2f:RRC4=%1.2f:RRC5=%1.2f:RRC6=%1.2f:RRC7=%1.2f:RRC8=%1.2f>",
00061             mappedRc.yaw, mappedRc.pitch, mappedRc.roll, mappedRc.throttle, rawRc.channel0, rawRc.channel1, rawRc.channel2, rawRc.channel3, rawRc.channel4, rawRc.channel5, rawRc.channel6, rawRc.channel7);
00062         }
00063         else if (mode == Status::PID_TUNING)
00064         {
00065             PidWrapper::FlightControllerPidParameters flightControllerPidParameters = _flightController.getPidParameters();
00066             PidWrapper::NavigationControllerPidParameters navigationControllerPidParameters = _navigationController.getPidParameters();
00067             _wireless->printf("<RYPIDP=%1.8f:RYPIDI=%1.8f:RYPIDD=%1.8f:RPPIDP=%1.8f:RPPIDI=%1.8f:RPPIDD=%1.8f:RRPIDP=%1.8f:RRPIDI=%1.8f:RRPIDD=%1.8f:SYPIDP=%1.8f:SYPIDI=%1.8f:SYPIDD=%1.8f:SPPIDP=%1.8f:SPPIDI=%1.8f:SPPIDD=%1.8f:SRPIDP=%1.8f:SRPIDI=%1.8f:SRPIDD=%1.8f:ARPIDP=%1.8f:ARPIDI=%1.8f:ARPIDD=%1.8f:ASPIDP=%1.8f:ASPIDI=%1.8f:ASPIDD=%1.8f>",
00068             flightControllerPidParameters.yawRate.p, flightControllerPidParameters.yawRate.i, flightControllerPidParameters.yawRate.d, 
00069             flightControllerPidParameters.pitchRate.p, flightControllerPidParameters.pitchRate.i, flightControllerPidParameters.pitchRate.d, 
00070             flightControllerPidParameters.rollRate.p, flightControllerPidParameters.rollRate.i, flightControllerPidParameters.rollRate.d, 
00071             flightControllerPidParameters.yawStab.p, flightControllerPidParameters.yawStab.i, flightControllerPidParameters.yawStab.d, 
00072             flightControllerPidParameters.pitchStab.p, flightControllerPidParameters.pitchStab.i, flightControllerPidParameters.pitchStab.d, 
00073             flightControllerPidParameters.rollStab.p, flightControllerPidParameters.rollStab.i, flightControllerPidParameters.rollStab.d,
00074             navigationControllerPidParameters.altitudeRate.p, navigationControllerPidParameters.altitudeRate.i, navigationControllerPidParameters.altitudeRate.d,
00075             navigationControllerPidParameters.altitudeStab.p, navigationControllerPidParameters.altitudeStab.i, navigationControllerPidParameters.altitudeStab.d);
00076         }
00077         else if (mode == Status::GPS)
00078         {
00079             Gps::Value gpsValues = _sensors.getGpsValues();
00080             
00081             _wireless->printf("<GLat=%1.6f:GLon=%1.6f:GAlt=%1.2f:GInit=%d>",
00082             gpsValues.latitude, gpsValues.longitude, gpsValues.altitude, gpsValues.fix);
00083         }/*
00084         else if (mode == Status::ZERO)
00085         {
00086                             _wirelessSerial.printf("<ZY=%1.6f:ZP=%1.6f:ZR=%1.6f>",
00087                 _zeroValues[0], _zeroValues[1], _zeroValues[2]);
00088                 break;
00089             
00090         }*/
00091         else if (mode == Status::RATE_TUNING)
00092         {
00093             if(_status.getFlightMode() == Status::RATE)
00094             {
00095                 PidWrapper::RatePidState ratePidState = _flightController.getRatePidState();
00096                 
00097                 //Yaw set point, Yaw actual, Yaw PID output
00098                 //Pitch set point, Pitch actual, Pitch PID output
00099                 //Roll set point, Roll actual, Roll PID output
00100                 _wireless->printf("<RYPIDS=%1.2f:RYPIDP=%1.2f:RYPIDO=%1.2f:RPPIDS=%1.2f:RPPIDP=%1.2f:RPPIDO=%1.2f:RRPIDS=%1.2f:RRPIDP=%1.2f:RRPIDO=%1.2f>",
00101                 ratePidState.yawRate.setPoint, ratePidState.yawRate.processValue, ratePidState.yawRate.output, ratePidState.pitchRate.setPoint, ratePidState.pitchRate.processValue, ratePidState.pitchRate.output, ratePidState.rollRate.setPoint, ratePidState.rollRate.processValue, ratePidState.rollRate.output);
00102             }
00103             else if (_status.getFlightMode() == Status::STAB)
00104             {
00105                 PidWrapper::StabPidState stabPidState = _flightController.getStabPidState();
00106                 
00107                 //Yaw set point, Yaw actual, Yaw PID output
00108                 //Pitch set point, Pitch actual, Pitch PID output
00109                 //Roll set point, Roll actual, Roll PID output
00110                 _wireless->printf("<RYPIDS=%1.2f:RYPIDP=%1.2f:RYPIDO=%1.2f:RPPIDS=%1.2f:RPPIDP=%1.2f:RPPIDO=%1.2f:RRPIDS=%1.2f:RRPIDP=%1.2f:RRPIDO=%1.2f>",
00111                 stabPidState.yawRate.setPoint, stabPidState.yawRate.processValue, stabPidState.yawRate.output, stabPidState.pitchRate.setPoint, stabPidState.pitchRate.processValue, stabPidState.pitchRate.output, stabPidState.rollRate.setPoint, stabPidState.rollRate.processValue, stabPidState.rollRate.output);
00112             }   
00113         }
00114         else if (mode == Status::STAB_TUNING)
00115         {
00116             if(_status.getFlightMode() == Status::RATE)
00117             {
00118                 //Yaw set point, Yaw actual, Yaw PID output
00119                 //Pitch set point, Pitch actual, Pitch PID output
00120                 //Roll set point, Roll actual, Roll PID output
00121                 _wireless->printf("<SYPIDS=0:SYPIDP=0:SYPIDO=0:SPPIDS=0:SPPIDP=0:SPPIDO=0:SRPIDS=0:SRPIDP=0:SRPIDO=0>");
00122             }
00123             else if (_status.getFlightMode() == Status::STAB)
00124             {
00125                 PidWrapper::StabPidState stabPidState = _flightController.getStabPidState();
00126                 
00127                 //Yaw set point, Yaw actual, Yaw PID output
00128                 //Pitch set point, Pitch actual, Pitch PID output
00129                 //Roll set point, Roll actual, Roll PID output
00130                 _wireless->printf("<SYPIDS=%1.2f:SYPIDP=%1.2f:SYPIDO=%1.2f:SPPIDS=%1.2f:SPPIDP=%1.2f:SPPIDO=%1.2f:SRPIDS=%1.2f:SRPIDP=%1.2f:SRPIDO=%1.2f>",
00131                 stabPidState.yawStab.setPoint, stabPidState.yawStab.processValue, stabPidState.yawStab.output, stabPidState.pitchStab.setPoint, stabPidState.pitchStab.processValue, stabPidState.pitchStab.output, stabPidState.rollStab.setPoint, stabPidState.rollStab.processValue, stabPidState.rollStab.output);
00132             } 
00133         }
00134         else if (mode == Status::ALTITUDE)
00135         {
00136             Sensors::Altitude altitude = _sensors.getAltitude();      
00137             Imu::Acceleration acceleration = _sensors.getImuAcceleration();
00138                   
00139             _wireless->printf("<ZVEL=%1.2f:CAlt=%1.2f:BAlt=%1.2f:LAlt=%1.2f>",
00140             acceleration.z, altitude.computed, altitude.barometer, altitude.lidar);
00141         }
00142         else if (mode == Status::VELOCITY)
00143         {
00144             //Imu::Velocity velocity = _sensors.getImuVelocity();
00145             //_wireless->printf("<XVEL=%1.2f:YVEL=%1.2f:ZVEL=%1.2f>",
00146             //velocity.x, velocity.y, velocity.z);  
00147             Imu::Acceleration acceleration = _sensors.getImuAcceleration();
00148             _wireless->printf("<XVEL=%1.2f:YVEL=%1.2f:ZVEL=%1.2f>",
00149             acceleration.x, acceleration.y, acceleration.z);   
00150         }
00151         else if (mode == Status::ALTITUDE_STATUS)
00152         {
00153             NavigationController::SetPoint setPoints = _navigationController.getSetPoint();
00154             Sensors::Altitude altitude = _sensors.getAltitude();
00155             _wireless->printf("<ACR=%1.2f:ATA=%1.2f:ATHR=%1.2f:CAlt=%1.2f:ZVEL=%1.2f>",
00156             setPoints.climbRate, setPoints.targetAltitude, (setPoints.throttle * 100), altitude.computed, _sensors.getImuVelocity().z);      
00157         }
00158         else if (mode == Status::LIDAR)
00159         {
00160             Sensors::Altitude altitude = _sensors.getAltitude();
00161             Imu::Angle angle = _sensors.getAngle();         
00162             _wireless->printf("<SP=%1.2f:SR=%1.2f:LAlt=%1.2f>", angle.pitch, angle.roll, altitude.lidar);   
00163         }
00164         
00165         //Check for wireless serial command
00166         while (_wireless->readable() > 0)
00167         {
00168             int c = _wireless->getc();
00169                                                 
00170             switch (c)
00171             {
00172                 case 60: // 
00173                     _wirelessSerialRxPos = 0;
00174                     break;
00175                 
00176                 case 10: // LF
00177                 case 13: // CR
00178                 case 62: // >
00179                     checkCommand();
00180                     break;
00181                     
00182                 default:
00183                     _wirelessSerialBuffer[_wirelessSerialRxPos++] = c;
00184                     if (_wirelessSerialRxPos > 200)
00185                     {
00186                         _wirelessSerialRxPos = 0;
00187                     }
00188                     break;
00189             }
00190         }
00191         
00192         Thread::wait(200);
00193     }
00194 }
00195 
00196 void BaseStation::checkCommand()
00197 {
00198     int length = _wirelessSerialRxPos;
00199     _wirelessSerialBuffer[_wirelessSerialRxPos] = 0;
00200     _wirelessSerialRxPos = 0;
00201 
00202     if (length < 1)
00203     {
00204         return;
00205     }
00206     
00207     char command = _wirelessSerialBuffer[0];
00208     double value = 0;
00209     if(length > 1)
00210     {
00211         value = atof((char*)&_wirelessSerialBuffer[2]);
00212     }
00213     
00214     PidWrapper::FlightControllerPidParameters flightControllerPidParameters = _flightController.getPidParameters();
00215     PidWrapper::NavigationControllerPidParameters navigationControllerPidParameters = _navigationController.getPidParameters();
00216     
00217     switch (command)
00218     {
00219         case 'a':
00220             navigationControllerPidParameters.altitudeRate.p = value;
00221             _navigationController.setAltitudeRatePidParameters(navigationControllerPidParameters.altitudeRate);
00222             break;
00223             
00224         case 'b':
00225             navigationControllerPidParameters.altitudeRate.i = value;
00226             _navigationController.setAltitudeRatePidParameters(navigationControllerPidParameters.altitudeRate);
00227             break;
00228             
00229         case 'c':
00230             navigationControllerPidParameters.altitudeRate.d = value;
00231             _navigationController.setAltitudeRatePidParameters(navigationControllerPidParameters.altitudeRate);
00232             break;
00233             
00234         case 'd':
00235             navigationControllerPidParameters.altitudeStab.p = value;
00236             _navigationController.setAltitudeStabPidParameters(navigationControllerPidParameters.altitudeStab);
00237             break;
00238             
00239         case 'e':
00240             navigationControllerPidParameters.altitudeStab.i = value;
00241             _navigationController.setAltitudeStabPidParameters(navigationControllerPidParameters.altitudeStab);
00242             break;
00243             
00244         case 'f':
00245             navigationControllerPidParameters.altitudeStab.d = value;
00246             _navigationController.setAltitudeStabPidParameters(navigationControllerPidParameters.altitudeStab);
00247             break;
00248             
00249         case 'g':
00250             _sensors.zeroAccel();
00251             break;
00252             
00253         //Set PID values
00254         case 'h':
00255             flightControllerPidParameters.yawRate.p = value;
00256             _flightController.setYawRatePidParameters(flightControllerPidParameters.yawRate);
00257             break;
00258             
00259         case 'i':
00260             flightControllerPidParameters.yawRate.i = value;
00261             _flightController.setYawRatePidParameters(flightControllerPidParameters.yawRate);
00262             break;
00263             
00264         case 'j':
00265             flightControllerPidParameters.yawRate.d = value;
00266             _flightController.setYawRatePidParameters(flightControllerPidParameters.yawRate);
00267             break;
00268             
00269         case 'k':
00270             flightControllerPidParameters.pitchRate.p = value;
00271             _flightController.setPitchRatePidParameters(flightControllerPidParameters.pitchRate);
00272             break;
00273             
00274         case 'l':
00275             flightControllerPidParameters.pitchRate.i = value;
00276             _flightController.setPitchRatePidParameters(flightControllerPidParameters.pitchRate); 
00277             break;
00278             
00279         case 'm':
00280             flightControllerPidParameters.pitchRate.d = value;
00281             _flightController.setPitchRatePidParameters(flightControllerPidParameters.pitchRate);
00282             break;
00283             
00284         case 'n':
00285             flightControllerPidParameters.rollRate.p = value;
00286             _flightController.setRollRatePidParameters(flightControllerPidParameters.rollRate);            
00287             break;
00288             
00289         case 'o':
00290             flightControllerPidParameters.rollRate.i = value;
00291             _flightController.setRollRatePidParameters(flightControllerPidParameters.rollRate);    
00292             break;
00293             
00294         case 'p':
00295             flightControllerPidParameters.rollRate.d = value;
00296             _flightController.setRollRatePidParameters(flightControllerPidParameters.rollRate);    
00297             break;
00298             
00299         case 'q':
00300             flightControllerPidParameters.yawStab.p = value;
00301             _flightController.setYawStabPidParameters(flightControllerPidParameters.yawStab);    
00302             break;
00303             
00304         case 'r':
00305             flightControllerPidParameters.yawStab.i = value;
00306             _flightController.setYawStabPidParameters(flightControllerPidParameters.yawStab);   
00307             break;
00308             
00309         case 's':
00310             flightControllerPidParameters.yawStab.d = value;
00311             _flightController.setYawStabPidParameters(flightControllerPidParameters.yawStab);   
00312             break;
00313             
00314         case 't':
00315             flightControllerPidParameters.pitchStab.p = value;
00316             _flightController.setPitchStabPidParameters(flightControllerPidParameters.pitchStab);   
00317             break;
00318             
00319         case 'u':
00320             flightControllerPidParameters.pitchStab.i = value;
00321             _flightController.setPitchStabPidParameters(flightControllerPidParameters.pitchStab); 
00322             break;
00323             
00324         case 'v':
00325             flightControllerPidParameters.pitchStab.d = value;
00326             _flightController.setPitchStabPidParameters(flightControllerPidParameters.pitchStab); 
00327             break;
00328             
00329         case 'w':
00330             flightControllerPidParameters.rollStab.p = value;
00331             _flightController.setRollStabPidParameters(flightControllerPidParameters.rollStab); 
00332             break;
00333             
00334         case 'x':
00335             flightControllerPidParameters.rollStab.i = value;
00336             _flightController.setRollStabPidParameters(flightControllerPidParameters.rollStab);
00337             break;
00338             
00339         case 'y':
00340             flightControllerPidParameters.rollStab.d = value;
00341             _flightController.setRollStabPidParameters(flightControllerPidParameters.rollStab);
00342             break;
00343             
00344         case 'z':
00345             _status.setBaseStationMode(static_cast<Status::BaseStationMode>(value));
00346             break;
00347             
00348         default:
00349             break;
00350     }
00351     
00352     return;
00353 }