// ==== PRIVATE FUNCTIONS =====================================================
static void clksyncSendRequest(SyncStatus sync) {
MacPacket packet;
Payload pld;
unsigned long s0;
packet = radioRequestPacket(4);
if(packet == NULL) { return; }
macSetDestAddr(packet, sync->master_addr);
macSetDestPan(packet, sync->master_pan);
pld = macGetPayload(packet);
paySetType(pld, CMD_CLOCK_UPDATE_REQUEST);
while(!radioTxQueueEmpty()) { radioProcess(); }
s0 = sclockGetGlobalTicks();
pld = macGetPayload(packet);
paySetData(pld, 4, (unsigned char*) &s0);
while(!radioEnqueueTxPacket(packet)) { radioProcess(); }
radioProcess();
sync->requests++;
if(sync->requests > MAX_REQUEST_ATTEMPTS) {
sync->state = STATE_REQUEST_TIMEOUT;
}
}
void clksyncHandleRequest(MacPacket packet) {
Payload pld;
MacPacket response;
unsigned long* frame;
unsigned long s0, m1, m2;
pld = macGetPayload(packet);
frame = (unsigned long*) payGetData(pld);
s0 = frame[0]; // Read requester time of flight
m1 = packet->timestamp + sclockGetOffsetTicks(); // Read local time of reception
response = radioRequestPacket(12); // Sending 3 longs
if(response == NULL) { return; }
macSetDestAddr(response, macGetSrcAddr(packet));
macSetDestPan(response, macGetSrcPan(packet));
pld = macGetPayload(response); // Create response packet
paySetType(pld, CMD_CLOCK_UPDATE_RESPONSE);
paySetData(pld, 4, (unsigned char*) &s0);
payAppendData(pld, 4, 4, (unsigned char*) & m1);
// Empty TX queue to minimize time of flight error
while(!radioTxQueueEmpty()) { radioProcess(); }
m2 = sclockGetGlobalTicks(); // Get approximate time of flight
payAppendData(pld, 8, 4, (unsigned char*) & m2);
while(!radioEnqueueTxPacket(response)) { radioProcess(); }
radioProcess();
}
void rgltrRunController(void) {
unsigned long time;
float steer, thrust;
PoseEstimateStruct pose;
if(!is_ready) { return; } // Don't run if not ready
if(reg_state == REG_OFF) { return; } // Don't run if not running
time = sclockGetGlobalTicks(); // Record system time
steer = 0.0;
thrust = 0.0;
pose.yaw = 0.0; // Initialize to make optimizer happy
pose.pitch = 0.0;
if(reg_state == REG_REMOTE_CONTROL) {
steer = rc_outputs.steer;
thrust = rc_outputs.thrust;
} else if(reg_state == REG_TRACK){
steer = runYawControl(pose.yaw);
thrust = runPitchControl(pose.pitch);
// ? = runRollControl(roll); // No roll actuator yet
}
// TODO: Roll control mixing
mcSteer(steer);
mcThrust(thrust);
}
void telemPopulateB(TelemetryStructB *telemetry) {
telemetry->time = sclockGetGlobalTicks();
telemetry->pose[0] = attGetYawBAMS();
telemetry->pose[1] = attGetPitchBAMS();
telemetry->pose[2] = attGetRollBAMS();
return;
}
unsigned long sclockGetGlobalMillis(void) {
return sclockGetGlobalTicks()/MILLIS_FACTOR;
}
