CC_ACK CON "y" CC_NullControl CON "z" 'Message Type Codes GPS_Sensor_NSats CON "n" GPS_Sensor_Altitude CON "t" GPS_Sensor_Speed CON "u" GPS_Sensor_Heading CON "v" GPS_Sensor_Longitude CON "w" GPS_Sensor_Latitude CON "x" Accelerometer_Sensor_X CON "o" Accelerometer_Sensor_Y CON "1" Motor CON "p" Servo_Left CON "q" Servo_Right CON "r" Controller_Switch CON "s" 'Pin Constants Controller con p0 GPS con p1 Accel_X con p2 Motor con p3 Elevon_Left con p4 Elevon_Right con p5 'set up constants with the minimum and maximum pulse widths minElevonAngle con 1645 '1500 microseconds pulse low maxElevonAngle con 4400 '4400 microseconds pulse high centeredElevonAngle con 3024 'centered pulse value leftElevonPulseWidth var WORD rightElevonPulseWidth var WORD ElevonStartVal con 90 elevonArg var BYTE motorInitPulseWidth con 1000 minMotorPulseWidth con 2000 '1 millisecond pulse low maxMotorPulseWidth con 3000 '2 milliseconds pulse high motorPulseWidth var WORD motorArg var BYTE isComputerControlled var BYTE buffer1 var BYTE buffer2 var BYTE motorPower var BYTE elevonRight var BYTE elevonLeft var BYTE TempByte var BYTE TempByte2 var BYTE TempWord var WORD TempFloat1 var FLOAT TempFloat2 var FLOAT StepVal var WORD LastAltitude var WORD LastHeading var FLOAT LastLat var FLOAT LastLon var FLOAT dist var FLOAT 'distance travereled each step for simulation Alpha var FLOAT 'angular distance covered on the earth's surface Initialize: GoSub GetIsComputerControlled isComputerControlled StepVal = 0 LastAltitude = 100 'in tenth's of meters as is returned by GPS LastHeading = 0.0 elevonRight = ElevonStartVal elevonLeft = 180 - ElevonStartVal 'needs to be reversed leftElevonPulseWidth = centeredElevonAngle rightElevonPulseWidth = centeredElevonAngle PulsOut Motor, motorInitPulseWidth ' initializes the escro on the motor so the motor will start working motorPower = 5 '50% poer initially dist = 1.0 '1 meter Alpha = dist / 6378100.0 'radius of the earth in meters LastLon = -122.328567; 'lat/lon of Seattle LastLat = 47.603828 'end Initialize main: SERIN s_in, I4800, [buffer1, buffer2] GoSub ProcessCommand GoSub DriveDeviceHandler goto main DriveDeviceHandler GoSub GetIsComputerControlled isComputerControlled if(isComputerControlled = 1) then GoSub Pulse_Servos GoSub Set_Motor_Power GoSub Adjust_Lat_Lon endif return ProcessCommand if(buffer1 = GPS_Sensor_NSats) then GoSub Write_NumSats elseif(buffer1 = GPS_Sensor_Latitude) GoSub Write_Latitude elseif(buffer1 = GPS_Sensor_Longitude) GoSub Write_Longitude elseif(buffer1 = GPS_Sensor_Altitude) GoSub Write_Altitude elseif(buffer1 = GPS_Sensor_Speed) GoSub Write_Speed elseif(buffer1 = GPS_Sensor_Heading) GoSub Write_Heading elseif(buffer1 = Accelerometer_Sensor_X) GoSub Write_Accelerometer_X elseif(buffer1 = Accelerometer_Sensor_Y) GoSub Write_Accelerometer_Y elseif(buffer1 = Motor) GoSub Set_Motor_Speed GoSub SendReciept elseif(buffer1 = Servo_Right) GoSub Set_Servo_Right GoSub SendReciept elseif(buffer1 = Servo_Left) GoSub Set_Servo_Left GoSub SendReciept elseif(buffer1 = Controller_Switch) GoSub Write_Controller else GoSub SendReciept endif return Write_Controller: SEROUT s_out, I4800, [Controller_Switch, DEC isComputerControlled, 10] return Write_NumSats: 'for simulation just returns 10 SEROUT s_out, I4800, [GPS_Sensor_NSats, "10", 10] return Adjust_Lat_Lon 'simulate this by stepping xxx amount per interval based on heading 'TempFloat1 = FASIN( (FCOS(LastHeading)*FCOS(LastLat)*FSIN(alpha)) + (FSIN(LastLat)*FCOS(alpha)) ) 'TempFloat2 = FACOS( (FCOS(alpha) - (FSIN(LastLat)*FSIN(TempFloat1))) / (FCOS(LastLat) * FCOS(TempFloat1)) ) + LastLon TempQuadrant var FLOAT TempDegree var FLOAT GoSub GetQuadrant, TempQuadrant GoSub GetTranslatedDegree, TempDegree TempSinDistance var FLOAT TempCosDistance var FLOAT TempSinDistance = FSIN(TempDegree) * 0.000009 TempCosDistance = FCOS(TempDegree) * 0.000009 if(TempQuadrant = 1) then LastLat = LastLat + TempCosDistance LastLon = LastLon + TempSinDistance elseif(TempQuadrant = 2) LastLat = LastLat - TempSinDistance LastLon = LastLon + TempCosDistance elseif(TempQuadrant = 3) LastLat = LastLat - TempCosDistance LastLon = LastLon - TempSinDistance elseif(TempQuadrant = 4) LastLat = LastLat + TempSinDistance LastLon = LastLon - TempCosDistance endif return GetQuadrant: if(LastHeading = 360) then LastHeading = 0.0 endif if(LastHeading < 90) then return 1.0 elseif(LastHeading < 180) return 2.0 elseif(LastHeading < 270) return 3.0 elseif(LastHeading < 360) return 4.0 endif return 0.0 GetTranslatedDegree: if(LastHeading = 360.0) then LastHeading = 0.0 endif if(LastHeading < 90.0) then return LastHeading elseif(LastHeading < 180.0) return (LastHeading - 90.0) elseif(LastHeading < 270.0) return (LastHeading -180.0) elseif(LastHeading < 360.0) return (LastHeading - 270.0) endif return 0.0 Write_Latitude: SEROUT s_out, I4800, [GPS_Sensor_Latitude, REAL LastLat, 10] return Write_Longitude: SEROUT s_out, I4800, [GPS_Sensor_Longitude, REAL LastLon, 10] return Write_Altitude: GoSub GetAltitude TempWord SEROUT s_out, I4800, [GPS_Sensor_Altitude, DEC TempWord, 10] return GetAltitude 'simulate this by setting base value and adjusting based on accelerometer pitch value GoSub GetAccelerometerX TempByte if ((TempByte > 5) OR (TempByte < -5)) then LastAltitude = LastAltitude + (TempByte / 20) endif return LastAltitude Write_Heading: GoSub GetHeading TempFloat1 SEROUT s_out, I4800, [GPS_Sensor_Heading, REAL TempFloat1, 10] return GetHeading 'simulate this by adjusting heading when yaw is adjusted and continue for each interval GoSub GetAccelerometerY TempByte 'turning left or turning left if ((TempByte > 5) OR (TempByte < -5)) then LastHeading = (LastHeading + ToFloat(TempByte / 20)) if(LastHeading < 0) then LastHeading = 360.0 + LastHeading elseif(LastHeading >= 360) LastHeading = LastHeading - 360.0 endif endif return LastHeading Write_Accelerometer_X: GoSub GetAccelerometerX TempByte SEROUT s_out, I4800, [Accelerometer_Sensor_X, DEC TempByte, 10] return Write_Accelerometer_Y: GoSub GetAccelerometerY TempByte SEROUT s_out, I4800, [Accelerometer_Sensor_Y, DEC TempByte, 10] return Write_Speed: SEROUT s_out, I4800, [GPS_Sensor_Speed, DEC motorPower, 10] return Set_Motor_Speed: motorPower = buffer2 return Set_Servo_Right: elevonRight = buffer2 return Set_Servo_Left: 'left needs to be reversed to perform the correct action elevonLeft = buffer2 elevonLeft = 180 - elevonLeft return GetAccelerometerX 'simulate this by adjusting purportional to the elevon levels '90-ish elevon level = level flight; accel returns -90 to 90 return ((elevonRight+(180-elevonLeft))/2) - ElevonStartVal GetAccelerometerY 'simulate this by adjusting purportional to the elevon levels '90 = level flight 'turning left. returns 0 to 90 depending on turning level if (elevonRight > ElevonStartVal) AND (elevonLeft < ElevonStartVal) then return ((elevonRight-(180-elevonLeft))/2) * -1 elseif(elevonLeft > ElevonStartVal) AND (elevonRight < ElevonStartVal) 'returns 0 to -90 return (((180-elevonLeft)-elevonRight)/2) else return 0 endif return 0 GetIsComputerControlled 'always computer controlled for simulation return 1 Pulse_Servos: 'take the output pins low so we can pulse it high Low Elevon_Left Low Elevon_Right GoSub GetTranslatedElevonPulseWidth[elevonLeft], leftElevonPulseWidth GoSub GetTranslatedElevonPulseWidth[elevonRight] rightElevonPulseWidth PulsOut Elevon_Left, leftElevonPulseWidth PulsOut Elevon_Right, rightElevonPulseWidth return GetTranslatedElevonPulseWidth [elevonArg] 'valid elevonArg values are 0-180 degrees, 90 being level flight. TempFloat1 = ToFloat(elevonArg) * 0.55 'scale the elevon angle to max 100 TempFloat1 = TempFloat1 / 100.0 'now make a percent TempFloat1 = (ToFloat(maxElevonAngle-minElevonAngle) * TempFloat1) + minElevonAngle 'get the actual pulse width TempWord = ToInt(TempFloat1) 'convert to a whole value that can be used for the pulsewidth 'SEROUT s_out, I4800, ["Final Pulse Width: ", DEC TempWord, 10, 13] 'make sure the resulting pulsewidth is within a valid range if(TempWord > maxElevonAngle) then TempWord = maxElevonAngle elseif(TempWord < minElevonAngle) TempWord = minElevonAngle endif return TempWord Set_Motor_Power: Low Motor 'set low so we can pulse it GoSub GetTranslatedMotorPulseWidth [motorPower], MotorPulseWidth PulsOut Motor, MotorPulseWidth return GetTranslatedMotorPulseWidth [motorArg] 'valid motorArg value is 0-10 TempFloat1 = ToFloat(motorPower) * (0.1) 'scale to a percent (0-1) TempFloat1 = (TempFloat1 * ToFloat(maxMotorPulseWidth-minMotorPulseWidth)) + minMotorPulseWidth 'scale to usable pulse width value TempWord = ToInt(TempFloat1) 'get actual pulse width value, valid between 1000-2000 'make sure within valid pulsewidth range if(TempWord > maxMotorPulseWidth) then TempWord = maxMotorPulseWidth elseif(TempWord < minMotorPulseWidth) TempWord = minMotorPulseWidth endif return TempWord SendReciept SEROUT s_out, I4800, [CC_ACK, CC_NULLControl, 10] return