Skip to content

Commit

Permalink
wip refactor
Browse files Browse the repository at this point in the history
  • Loading branch information
Thibaud Teil committed Sep 27, 2024
1 parent 4789a0b commit b1633b8
Showing 1 changed file with 66 additions and 43 deletions.
109 changes: 66 additions & 43 deletions src/fswAlgorithms/attGuidance/flybyPoint/flybyPoint.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -40,65 +40,80 @@ void FlybyPoint::Reset(uint64_t CurrentSimNanos)
*/
void FlybyPoint::UpdateState(uint64_t CurrentSimNanos)
{
/*! create and zero the output message */
AttRefMsgPayload attMsgBuffer = this->attRefOutMsg.zeroMsgPayload;
NavTransMsgPayload relativeState = this->filterInMsg();

/*! compute dt from current time and last filter read time [s] */
double dt = (CurrentSimNanos - this->lastFilterReadTime)*NANO2SEC;
bool invalidSolution = false;
/*! Read position and velocity */
r_BN_N, v_BN_N = this->readRelativeState();

/*! compute dt from current time and last filter read time */
this->dt = (CurrentSimNanos - this->lastFilterReadTime)*NANO2SEC;
bool validSolution = true;
if ((dt >= this->timeBetweenFilterData) || this->firstRead) {
/*! set firstRead to false if this was the first read after a reset */
if (this->firstRead) {
this->firstRead = false;
}
/*! compute velocity/radius ratio at time of read */
this->r_BN_N = Eigen::Map<Eigen::Vector3d>(relativeState.r_BN_N);
this->v_BN_N = Eigen::Map<Eigen::Vector3d>(relativeState.v_BN_N);
this->f0 = this->v_BN_N.norm() / this->r_BN_N.norm();

/*! compute radial (ur_N), velocity (uv_N), along-track (ut_N), and out-of-plane (uh_N) unit direction vectors */
Eigen::Vector3d ur_N = this->r_BN_N.normalized();
Eigen::Vector3d uv_N = this->v_BN_N.normalized();
/*! assert r and v are not collinear (collision trajectory) */
if (std::abs(1 - ur_N.dot(uv_N)) < this->toleranceForCollinearity) {
invalidSolution = false;
}
else {
invalidSolution = true;
}

Eigen::Vector3d uh_N = ur_N.cross(uv_N).normalized();
Eigen::Vector3d ut_N = uh_N.cross(ur_N).normalized();

// compute flight path angle at the time of read
this->gamma0 = std::atan(this->v_BN_N.dot(ur_N) / this->v_BN_N.dot(ut_N));

/*! compute inertial-to-reference DCM at time of read */
this->R0N.row(0) = ur_N;
this->R0N.row(1) = ut_N;
this->R0N.row(2) = uh_N;
this->checkValidity();
this->computeFlybyParameters()
thgis->computeRN();

/*! update lastFilterReadTime to current time and dt to zero */
this->lastFilterReadTime = CurrentSimNanos;
dt = 0;
this->dt = 0;
}

auto sigma_RN, omega_RN_N, omegaDot_RN_N = this->computeGuidanceSolution();
this->writeMessages(sigma_RN, omega_RN_N, omegaDot_RN_N);
}

std::tuple<Eigen::Vector3d, Eigen::Vector3d> readRelativeState() const {
AttRefMsgPayload attMsgBuffer = this->attRefOutMsg.zeroMsgPayload;
NavTransMsgPayload relativeState = this->filterInMsg();

r_BN_N = Eigen::Map<Eigen::Vector3d>(relativeState.r_BN_N);
v_BN_N = Eigen::Map<Eigen::Vector3d>(relativeState.v_BN_N);

return std::tuple<r_BN_N, v_BN_N>
}

void computeFlybyParameters(Eigen::Vector3d &r_BN_N, Eigen::Vector3d &v_BN_N){
/*! compute velocity/radius ratio at time of read */
r_BN_N = Eigen::Map<Eigen::Vector3d>(relativeState.r_BN_N);
v_BN_N = Eigen::Map<Eigen::Vector3d>(relativeState.v_BN_N);
this->f0 = v_BN_N.norm() / r_BN_N.norm();

/*! compute radial (ur_N), velocity (uv_N), along-track (ut_N), and out-of-plane (uh_N) unit direction vectors */
Eigen::Vector3d ur_N = r_BN_N.normalized();
Eigen::Vector3d uv_N = v_BN_N.normalized();

Eigen::Vector3d uh_N = ur_N.cross(uv_N).normalized();
Eigen::Vector3d ut_N = uh_N.cross(ur_N).normalized();
double gamma0 = std::atan(v_BN_N.dot(ur_N) / v_BN_N.dot(ut_N));
double distanceClosestApproach = -r_BN_N.norm()*std::sin(gamma0);

double maxPredictedRate = v_BN_N.norm()/distanceClosestApproach*180/M_PI;
if (maxPredictedRate > this->maxRate && this->maxRate > 0) {
validity = false;
}
double maxPredictedAcceleration = 3*std::sqrt(3)/8*pow(v_BN_N.norm()/distanceClosestApproach, 2)*180/M_PI;
if (maxPredictedAcceleration > this->maxAcceleration && this->maxAcceleration > 0) {
validity = false;
// compute flight path angle at the time of read
this->gamma0 = std::atan(this->v_BN_N.dot(ur_N) / this->v_BN_N.dot(ut_N));
}

bool checkValidity(Eigen::Vector3d &r_BN_N, Eigen::Vector3d &v_BN_N) const{
bool valid = true;
/*! assert r and v are not collinear (collision trajectory) */
if (std::abs(1 - ur_N.dot(uv_N)) < this->toleranceForCollinearity) {
valid = false;
}
return valid;
}

void computeRN(Eigen::Vector3d &r_BN_N, Eigen::Vector3d &v_BN_N){
/*! compute radial (ur_N), velocity (uv_N), along-track (ut_N), and out-of-plane (uh_N) unit direction vectors */
Eigen::Vector3d ur_N = r_BN_N.normalized();
Eigen::Vector3d uv_N = v_BN_N.normalized();

Eigen::Vector3d uh_N = ur_N.cross(uv_N).normalized();
Eigen::Vector3d ut_N = uh_N.cross(ur_N).normalized();

/*! compute inertial-to-reference DCM at time of read */
this->R0N.row(0) = ur_N;
this->R0N.row(1) = ut_N;
this->R0N.row(2) = uh_N;
}
std::tuple<Eigen::Vector3d, Eigen::Vector3d, Eigen::Vector3d> computeGuidanceSolution() const{
/*! compute DCM (RtR0) of reference frame from last read time */
Eigen::Vector3d PRV_theta;
PRV_theta << 0, 0, theta;
Expand Down Expand Up @@ -127,6 +142,14 @@ void FlybyPoint::UpdateState(uint64_t CurrentSimNanos)
Eigen::Vector3d omega_RN_N = RtN.transpose()*omega_RN_R;
Eigen::Vector3d omegaDot_RN_N = RtN.transpose()*omegaDot_RN_R;

return {sigma_RN, omega_RN_N, omegaDot_RN_N}

}
void writeMessages(uint64_t currentSimNanos,
Eigen::Vector3d &sigma_RN,
Eigen::Vector3d &omega_RN_N,
Eigen::Vector3d &omegaDot_RN_N){

eigenVector3d2CArray(sigma_RN, attMsgBuffer.sigma_RN);
eigenVector3d2CArray(omega_RN_N, attMsgBuffer.omega_RN_N);
eigenVector3d2CArray(omegaDot_RN_N, attMsgBuffer.domega_RN_N);
Expand Down

0 comments on commit b1633b8

Please sign in to comment.