void Com::loop(int time) {
while(Serial.available()>0) {
char cmd = Serial.read();
switch(cmd) {
case 't':
temp = readLong();
sendOk();
Serial.print("temp = ");
Serial.println(temp);
break;
case 's': // read settings
sendSettings();
break;
case 'P': // write PID
updatePid();
break;
case 'G': // write Gain
updateFloat(&g_userControlGain);
break;
case 'W': // write Windup Guard
updateFloat(&g_pidWindupGuard);
break;
default:
sendError();
break;
}
}
{
Telem telem;
sendPacketStart(PACKET_TELEMETRY);
telem.Yaw = g_telemetryCom.Yaw;
telem.Pitch = g_telemetryCom.Pitch;
telem.Roll = g_telemetryCom.Roll;
telem.MotorL = g_motorLeft.get();
telem.MotorR = g_motorRight.get();
telem.MotorF = g_motorFront.get();
telem.MotorB = g_motorBack.get();
telem.BatteryLevel = g_batteryManager.getLastReading();
telem.UserElev = g_receiverCom.pos[RECEIVER_Throttle];
telem.UserPitch = g_receiverCom.pos[RECEIVER_Elevator];
telem.UserRoll = g_receiverCom.pos[RECEIVER_Aileron];
telem.UserYaw = g_receiverCom.pos[RECEIVER_Rudder];
telem.loopCount = g_loopCount;
Serial.write((byte*)&telem, sizeof(Telem));
}
if(time - _lastSlowTime > 1000) {
_lastSlowTime = time;
Serial.print("RECV: ");
Serial.print(g_receiverCom.hasSignal ? "HAS SIGNAL " : "NO SIGNAL ");
for(int i=0; i<SERVO_INPUT_CHANNELS; i++) {
if(i != 0) {
Serial.print(", ");
}
Serial.print(g_receiverCom.pos[i]);
}
Serial.print(" TELEM: ");
Serial.print(g_telemetryCom.Yaw);
Serial.print(", ");
Serial.print(g_telemetryCom.Pitch);
Serial.print(", ");
Serial.print(g_telemetryCom.Roll);
Serial.print(" MOTOR: ");
Serial.print(g_motorFront.get());
Serial.print(", ");
Serial.print(g_motorBack.get());
Serial.print(", ");
Serial.print(g_motorLeft.get());
Serial.print(", ");
Serial.print(g_motorRight.get());
Serial.print(" PID: ");
Serial.print(g_rollPid.getLastPosition());
Serial.print(", ");
Serial.print(g_pitchPid.getLastPosition());
Serial.print(", ");
Serial.print(g_yawPid.getLastPosition());
Serial.print(" BAT: ");
Serial.print(g_batteryManager.getLastReading());
Serial.println(g_batteryManager.getIsWarning() ? " warning" : " ");
}
}
