From af945401f796486fd4ff965b319988ccd2bf3220 Mon Sep 17 00:00:00 2001 From: alanbjohnston Date: Sat, 26 Dec 2020 16:16:33 -0500 Subject: [PATCH] moved sim_mode code --- afsk/main.c | 161 +++++++++++++++++++++++++++------------------------- 1 file changed, 83 insertions(+), 78 deletions(-) diff --git a/afsk/main.c b/afsk/main.c index 2678b065..899c2d58 100644 --- a/afsk/main.c +++ b/afsk/main.c @@ -1065,7 +1065,88 @@ void get_tlm_fox() { sampleTime = (int) millis(); } else printf("first time - no sleep\n"); + + + float voltage[9], current[9], sensor[17], other[3]; + memset(voltage, 0, sizeof(voltage)); + memset(current, 0, sizeof(current)); + memset(sensor, 0, sizeof(sensor)); + memset(other, 0, sizeof(other)); + + if (sim_mode) { + // simulated telemetry + + double time = ((long int)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] * (float) sin(2.0 * 3.14 * time / (46.0 * speed)) + rnd_float(-2, 2); + double Yi = eclipse * amps_max[1] * (float) sin((2.0 * 3.14 * time / (46.0 * speed)) + (3.14 / 2.0)) + rnd_float(-2, 2); + double Zi = eclipse * amps_max[2] * (float) sin((2.0 * 3.14 * time / (46.0 * speed)) + 3.14 + angle[2]) + rnd_float(-2, 2); + + double Xv = eclipse * volts_max[0] * (float) sin(2.0 * 3.14 * time / (46.0 * speed)) + rnd_float(-0.2, 0.2); + double Yv = eclipse * volts_max[1] * (float) 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] * (float) 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.0f) * Xi); + current[map[PLUS_Y]] = (Yi >= 0) ? Yi : 0; + current[map[MINUS_Y]] = (Yi >= 0) ? 0 : ((-1.0f) * Yi); + current[map[PLUS_Z]] = (Zi >= 0) ? Zi : 0; + current[map[MINUS_Z]] = (Zi >= 0) ? 0 : ((-1.0f) * Zi); + + voltage[map[PLUS_X]] = (Xv >= 1) ? Xv : rnd_float(0.9, 1.1); + voltage[map[MINUS_X]] = (Xv <= -1) ? ((-1.0f) * 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.0f) * 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.0f) * 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.0f) : ((temp_min - tempS) / 50.0f); + tempS += +rnd_float(-1.0, 1.0); + // IHUcpuTemp = (int)((tempS + rnd_float(-1.0, 1.0)) * 10 + 0.5); + other[IHU_TEMP] = tempS; + + 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) - 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"); + } else + NormalModeFailure = 0; + + if (batt > 4.5) + batt = 4.5; + + voltage[map[BAT]] = batt + rnd_float(-0.01, 0.01); + + // end of simulated telemetry + } + +else { int count1; char * token; char cmdbuffer[1000]; @@ -1078,11 +1159,7 @@ void get_tlm_fox() { const char space[2] = " "; token = strtok(cmdbuffer, space); - float voltage[9], current[9], sensor[17], other[3]; - memset(voltage, 0, sizeof(voltage)); - memset(current, 0, sizeof(current)); - memset(sensor, 0, sizeof(sensor)); - memset(other, 0, sizeof(other)); + for (count1 = 0; count1 < 8; count1++) { if (token != NULL) { @@ -1192,79 +1269,7 @@ void get_tlm_fox() { } } - - if (sim_mode) { - // simulated telemetry - - double time = ((long int)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] * (float) sin(2.0 * 3.14 * time / (46.0 * speed)) + rnd_float(-2, 2); - double Yi = eclipse * amps_max[1] * (float) sin((2.0 * 3.14 * time / (46.0 * speed)) + (3.14 / 2.0)) + rnd_float(-2, 2); - double Zi = eclipse * amps_max[2] * (float) sin((2.0 * 3.14 * time / (46.0 * speed)) + 3.14 + angle[2]) + rnd_float(-2, 2); - - double Xv = eclipse * volts_max[0] * (float) sin(2.0 * 3.14 * time / (46.0 * speed)) + rnd_float(-0.2, 0.2); - double Yv = eclipse * volts_max[1] * (float) 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] * (float) 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.0f) * Xi); - current[map[PLUS_Y]] = (Yi >= 0) ? Yi : 0; - current[map[MINUS_Y]] = (Yi >= 0) ? 0 : ((-1.0f) * Yi); - current[map[PLUS_Z]] = (Zi >= 0) ? Zi : 0; - current[map[MINUS_Z]] = (Zi >= 0) ? 0 : ((-1.0f) * Zi); - - voltage[map[PLUS_X]] = (Xv >= 1) ? Xv : rnd_float(0.9, 1.1); - voltage[map[MINUS_X]] = (Xv <= -1) ? ((-1.0f) * 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.0f) * 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.0f) * 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.0f) : ((temp_min - tempS) / 50.0f); - tempS += +rnd_float(-1.0, 1.0); - // IHUcpuTemp = (int)((tempS + rnd_float(-1.0, 1.0)) * 10 + 0.5); - other[IHU_TEMP] = tempS; - - 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) - 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"); - } else - NormalModeFailure = 0; - - if (batt > 4.5) - batt = 4.5; - - voltage[map[BAT]] = batt + rnd_float(-0.01, 0.01); - - // end of simulated telemetry - } + } for (count1 = 0; count1 < 8; count1++) { if (voltage[count1] < voltage_min[count1])