func (p *Pilot) updateAfterError() {
if p.enabled {
p.alarm = RAISED
// make sure the steering is disabled
p.steeringChan <- steering.NewMessage(0, false)
}
}
func (p *Pilot) updateAfterTimeout() {
if p.enabled {
p.alarm = RAISED
p.leds[dashboard.NoGPSFix] = true
// make sure the steering is disabled
p.steeringChan <- steering.NewMessage(0, false)
}
}
func (s *Stepper) processGPSMessage(m pilot.GPSFeedBackAction) {
// Update the state
s.mu.Lock()
defer s.mu.Unlock()
now := time.Now()
switch s.plan.State {
case GO:
s.plan.Input.Heading = m.Heading
s.plan.State = PREPARING
s.plan.Start = types.JSONTime(now)
s.plan.Points = []Point{{
Timestamp: types.JSONTime(now),
FixDate: m.Date,
FixTime: m.Time,
Course: m.Heading,
Speed: m.Speed,
DeltaSteering: 0,
Latitude: m.Latitude,
Longitude: m.Longitude,
Validity: m.Validity,
}}
case PREPARING:
if time.Time(s.plan.Start).Add(time.Duration(s.plan.Input.Duration)).Before(now) {
// send message to steering -- that's where we punch the system
s.steeringChan <- steering.NewMessage(s.plan.Input.Step, true)
s.plan.Points = append(s.plan.Points, Point{
Timestamp: types.JSONTime(now),
FixDate: m.Date,
FixTime: m.Time,
Course: m.Heading,
Speed: m.Speed,
DeltaSteering: s.plan.Input.Step,
Latitude: m.Latitude,
Longitude: m.Longitude,
Validity: m.Validity,
})
s.plan.State = RUNNING
} else {
s.plan.Points = append(s.plan.Points, Point{
Timestamp: types.JSONTime(now),
FixDate: m.Date,
FixTime: m.Time,
Course: m.Heading,
Speed: m.Speed,
DeltaSteering: 0,
Latitude: m.Latitude,
Longitude: m.Longitude,
Validity: m.Validity,
})
}
case RUNNING:
s.plan.Points = append(s.plan.Points, Point{
Timestamp: types.JSONTime(now),
FixDate: m.Date,
FixTime: m.Time,
Course: m.Heading,
Speed: m.Speed,
DeltaSteering: 0,
Latitude: m.Latitude,
Longitude: m.Longitude,
Validity: m.Validity,
})
if time.Time(s.plan.Start).Add(time.Duration(2 * s.plan.Input.Duration)).Before(now) {
// Time is up
// write to file
f, err := os.Create("/tmp/systemCalibration-" + time.Now().Format(time.RFC3339))
if err != nil {
log.Error("Failed to open experiment log file: %v", err)
break
}
defer f.Close()
json.NewEncoder(f).Encode(s.plan)
// tell the pilot the calibration test is over and data can be collected
log.Notice("The bump test is over. You can disable the autopilot, stop the program and start another test.")
s.steeringChan <- steering.NewMessage(0, false)
s.plan.State = DONE
}
case DONE:
// Nothing
log.Notice("DONE")
}
}
func (p *Pilot) updateFeedback(gpsHeading GPSFeedBackAction) {
p.course = gpsHeading.Heading
p.speed = gpsHeading.Speed
// Set the heading with the current GPS heading if it has not been set before
if p.enabled && !p.headingSet && gpsHeading.Validity {
log.Info("Heading to %v", gpsHeading.Heading)
p.heading = gpsHeading.Heading
p.pid.Set(0) // Reference is always 0 for us
p.headingSet = true
}
// check the validity of the message validity of the gps message
validityAlarm := checkValidityError(gpsHeading.Validity)
// check the speed
speedAlarm := checkSpeedError(gpsHeading.Speed)
headingError := ComputeHeadingError(p.heading, gpsHeading.Heading, p.headingOffset)
headingAlarm := !validityAlarm && !speedAlarm && p.checkHeadingError(headingError)
/////////////////////////
// Update pilot state from previous checks
////////////////////////
if p.enabled {
// Update alarm state from the previously computed alarms
p.alarm = p.alarm || headingAlarm || validityAlarm || speedAlarm
if p.alarm == UNRAISED {
log.Notice("Heading error is %v", headingError)
}
// Update alarm state from the previously computed alarms
if bool(headingAlarm) {
p.leds[dashboard.HeadingErrorOutOfBounds] = true
}
if bool(validityAlarm) {
p.leds[dashboard.InvalidGPSData] = true
}
if bool(speedAlarm) {
p.leds[dashboard.SpeedTooLow] = true
}
headingControl := p.pid.Update(headingError)
steeringEnabled := p.computeSteeringState()
if steeringEnabled {
log.Notice("Heading control is %v", headingControl)
log.Notice("Steering Enabled")
// check the PID output
minPIDoutput, maxPIDOutput := p.pid.OutputLimits()
if headingControl <= minPIDoutput || headingControl >= maxPIDOutput {
p.leds[dashboard.CorrectionAtLimit] = true
}
p.steeringChan <- steering.NewMessage(headingControl, true)
} else {
log.Notice("Steering Disabled")
p.steeringChan <- steering.NewMessage(0, false)
}
} else {
////////////////////////
// <This section is updated when the pilot is not enabled>
////////////////////////
// Alarms are UNRAISED
p.alarm = UNRAISED
// Update alarm state from the previously computed alarms
p.leds[dashboard.HeadingErrorOutOfBounds] = false // Doesn't make sense when disabled
p.leds[dashboard.InvalidGPSData] = bool(validityAlarm)
p.leds[dashboard.SpeedTooLow] = bool(speedAlarm)
p.leds[dashboard.CorrectionAtLimit] = false // Doesn't make sense when disabled
// make sure the steering is disabled
p.steeringChan <- steering.NewMessage(0, false)
////////////////////////
// </This section is updated when the pilot is not enabled>
////////////////////////
}
}
