diff --git a/libraries/AP_Mount/AP_Mount.cpp b/libraries/AP_Mount/AP_Mount.cpp index 6e387e2c6d..f19ba7fe93 100644 --- a/libraries/AP_Mount/AP_Mount.cpp +++ b/libraries/AP_Mount/AP_Mount.cpp @@ -1,9 +1,13 @@ #include "AP_Mount.h" -AP_Mount::AP_Mount(GPS *gps, AP_DCM *dcm) +AP_Mount::AP_Mount(GPS *gps, AP_DCM *dcm, AP_Var::Key key, const prog_char_t *name): +_group(key, name), +_mountMode(&_group, 0, 0, name ? PSTR("MODE") : 0), // suppress name if group has no name +_mountType(&_group, 0, 0, name ? PSTR("TYPE") : 0), +_dcm(dcm); +_gps(gps); { - _dcm=dcm; - _gps=gps; + } void AP_Mount::SetPitchYaw(Location targetGPSLocation) @@ -11,7 +15,7 @@ void AP_Mount::SetPitchYaw(Location targetGPSLocation) _targetGPSLocation=targetGPSLocation; //set mode - _mountmode=gps; + _mountMode=gps; //update mount position UpDateMount(); @@ -22,7 +26,7 @@ void AP_Mount::SetGPSTarget(Location targetGPSLocation) _targetGPSLocation=targetGPSLocation; //set mode - _mountmode=gps; + _mountMode=gps; //update mount position UpDateMount(); @@ -35,7 +39,7 @@ void AP_Mount::SetAssisted(int roll, int pitch, int yaw) _AssistAngles.z = yaw; //set mode - _mountmode=assisted; + _mountMode=assisted; //update mount position UpDateMount(); @@ -49,7 +53,7 @@ void AP_Mount::SetMountFreeRoam(int roll, int pitch, int yaw) _RoamAngles.z=yaw; //set mode - _mountmode=roam; + _mountMode=roam; //now update mount position UpDateMount(); @@ -63,7 +67,7 @@ void AP_Mount::SetMountLanding(int roll, int pitch, int yaw) _LandingAngles.z=yaw; //set mode - _mountmode=landing; + _mountMode=landing; //now update mount position UpDateMount(); @@ -72,7 +76,7 @@ void AP_Mount::SetMountLanding(int roll, int pitch, int yaw) void AP_Mount::SetNone() { //set mode - _mountmode=none; + _mountMode=none; //now update mount position UpDateMount(); @@ -80,7 +84,7 @@ void AP_Mount::SetNone() void AP_Mount::UpDateMount() { - switch(_mountmode) + switch(_mountMode) { case gps: { @@ -90,8 +94,7 @@ void AP_Mount::UpDateMount() } m = _dcm->get_dcm_transposed(); targ = m*_GPSVector; - pitchAngle = degrees(atan2(-targ.x,targ.z))*100; //pitch - rollAngle = degrees(atan2(targ.y,targ.z))*100; //roll + this->CalcMountAnglesFromVector(*targ) break; } case stabilise: @@ -109,10 +112,8 @@ void AP_Mount::UpDateMount() case assisted: { m = _dcm->get_dcm_transposed(); - //rotate vector targ = m*_AssistVector; - pitchAngle = degrees(atan2(-targ.x,targ.z))*100; //pitch - rollAngle = degrees(atan2(targ.y,targ.z))*100; //roll + this->CalcMountAnglesFromVector(*targ) break; } case landing: @@ -137,7 +138,7 @@ void AP_Mount::UpDateMount() void AP_Mount::SetMode(MountMode mode) { - _mountmode=mode; + _mountMode=mode; } void AP_Mount::CalcGPSTargetVector(struct Location *target) @@ -146,3 +147,38 @@ void AP_Mount::CalcGPSTargetVector(struct Location *target) _targetVector.y = (target->lat-_gps->latitude)*.01113195; _targetVector.z = (_gps->altitude-target->alt); } + +void AP_Mount::CalcMountAnglesFromVector(Vector3f *targ) +{ + switch(_mountType) + { + case pitch_yaw: + { + //need to tidy up maths for below + pitchAngle = atan2((sqrt(sq(targ.y) + sq(targ.x)) * .01113195), targ.z) * -1; + yawAngle = 9000 + atan2(-targ.y, targ.x) * 5729.57795; + break; + } + case pitch_roll: + { + pitchAngle = degrees(atan2(-targ.x,targ.z))*100; //pitch + rollAngle = degrees(atan2(targ.y,targ.z))*100; //roll + break; + } + case pitch_roll_yaw: + { + //to do + break; + } + case none: + { + //do nothing + break; + } + default: + { + //do nothing + break; + } + } +} diff --git a/libraries/AP_Mount/AP_Mount.h b/libraries/AP_Mount/AP_Mount.h index d6c822b2a8..4ee2930edc 100644 --- a/libraries/AP_Mount/AP_Mount.h +++ b/libraries/AP_Mount/AP_Mount.h @@ -13,6 +13,8 @@ * * * Usage: Use in main code to control mounts attached to * * vehicle. * +* 1. initialise class * +* 2. setMounttype * * * * *Comments: All angles in degrees * 100, distances in meters* * unless otherwise stated. * @@ -27,9 +29,12 @@ class AP_Mount { +protected: + AP_Var_group _group; // must be before all vars to keep ctor init order correct + public: //Constructors - AP_Mount(GPS *gps, AP_DCM *dcm); + AP_Mount(GPS *gps, AP_DCM *dcm, AP_Var::Key key, const prog_char_t *name); //enums enum MountMode{ @@ -43,14 +48,15 @@ public: enum MountType{ pitch_yaw = 0, - pitch_roll = 1, //note the correct english spelling :) + pitch_roll = 1, pitch_roll_yaw = 2, + none = 3; }; //Accessors - void SetPitchYaw(int pitchCh, int yawCh); - void SetPitchRoll(int pitchCh, int rollCh); - void SetPitchRollYaw(int pitchCh, int rollCh, int yawCh); + //void SetPitchYaw(int pitchCh, int yawCh); + //void SetPitchRoll(int pitchCh, int rollCh); + //void SetPitchRollYaw(int pitchCh, int rollCh, int yawCh); void SetGPSTarget(Location targetGPSLocation); //used to tell the mount to track GPS location void SetAssisted(int roll, int pitch, int yaw); @@ -65,15 +71,17 @@ public: int pitchAngle; //degrees*100 int rollAngle; //degrees*100 int yawAngle; //degrees*100 -protected: + +private: //methods void CalcGPSTargetVector(struct Location *target); + void CalcMountAnglesFromVector(Vector3f *targ); //void CalculateDCM(int roll, int pitch, int yaw); //members AP_DCM *_dcm; GPS *_gps; - MountMode _mountmode; + MountMode _mountMode; MountType _mountType; struct Location _targetGPSLocation;