diff --git a/afsk/main.c b/afsk/main.c index 64dc5cb1..55b16f90 100644 --- a/afsk/main.c +++ b/afsk/main.c @@ -696,26 +696,14 @@ for (int j = 0; j < frameCnt; j++) token = strtok(NULL, space); } } - } - - tlm[1][A] = (int)(voltage[map[BUS]] /15.0 + 0.5) % 100; // Current of 5V supply to Pi - tlm[1][B] = (int) (99.5 - current[map[PLUS_X]]/10.0) % 100; // +X current [4] - tlm[1][C] = (int) (99.5 - current[map[MINUS_X]]/10.0) % 100; // X- current [10] - tlm[1][D] = (int) (99.5 - current[map[PLUS_Y]]/10.0) % 100; // +Y current [7] - - tlm[2][A] = (int) (99.5 - current[map[MINUS_Y]]/10.0) % 100; // -Y current [10] - tlm[2][B] = (int) (99.5 - current[map[PLUS_Z]]/10.0) % 100; // +Z current [10] // was 70/2m transponder power, AO-7 didn't have a Z panel - tlm[2][C] = (int) (99.5 - current[map[MINUS_Z]]/10.0) % 100; // -Z current (was timestamp) - tlm[2][D] = (int)(50.5 + current[map[BAT]]/10.0) % 100; // NiMH Battery current + } - tlm[3][A] = abs((int)((voltage[map[BAT]] * 10.0) - 65.5) % 100); - tlm[3][B] = (int)(voltage[map[BUS]] * 10.0) % 100; // 5V supply to Pi - - batteryVoltage = voltage[map[BAT]]; + batteryVoltage = voltage[map[BAT]]; + + double cpuTemp; FILE *cpuTempSensor = fopen("/sys/class/thermal/thermal_zone0/temp", "r"); if (cpuTempSensor) { - double cpuTemp; fscanf (cpuTempSensor, "%lf", &cpuTemp); cpuTemp /= 1000; @@ -723,11 +711,96 @@ for (int j = 0; j < frameCnt; j++) printf("CPU Temp Read: %6.1f\n", cpuTemp); #endif - tlm[4][B] = (int)((95.8 - cpuTemp)/1.48 + 0.5) % 100; - } fclose (cpuTempSensor); + +if (sim_mode) +{ + // simulated telemetry + + double time = (millis() - time_start)/1000.0; + + if ((time - eclipse_time) > period) + { + eclipse = (eclipse == 1) ? 0 : 1; + eclipse_time = time; + printf("\n\nSwitching eclipse mode! \n\n"); + } + +/* + double Xi = eclipse * amps_max[0] * sin(2.0 * 3.14 * time / (46.0 * speed)) * fabs(sin(2.0 * 3.14 * time / (46.0 * speed))) + rnd_float(-2, 2); + double Yi = eclipse * amps_max[1] * sin((2.0 * 3.14 * time / (46.0 * speed)) + (3.14/2.0)) * fabs(sin((2.0 * 3.14 * time / (46.0 * speed)) + (3.14/2.0))) + rnd_float(-2, 2); + double Zi = eclipse * amps_max[2] * sin((2.0 * 3.14 * time / (46.0 * speed)) + 3.14 + angle[2]) * fabs(sin((2.0 * 3.14 * time / (46.0 * speed)) + 3.14 + angle[2])) + rnd_float(-2, 2); +*/ + double Xi = eclipse * amps_max[0] * sin(2.0 * 3.14 * time / (46.0 * speed)) + rnd_float(-2, 2); + double Yi = eclipse * amps_max[1] * sin((2.0 * 3.14 * time / (46.0 * speed)) + (3.14/2.0)) + rnd_float(-2, 2); + double Zi = eclipse * amps_max[2] * sin((2.0 * 3.14 * time / (46.0 * speed)) + 3.14 + angle[2]) + rnd_float(-2, 2); + + double Xv = eclipse * volts_max[0] * sin(2.0 * 3.14 * time / (46.0 * speed)) + rnd_float(-0.2, 0.2); + double Yv = eclipse * volts_max[1] * sin((2.0 * 3.14 * time / (46.0 * speed)) + (3.14/2.0)) + rnd_float(-0.2, 0.2); + double Zv = 2.0 * eclipse * volts_max[2] * sin((2.0 * 3.14 * time / (46.0 * speed)) + 3.14 + angle[2]) + rnd_float(-0.2, 0.2); + + // printf("Yi: %f Zi: %f %f %f Zv: %f \n", Yi, Zi, amps_max[2], angle[2], Zv); + + current[map[PLUS_X]] = ( Xi >= 0) ? Xi: 0; + current[map[MINUS_X]] = ( Xi >= 0) ? 0: ((-1.0) * Xi); + current[map[PLUS_Y]] = ( Yi >= 0) ? Yi: 0; + current[map[MINUS_Y]] = ( Yi >= 0) ? 0: ((-1.0) * Yi); + current[map[PLUS_Z]] = ( Zi >= 0) ? Zi: 0; + current[map[MINUS_Z]] = ( Zi >= 0) ? 0: ((-1.0) * Zi); + + voltage[map[PLUS_X]] = ( Xv >= 1) ? Xv: rnd_float(0.9, 1.1); + voltage[map[MINUS_X]] = ( Xv <= -1) ? ((-1.0) * Xv): rnd_float(0.9, 1.1); + voltage[map[PLUS_Y]] = ( Yv >= 1) ? Yv: rnd_float(0.9, 1.1); + voltage[map[MINUS_Y]] = ( Yv <= -1) ? ((-1.0) * Yv): rnd_float(0.9, 1.1); + voltage[map[PLUS_Z]] = ( Zv >= 1) ? Zv: rnd_float(0.9, 1.1); + voltage[map[MINUS_Z]] = ( Zv <= -1) ? ((-1.0) * Zv): rnd_float(0.9, 1.1); + + // printf("temp: %f Time: %f Eclipse: %d : %f %f | %f %f | %f %f\n",tempS, time, eclipse, voltage[map[PLUS_X]], voltage[map[MINUS_X]], voltage[map[PLUS_Y]], voltage[map[MINUS_Y]], current[map[PLUS_Z]], current[map[MINUS_Z]]); + + tempS += (eclipse > 0) ? ((temp_max - tempS)/50.0): ((temp_min - tempS)/50.0); + cpuTemp = (int)((tempS + rnd_float(-1.0, 1.0)) * 10 + 0.5); + + voltage[map[BUS]] = rnd_float(5.0, 5.005); + current[map[BUS]] = rnd_float(158, 171); + +// float charging = current[map[PLUS_X]] + current[map[MINUS_X]] + current[map[PLUS_Y]] + current[map[MINUS_Y]] + current[map[PLUS_Z]] + current[map[MINUS_Z]]; + float charging = eclipse * (fabs(amps_max[0] * 0.707) + fabs(amps_max[1] * 0.707) + rnd_float(-4.0, 4.0)); + + current[map[BAT]] = ((current[map[BUS]] * voltage[map[BUS]]) / (batt * 1.0)) - charging; + +// printf("charging: %f bat curr: %f bus curr: %f bat volt: %f bus volt: %f \n",charging, current[map[BAT]], current[map[BUS]], batt, voltage[map[BUS]]); + + batt -= (batt > 3.5) ? current[map[BAT]]/30000: current[map[BAT]]/3000; + if (batt < 3.0) + { + batt = 3.0; + NormalModeFailure = 1; + printf("Safe Mode!\n"); + } + if (batt > 4.5) + batt = 4.5; + + voltage[map[BAT]] = batt + rnd_float(-0.01, 0.01); + +// end of simulated telemetry +} + tlm[1][A] = (int)(voltage[map[BUS]] /15.0 + 0.5) % 100; // Current of 5V supply to Pi + tlm[1][B] = (int) (99.5 - current[map[PLUS_X]]/10.0) % 100; // +X current [4] + tlm[1][C] = (int) (99.5 - current[map[MINUS_X]]/10.0) % 100; // X- current [10] + tlm[1][D] = (int) (99.5 - current[map[PLUS_Y]]/10.0) % 100; // +Y current [7] + + tlm[2][A] = (int) (99.5 - current[map[MINUS_Y]]/10.0) % 100; // -Y current [10] + tlm[2][B] = (int) (99.5 - current[map[PLUS_Z]]/10.0) % 100; // +Z current [10] // was 70/2m transponder power, AO-7 didn't have a Z panel + tlm[2][C] = (int) (99.5 - current[map[MINUS_Z]]/10.0) % 100; // -Z current (was timestamp) + tlm[2][D] = (int)(50.5 + current[map[BAT]]/10.0) % 100; // NiMH Battery current + + tlm[3][A] = abs((int)((voltage[map[BAT]] * 10.0) - 65.5) % 100); + tlm[3][B] = (int)(voltage[map[BUS]] * 10.0) % 100; // 5V supply to Pi + + tlm[4][B] = (int)((95.8 - cpuTemp)/1.48 + 0.5) % 100; + tlm[6][B] = 0 ; tlm[6][D] = 49 + rand() % 3;