This repository has been archived by the owner on Aug 2, 2021. It is now read-only.
-
Notifications
You must be signed in to change notification settings - Fork 19
/
HunterTruckAPI.c
137 lines (117 loc) · 3.5 KB
/
HunterTruckAPI.c
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
/*
* File: HunterTruckAPI.c
* Author: Chris Hajduk
*
* Created on August 24, 2014, 9:08 PM
*/
#include "debug.h"
#include "GPS.h"
#include "UART1.h"
#include "UART2.h"
#include "net.h"
#include "commands.h"
#include "HunterTruckAPI.h"
#include "StartupErrorCodes.h"
#include "timer.h"
int lastCommandSentCode = 0;
float datalinkTime = 0;
float datalinkLastTime = 0;
char sData = 0;
char tData = 0;
void initTruck(){
//Setup Debugging Connection
initUART1();
//Check for any errors
checkErrorCodes();
//Setup GPS
initGPS();
initTimer4();
//Setup Input and Output
initPWM(0b11,0b11);
PWMOutputCalibration(1,HUNTER_TRUCK_STEERING_SCALE_FACTOR, HUNTER_TRUCK_STEERING_OFFSET);
PWMOutputCalibration(2,HUNTER_TRUCK_THROTTLE_SCALE_FACTOR, HUNTER_TRUCK_THROTTLE_OFFSET);
setThrottle(0);
setSteering(0);
//Setup Datalink
// initDataLink();
}
void setSteering(int percent){
//This is channel 1
//Limit the range between -100% and 100%
if (percent > 100)
percent = 100;
if (percent < -100)
percent = -100;
sData = percent; //Records the steering history
setPWM(1,(int)(percent * 10.24));//Range: Percentage (-100/100) converted to -1024 to 1024
}
void setThrottle(int percent){
//This is channel 2
//Limit the range between 0% and 100%
if (percent > 100)
percent = 100;
if (percent < -100)
percent = -100;
tData = percent; //Records the throttle history
setPWM(2,(int)(percent * 10.24));//Range: Percentage (0/100) converted to -1024 to 1024
}
void background(){
asm("CLRWDT");
// readDatalink();
// time = getUTCTime();
// writeDatalink(DATALINK_SEND_FREQUENCY);
// outboundBufferMaintenance();
// inboundBufferMaintenance();
}
void readDatalink(void){
struct command* cmd = popCommand();
if ( cmd ) {
if (lastCommandSentCode == cmd->cmd){
lastCommandSentCode++;
}
else{
lastCommandSentCode = cmd->cmd * 100;
}
switch (cmd->cmd) {
case DEBUG_TEST: // Debugging command, writes to debug UART
debug((char*) cmd->data);
break;
//TODO: Add commands here
default:
break;
}
destroyCommand( cmd );
}
}
int writeDatalink(long frequency){
if (datalinkTime - datalinkLastTime > frequency) {
datalinkLastTime = datalinkTime;
struct telem_block* statusData = getDebugTelemetryBlock();//createTelemetryBlock();
if (statusData == 0){
return 0;
}
statusData->lat = getLatitude();
statusData->lon = getLongitude();
statusData->millis = getUTCTime();
statusData->groundSpeed = getSpeed();
statusData->heading = getHeading();
statusData->lastCommandSent = lastCommandSentCode;
statusData->errorCodes = getErrorCodes();
statusData->gpsStatus = (char)(getSatellites() + (isGPSLocked() << 4));
statusData->steeringSetpoint = getPWM(1);
statusData->throttleSetpoint = getPWM(2);
statusData->steeringOutput = sData;
statusData->throttleOutput = tData;
if (BLOCKING_MODE) {
sendTelemetryBlock(statusData);
destroyTelemetryBlock(statusData);
} else {
return pushOutboundTelemetryQueue(statusData);
}
}
else if (datalinkTime < datalinkLastTime){
datalinkLastTime = datalinkTime;
return 0;
}
return 0;
}