Merge pull request #15 from ChristopherRabotin/issue-13

Implemented Naasz (with personal changes)
ChristopherRabotin · Jan 12, 2017 · ed1dc3a · ed1dc3a
2 parents d87a9f0 + 023e08c
commit ed1dc3a
Show file tree

Hide file tree

Showing 9 changed files with 557 additions and 370 deletions.
diff --git a/src/dynamics/astro.go b/src/dynamics/astro.go
@@ -20,14 +20,14 @@ var wg sync.WaitGroup
 // Astrocodile is an orbit propagator.
 // It's a play on words from STK's Atrogrator.
 type Astrocodile struct {
-	Vehicle   *Spacecraft // As pointer because SC may be altered during propagation.
-	Orbit     *Orbit      // As pointer because the orbit changes during propagation.
-	StartDT   time.Time
-	EndDT     time.Time
-	CurrentDT time.Time
-	StopChan  chan (bool)
-	histChan  chan<- (AstroState)
-	done      bool
+	Vehicle        *Spacecraft // As pointer because SC may be altered during propagation.
+	Orbit          *Orbit      // As pointer because the orbit changes during propagation.
+	StartDT        time.Time
+	EndDT          time.Time
+	CurrentDT      time.Time
+	StopChan       chan (bool)
+	histChan       chan<- (AstroState)
+	done, collided bool
 }
 
 // NewAstro returns a new Astrocodile instance from the position and velocity vectors.
@@ -52,7 +52,7 @@ func NewAstro(s *Spacecraft, o *Orbit, start, end time.Time, conf ExportConfig)
 		end = end.UTC()
 	}
 
-	a := &Astrocodile{s, o, start, end, start, make(chan (bool), 1), histChan, false}
+	a := &Astrocodile{s, o, start, end, start, make(chan (bool), 1), histChan, false, false}
 	// Write the first data point.
 	if histChan != nil {
 		histChan <- AstroState{a.CurrentDT, *s, *o}
@@ -168,9 +168,14 @@ func (a *Astrocodile) SetState(i uint64, s []float64) {
 	a.Vehicle.FuncQ = make([]func(), 5) // Clear the queue.
 
 	// Orbit sanity checks
-	if rNorm := norm(a.Orbit.GetR()); rNorm < a.Orbit.Origin.Radius {
-		a.Vehicle.logger.Log("level", "critical", "subsys", "astro", "collided", a.Orbit.Origin.Name)
-	} else if rNorm > a.Orbit.Origin.SOI {
+	if !a.collided && a.Orbit.GetRNorm() < a.Orbit.Origin.Radius {
+		a.collided = true
+		a.Vehicle.logger.Log("level", "critical", "subsys", "astro", "collided", a.Orbit.Origin.Name, "dt", a.CurrentDT)
+	} else if a.collided && a.Orbit.GetRNorm() > a.Orbit.Origin.Radius*1.01 {
+		// Now further from the 1% dead zone
+		a.collided = false
+		a.Vehicle.logger.Log("level", "critical", "subsys", "astro", "revived", a.Orbit.Origin.Name, "dt", a.CurrentDT)
+	} else if a.Orbit.GetRNorm() > a.Orbit.Origin.SOI {
 		a.Vehicle.ToXCentric(Sun, a.CurrentDT, a.Orbit)
 	}
 
@@ -189,7 +194,7 @@ func (a *Astrocodile) Func(t float64, f []float64) (fDot []float64) {
 	}
 	tmpOrbit := NewOrbitFromOE(f[0], f[1], f[2], f[3], f[4], f[5], a.Orbit.Origin)
 	p := tmpOrbit.GetSemiParameter()
-	h := tmpOrbit.GetH()
+	h := tmpOrbit.GetHNorm()
 	r := tmpOrbit.GetRNorm()
 	sini, cosi := math.Sincos(tmpOrbit.i)
 	sinν, cosν := math.Sincos(tmpOrbit.ν)
@@ -219,7 +224,9 @@ func (a *Astrocodile) Func(t float64, f []float64) (fDot []float64) {
 			fDot[i] = math.Mod(fDot[i], 2*math.Pi)
 		}
 		if math.IsNaN(fDot[i]) {
-			panic(fmt.Errorf("fDot[%d]=NaN @ dt=%s\np=%f\th=%f\tsin=%f\tdv=%+v\ntmp:%s\ncur:%s", i, a.CurrentDT, p, h, sinν, Δv, tmpOrbit, a.Orbit))
+			Rcur, Vcur := a.Orbit.GetRV()
+			Rtmp, Vtmp := tmpOrbit.GetRV()
+			panic(fmt.Errorf("fDot[%d]=NaN @ dt=%s\np=%f\th=%f\tdv=%+v\ntmp:%s\ncur:%s\nR_cur=%+v\tV_cur=%+v\nR_tmp=%+v\tV_tmp=%+v", i, a.CurrentDT, p, h, Δv, tmpOrbit, a.Orbit, Rcur, Vcur, Rtmp, Vtmp))
 		}
 	}
 	return