New version of quadcopter software written to OO principles
Dependencies: mbed MODSERIAL filter mbed-rtos ConfigFile PID PPM FreeIMU_external_magnetometer TinyGPS
Diff: NavigationController/NavigationController.cpp
- Revision:
- 3:4823d6750629
- Parent:
- 2:969dfa4f2436
diff -r 969dfa4f2436 -r 4823d6750629 NavigationController/NavigationController.cpp --- a/NavigationController/NavigationController.cpp Wed Apr 01 11:19:21 2015 +0000 +++ b/NavigationController/NavigationController.cpp Fri Apr 24 16:50:20 2015 +0000 @@ -35,7 +35,7 @@ //Get Rc commands _mappedRc = _rc.getMappedRc(); - //Get angle + //Get angle to calculate yaw _angle = _sensors.getAngle(); //Reset accel data if not flying @@ -43,6 +43,9 @@ { //Reset accel _sensors.zeroAccel(); + + //Reset Gps + _sensors.zeroPos(); } //Update yaw target @@ -61,10 +64,15 @@ else _status.setMotorsSpinning(false); //Update target altitude + _setPoints.climbRate = 0; updateAltitudeTarget(); } else if(_navigationMode == Status::ALTITUDE_HOLD) { + //Motors are directly controlled by rc remote + //if(_mappedRc.throttle >= RC_DEAD_ZONE) _status.setMotorsSpinning(true); + //else _status.setMotorsSpinning(false); + //Check if throttle is in dead zone if(_status.getDeadZone() == true) _setPoints.climbRate = 0; else @@ -72,13 +80,35 @@ //Throttle not in dead zone so map to climb rate _setPoints.climbRate = map(_mappedRc.throttle, RC_THRUST_MIN, RC_THRUST_MAX, MIN_CLIMB_RATE, MAX_CLIMB_RATE); + //float target = _setPoints.targetAltitude + (_setPoints.climbRate * 0.02); + //Update target altitude updateAltitudeTarget(); } - //Get PID output - _altitudeHoldPidOutput = _altitudeController->compute(_setPoints.targetAltitude, _setPoints.climbRate); + //If altitude is less than 10cm the directly map the rc throttle stick to the throttle + //else use the throttle from the altitude PID controller + if(_altitude.computed > 10) _altitudeHoldPidOutput = _altitudeController->compute(_setPoints.targetAltitude, _setPoints.climbRate); + else _status.setMotorsSpinning(false); } + /* else if(_navigationMode == Status::AUTO_LAND) + { + //Motors are directly controlled by rc remote + if(_mappedRc.throttle >= RC_DEAD_ZONE) _status.setMotorsSpinning(true); + else _status.setMotorsSpinning(false); + + //Get altitude + _altitude = _sensors.getAltitude(); + + if(_altitude.computed > 1000) _setPoints.targetAltitude = 300; + else if(_altitude.computed < 600) _setPoints.targetAltitude = 100; + else if(_altitude.computed < 300) _setPoints.targetAltitude = 0; + + //If altitude is less than 10 the directly map the rc throttle stick to the throttle + //else use the throttle from the altitude PID controller + if(_altitude.computed > 10) _altitudeHoldPidOutput = _altitudeController->compute(_setPoints.targetAltitude, _setPoints.climbRate); + else _altitudeHoldPidOutput = _mappedRc.throttle; + }*/ if(_state == Status::STANDBY) { @@ -148,7 +178,6 @@ { _altitude = _sensors.getAltitude(); _setPoints.targetAltitude = _altitude.computed; - if(_setPoints.targetAltitude <= ALTITUDE_MIN) _setPoints.targetAltitude = ALTITUDE_MIN; else if(_setPoints.targetAltitude >= ALTITUDE_MAX) _setPoints.targetAltitude = ALTITUDE_MAX; }