357 lines
10 KiB
C
357 lines
10 KiB
C
//
|
||
// Created by cfif on 05.05.23.
|
||
//
|
||
#include "MainModesArbiter_Private.h"
|
||
#include <stdlib.h>
|
||
#include "dig_pot.h"
|
||
|
||
/*
|
||
|
||
// Функция расчета D для Rwa
|
||
int32_t calc_D_for_Rwa(float Rwa_desired, float Rab, float Rw) {
|
||
float D_float = 256.0f - ((Rwa_desired - Rw) / Rab) * 256.0f;
|
||
int32_t D = (int32_t) (D_float + 0.5f); // Округление
|
||
|
||
if (D > 255) D = 255;
|
||
if (D < 0) D = 0;
|
||
|
||
return D;
|
||
}
|
||
|
||
// Параметры
|
||
|
||
const float R_const = 130.0f;
|
||
const float Rw_typ = 00.0f; //60.0f // Типовое сопротивление движка
|
||
const float Rw_max = 120.0f; // Максимальное (для консервативного расчета)
|
||
|
||
int32_t CalculateSteps(float Rout_required, uint8_t *CountStep100, uint8_t *CountStep10) {
|
||
const float R_ab_100k = 100000.0f;
|
||
const float R_ab_10k = 10000.0f;
|
||
const float R_w_100k = 120.0f;
|
||
const float R_w_10k = 120.0f;
|
||
const float R__const = R_w_100k + R_w_10k; // 150 Ом — всегда присутствует
|
||
|
||
const float Step100 = R_ab_100k / 256.0f; // ~390.625 Ом
|
||
const float Step10 = R_ab_10k / 256.0f; // ~39.0625 Ом
|
||
|
||
|
||
|
||
// Вычитаем постоянную составляющую
|
||
float Rout = Rout_required - R__const;
|
||
if (Rout < 0.0f) Rout = 0.0f;
|
||
|
||
// Расчёт грубого каскада (без вычитания R_w — он уже в R_const)
|
||
*CountStep100 = (int32_t)(Rout / Step100);
|
||
if (*CountStep100 > 255) *CountStep100 = 255;
|
||
if (*CountStep100 < 0) *CountStep100 = 0;
|
||
|
||
// Остаток для точного каскада
|
||
float R_tail = Rout - (float)(*CountStep100) * Step100;
|
||
if (R_tail < 0.0f) R_tail = 0.0f;
|
||
|
||
// Расчёт точного каскада с округлением
|
||
*CountStep10 = (int32_t)(R_tail / Step10 + 0.5f);
|
||
if (*CountStep10 > 255) *CountStep10 = 255;
|
||
if (*CountStep10 < 0) *CountStep10 = 0;
|
||
|
||
return 0;
|
||
}
|
||
*/
|
||
|
||
|
||
void Mma_Init(
|
||
tMma *env,
|
||
tGpios *gpios,
|
||
tSerialPorts *serialPorts,
|
||
tSerialLinPorts *serialLinPorts,
|
||
tCanPorts *canPorts,
|
||
tSpiPorts *spiPorts,
|
||
tRtcs *rtcs
|
||
) {
|
||
env->gpios = gpios;
|
||
env->serialPorts = serialPorts;
|
||
env->serialLinPorts = serialLinPorts;
|
||
env->canPorts = canPorts;
|
||
env->spiPorts = spiPorts;
|
||
env->rtcs = rtcs;
|
||
|
||
InitThreadAtrStatic(&env->thread.attr, "Mma", env->thread.controlBlock, env->thread.stack, osPriorityNormal);
|
||
env->thread.id = 0;
|
||
}
|
||
|
||
static _Noreturn void Mma_Thread(tMma *env) {
|
||
|
||
// Запуск устройства
|
||
Mma_InitStage(env);
|
||
|
||
|
||
/*
|
||
uint8_t step100, step10;
|
||
|
||
float required_resistance = 10860;
|
||
|
||
|
||
step100 = 255 - step100;
|
||
step10 = 255 - step10;
|
||
|
||
result = setSpiSensor(&env->sensorSPI, SENSOR_TYPE_A, SENSOR_ADR_1, step100);
|
||
result = setSpiSensor(&env->sensorSPI, SENSOR_TYPE_A, SENSOR_ADR_2, step10);
|
||
|
||
|
||
required_resistance = 0.03f; // DUCT1
|
||
CalculateSteps(required_resistance, &step100, &step10);
|
||
|
||
step100 = 255 - 20;
|
||
step10 = 255 - 20;
|
||
|
||
result = setSpiSensor(&env->sensorSPI, SENSOR_TYPE_A, SENSOR_ADR_1, step100);
|
||
result = setSpiSensor(&env->sensorSPI, SENSOR_TYPE_A, SENSOR_ADR_2, step10);
|
||
|
||
|
||
|
||
required_resistance = 3000.03f; // DUCT1
|
||
CalculateSteps(required_resistance, &step100, &step10);
|
||
|
||
step100 = 255 - step100;
|
||
step10 = 255 - step10;
|
||
|
||
result = setSpiSensor(&env->sensorSPI, SENSOR_TYPE_A, SENSOR_ADR_1, step100);
|
||
result = setSpiSensor(&env->sensorSPI, SENSOR_TYPE_A, SENSOR_ADR_2, step10);
|
||
|
||
|
||
required_resistance = 4000.03f; // DUCT2
|
||
CalculateSteps(required_resistance, &step100, &step10);
|
||
|
||
step100 = 255 - step100;
|
||
step10 = 255 - step10;
|
||
|
||
result = setSpiSensor(&env->sensorSPI, SENSOR_TYPE_A, SENSOR_ADR_3, step100);
|
||
result = setSpiSensor(&env->sensorSPI, SENSOR_TYPE_A, SENSOR_ADR_4, step10);
|
||
|
||
|
||
required_resistance = 5000.03f; // DUCT3
|
||
CalculateSteps(required_resistance, &step100, &step10);
|
||
|
||
step100 = 255 - step100;
|
||
step10 = 255 - step10;
|
||
|
||
result = setSpiSensor(&env->sensorSPI, SENSOR_TYPE_A, SENSOR_ADR_5, step100);
|
||
result = setSpiSensor(&env->sensorSPI, SENSOR_TYPE_A, SENSOR_ADR_6, step10);
|
||
|
||
|
||
required_resistance = 6000.03f; // DUCT4
|
||
CalculateSteps(required_resistance, &step100, &step10);
|
||
|
||
step100 = 255 - step100;
|
||
step10 = 255 - step10;
|
||
|
||
result = setSpiSensor(&env->sensorSPI, SENSOR_TYPE_A, SENSOR_ADR_7, step100);
|
||
result = setSpiSensor(&env->sensorSPI, SENSOR_TYPE_A, SENSOR_ADR_8, step10);
|
||
|
||
|
||
required_resistance = 7000.03f; // DUCT5
|
||
CalculateSteps(required_resistance, &step100, &step10);
|
||
|
||
step100 = 255 - step100;
|
||
step10 = 255 - step10;
|
||
|
||
result = setSpiSensor(&env->sensorSPI, SENSOR_TYPE_A, SENSOR_ADR_9, step100);
|
||
result = setSpiSensor(&env->sensorSPI, SENSOR_TYPE_A, SENSOR_ADR_10, step10);
|
||
|
||
|
||
required_resistance = 8000.03f; // DUCT6
|
||
CalculateSteps(required_resistance, &step100, &step10);
|
||
|
||
step100 = 255 - step100;
|
||
step10 = 255 - step10;
|
||
|
||
result = setSpiSensor(&env->sensorSPI, SENSOR_TYPE_A, SENSOR_ADR_11, step100);
|
||
result = setSpiSensor(&env->sensorSPI, SENSOR_TYPE_A, SENSOR_ADR_12, step10);
|
||
|
||
|
||
required_resistance = 9000.03f; // EVAP TEMP F
|
||
CalculateSteps(required_resistance, &step100, &step10);
|
||
|
||
step100 = 255 - step100;
|
||
step10 = 255 - step10;
|
||
|
||
result = setSpiSensor(&env->sensorSPI, SENSOR_TYPE_A, SENSOR_ADR_13, step100);
|
||
result = setSpiSensor(&env->sensorSPI, SENSOR_TYPE_A, SENSOR_ADR_14, step10);
|
||
|
||
|
||
|
||
//------------------
|
||
|
||
|
||
required_resistance = 10000.03f; // DUCT1
|
||
CalculateSteps(required_resistance, &step100, &step10);
|
||
|
||
step100 = 255 - step100;
|
||
step10 = 255 - step10;
|
||
|
||
result = setSpiSensor(&env->sensorSPI, SENSOR_TYPE_B, SENSOR_ADR_1, step100);
|
||
result = setSpiSensor(&env->sensorSPI, SENSOR_TYPE_B, SENSOR_ADR_2, step10);
|
||
|
||
|
||
required_resistance = 11000.03f; // DUCT2
|
||
CalculateSteps(required_resistance, &step100, &step10);
|
||
|
||
step100 = 255 - step100;
|
||
step10 = 255 - step10;
|
||
|
||
result = setSpiSensor(&env->sensorSPI, SENSOR_TYPE_B, SENSOR_ADR_3, step100);
|
||
result = setSpiSensor(&env->sensorSPI, SENSOR_TYPE_B, SENSOR_ADR_4, step10);
|
||
|
||
|
||
required_resistance = 12000.03f; // DUCT3
|
||
CalculateSteps(required_resistance, &step100, &step10);
|
||
|
||
step100 = 255 - step100;
|
||
step10 = 255 - step10;
|
||
|
||
result = setSpiSensor(&env->sensorSPI, SENSOR_TYPE_B, SENSOR_ADR_5, step100);
|
||
result = setSpiSensor(&env->sensorSPI, SENSOR_TYPE_B, SENSOR_ADR_6, step10);
|
||
|
||
|
||
required_resistance = 13000.03f; // DUCT4
|
||
CalculateSteps(required_resistance, &step100, &step10);
|
||
|
||
step100 = 255 - step100;
|
||
step10 = 255 - step10;
|
||
|
||
result = setSpiSensor(&env->sensorSPI, SENSOR_TYPE_B, SENSOR_ADR_7, step100);
|
||
result = setSpiSensor(&env->sensorSPI, SENSOR_TYPE_B, SENSOR_ADR_8, step10);
|
||
|
||
|
||
required_resistance = 14000.03f; // EVAP TEMP R
|
||
CalculateSteps(required_resistance, &step100, &step10);
|
||
|
||
step100 = 255 - step100;
|
||
step10 = 255 - step10;
|
||
|
||
result = setSpiSensor(&env->sensorSPI, SENSOR_TYPE_B, SENSOR_ADR_9, step100);
|
||
result = setSpiSensor(&env->sensorSPI, SENSOR_TYPE_B, SENSOR_ADR_10, step10);
|
||
|
||
*/
|
||
asm("nop");
|
||
|
||
|
||
// D100 = (int32_t)(255.0f - (R / Step100));
|
||
// D10 = (int32_t)(255.0f - (R / Step10));
|
||
|
||
|
||
// D100 = 255 - (R / 390.625);
|
||
// int32_t div
|
||
|
||
|
||
// result = setSpiSensor(&env->sensorSPI, SENSOR_TYPE_A, SENSOR_ADR_1, 255-27);
|
||
// result = setSpiSensor(&env->sensorSPI, SENSOR_TYPE_A, SENSOR_ADR_2, 255-19);
|
||
|
||
// result = setSpiSensor(&env->sensorSPI, SENSOR_TYPE_A, SENSOR_ADR_1, step100);
|
||
// result = setSpiSensor(&env->sensorSPI, SENSOR_TYPE_A, SENSOR_ADR_2, step10);
|
||
|
||
uint32_t step = 0;
|
||
|
||
for (;;) {
|
||
|
||
/*
|
||
Rwa_desired_100k += 1000.0f;
|
||
D100 = calc_D_for_Rwa(Rwa_desired_100k, R_ab_100k, Rw_typ);
|
||
|
||
if (Rwa_desired_100k > R_ab_100k) {
|
||
Rwa_desired_100k = 0;
|
||
}
|
||
|
||
|
||
Rwa_desired_10k += 1000.0f;
|
||
D10 = calc_D_for_Rwa(Rwa_desired_10k, R_ab_10k, Rw_typ);
|
||
|
||
if (Rwa_desired_10k > R_ab_10k) {
|
||
Rwa_desired_10k = 0;
|
||
}
|
||
*/
|
||
|
||
|
||
// 27
|
||
// result = setSpiSensor(&env->sensorSPI, SENSOR_TYPE_A, SENSOR_ADR_1, step100);
|
||
asm("nop");
|
||
// ++step;
|
||
|
||
// if (step> 255)
|
||
// step =0;
|
||
|
||
// 19
|
||
// result = setSpiSensor(&env->sensorSPI, SENSOR_TYPE_A, SENSOR_ADR_2, step10);
|
||
asm("nop");
|
||
|
||
|
||
GpioPinSet(&env->gpios->Led.ledR1, true);
|
||
SystemDelayMs(1000);
|
||
GpioPinSet(&env->gpios->Led.ledR1, false);
|
||
SystemDelayMs(1000);
|
||
|
||
/*
|
||
result = setSpiSensor(&env->sensorSPI, SENSOR_TYPE_A, SENSOR_ADR_1, D100);
|
||
asm("nop");
|
||
result = setSpiSensor(&env->sensorSPI, SENSOR_TYPE_A, SENSOR_ADR_2, D10);
|
||
asm("nop");
|
||
result = setSpiSensor(&env->sensorSPI, SENSOR_TYPE_A, SENSOR_ADR_3, D100);
|
||
asm("nop");
|
||
result = setSpiSensor(&env->sensorSPI, SENSOR_TYPE_A, SENSOR_ADR_4, D10);
|
||
asm("nop");
|
||
*/
|
||
/*
|
||
|
||
|
||
GpioPinSet(&env->gpios->Led.ledR1, true);
|
||
SystemDelayMs(1000);
|
||
GpioPinSet(&env->gpios->Led.ledR1, false);
|
||
SystemDelayMs(1000);
|
||
|
||
data = 0;
|
||
result = setSpiSensor(&env->sensorSPI, data);
|
||
*/
|
||
/*
|
||
GpioPinSet(&env->gpios->Led.ledR1, true);
|
||
SystemDelayMs(1000);
|
||
GpioPinSet(&env->gpios->Led.ledR1, false);
|
||
SystemDelayMs(1000);
|
||
|
||
uint16_t data = 255;
|
||
bool result = setSpiSensor(&env->sensorSPI, data);
|
||
|
||
GpioPinSet(&env->gpios->Led.ledR1, true);
|
||
SystemDelayMs(1000);
|
||
GpioPinSet(&env->gpios->Led.ledR1, false);
|
||
SystemDelayMs(1000);
|
||
|
||
data = 128;
|
||
result = setSpiSensor(&env->sensorSPI, data);
|
||
|
||
GpioPinSet(&env->gpios->Led.ledR1, true);
|
||
SystemDelayMs(1000);
|
||
GpioPinSet(&env->gpios->Led.ledR1, false);
|
||
SystemDelayMs(1000);
|
||
|
||
data = 1;
|
||
result = setSpiSensor(&env->sensorSPI, data);
|
||
|
||
GpioPinSet(&env->gpios->Led.ledR1, true);
|
||
SystemDelayMs(1000);
|
||
GpioPinSet(&env->gpios->Led.ledR1, false);
|
||
SystemDelayMs(1000);
|
||
|
||
data = 0;
|
||
result = setSpiSensor(&env->sensorSPI, data);
|
||
*/
|
||
}
|
||
}
|
||
|
||
|
||
void Mma_StartThread(tMma *env) {
|
||
if (!env->thread.id) {
|
||
env->thread.id = osThreadNew((osThreadFunc_t) (Mma_Thread), (void *) (env), &env->thread.attr);
|
||
} else {
|
||
osThreadResume(env->thread.id);
|
||
}
|
||
} |