diff --git a/AgroBase/AgroBase/Forms/Operacoes/frmOperacaoManual.cs b/AgroBase/AgroBase/Forms/Operacoes/frmOperacaoManual.cs index 452940863..27e89f21d 100644 --- a/AgroBase/AgroBase/Forms/Operacoes/frmOperacaoManual.cs +++ b/AgroBase/AgroBase/Forms/Operacoes/frmOperacaoManual.cs @@ -83,8 +83,8 @@ namespace AgroBase.Forms.Operacoes } chbPulverizador.Checked = pC.PulverizadorAutomatico; - chbSonarAtivado.Checked = pC.MovAuxilioSonar; - chbSonarAtivado.Checked = pC.DirAuxilioSonar; + chbSonarAtivado.Checked = pC.MovAuxilioSonar || pC.DirAuxilioSonar; + chbImuAtivado.Checked = pC.ImuAuxilioMovimento || pC.ImuAuxilioDirecional; InicializarComandosDirecional(); PopularInformacoesGerais(); @@ -204,6 +204,13 @@ namespace AgroBase.Forms.Operacoes chbSonarAtivado.Checked = false; } + bool imuAtivado = Variaveis.OperacaoEmAndamento.Sensoriamento.IMU.saude.status == StatusModulo.Operante; + chbImuAtivado.Enabled = imuAtivado; + if (!imuAtivado) + { + chbImuAtivado.Checked = false; + } + if (!ReqFrameCam) _ = AtualizarCameraSolo(); diff --git a/Firmware/Modulos/I2CService.h b/Firmware/Modulos/I2CService.h index 888ebb4f1..f628aa062 100644 --- a/Firmware/Modulos/I2CService.h +++ b/Firmware/Modulos/I2CService.h @@ -144,8 +144,14 @@ class I2CService { MostrarLog("I2C", "Iniciando Wire..."); I2CIniciado = Wire.begin(_pinoSDA, _pinoSCL); + MostrarLog( + "I2C", + "Wire configurado | clock=" + String(Wire.getClock()) + + " timeout=" + String(Wire.getTimeOut()) + "ms" + ); if (I2CIniciado) { + Wire.setClock(100000); Wire.setTimeOut(TimeoutWireMs); } @@ -465,6 +471,19 @@ class I2CService { bool precisaRecuperar = _recoveryPendente; String motivo = _motivoRecoveryPendente; + ContextoBarramento contexto = _contextoAtual; + + if ( + contexto.valido && + contexto.canalMuxSolicitado >= 0 && + !_recoveryPendente + ) { + DesabilitarMuxAtualSemMutex(); + + precisaRecuperar = _recoveryPendente; + motivo = _motivoRecoveryPendente; + } + if (precisaRecuperar) { CapturarContextoFalha(); i2cMutexReiniciando = true; @@ -473,6 +492,7 @@ class I2CService { LimparContextoAtual(); idAtualUsandoI2C = -1; tempoEntradaI2C = 0; + xSemaphoreGive(i2cMutex); if (precisaRecuperar) { @@ -668,6 +688,12 @@ class I2CService { } uint8_t canalGlobal = (indiceMux * 10) + canalMux; + + int canalAnterior = CanalGlobalAtivoAtual(); + int sdaAntes = digitalRead(_pinoSDA); + int sclAntes = digitalRead(_pinoSCL); + int rdyAntes = TCAReadyOk() ? 1 : 0; + uint32_t inicio = millis(); if (CanalMuxEmQuarentena(canalGlobal)) { MostrarLog("MUX", "Canal bloqueado por quarentena | canalGlobal=" + String(canalGlobal)); @@ -715,6 +741,26 @@ class I2CService { Wire.write((uint8_t)(1U << canalMux)); int erro = Wire.endTransmission(); + uint32_t duracao = (uint32_t)(millis() - inicio); + int sdaDepois = digitalRead(_pinoSDA); + int sclDepois = digitalRead(_pinoSCL); + int rdyDepois = TCAReadyOk() ? 1 : 0; + + MostrarLog( + "MUX", + "Troca" + " | anterior=" + String(canalAnterior) + + " solicitado=" + String(canalGlobal) + + " erro=" + String(erro) + + " duracao=" + String(duracao) + "ms" + + " antes[SDA=" + String(sdaAntes) + + " SCL=" + String(sclAntes) + + " RDY=" + String(rdyAntes) + "]" + + " depois[SDA=" + String(sdaDepois) + + " SCL=" + String(sclDepois) + + " RDY=" + String(rdyDepois) + "]" + ); + if (erro != 0) { MostrarLog( "MUX", @@ -785,7 +831,7 @@ class I2CService { int leituraADC = (int)ads.readADC_SingleEnded(canal); uint32_t duracaoLeitura = (uint32_t)(millis() - inicioLeitura); - if (!TCAReadyOk() || duracaoLeitura > LimiteTempoI2C) { + if (!TCAReadyOk() || duracaoLeitura > LimiteLeituraADS_MS) { RegistrarFalhaTransacao( !TCAReadyOk() ? "READY caiu durante leitura ADS" @@ -1015,6 +1061,7 @@ class I2CService { static constexpr uint32_t TimeoutWireMs = 80; static constexpr uint32_t LimiteReadyLowMs = 200; static constexpr uint32_t CooldownRecoveryMs = 1000; + static constexpr uint32_t LimiteLeituraADS_MS = 50; static constexpr uint32_t LimiteFalhasTransacaoRecovery = 3; static constexpr uint32_t MaxFalhasRecoveryAntesRestart = 3; static constexpr uint32_t JanelaQuarentenaMs = 120000; @@ -1205,10 +1252,18 @@ class I2CService { static void PrepararContextoAtual(int idSensor, int canalMuxSolicitado) { ContextoBarramento contexto; + contexto.valido = true; contexto.idSensor = idSensor; contexto.canalMuxSolicitado = canalMuxSolicitado; - contexto.canalMuxAtivo = CanalGlobalAtivoAtual(); + + // Uma transacao direta no barramento principal nao deve herdar + // o ultimo canal que ficou selecionado no MUX. + contexto.canalMuxAtivo = + canalMuxSolicitado >= 0 + ? CanalGlobalAtivoAtual() + : -1; + contexto.enderecoADS = 0xFF; contexto.inicioMs = millis(); @@ -1540,8 +1595,14 @@ class I2CService { if (barramentoLivre) { I2CIniciado = Wire.begin(_pinoSDA, _pinoSCL); + MostrarLog( + "I2C", + "Wire configurado | clock=" + String(Wire.getClock()) + + " timeout=" + String(Wire.getTimeOut()) + "ms" + ); if (I2CIniciado) { + Wire.setClock(100000); Wire.setTimeOut(TimeoutWireMs); wireReiniciado = true; } @@ -1685,6 +1746,47 @@ class I2CService { DelayMs(50); } } + + static bool DesabilitarMuxAtualSemMutex() { + if (enderecoMuxAtivo == 0xFF) { + MostrarLog("MUX", "Nenhum MUX marcado como ativo"); + canalAtivo = -1; + return true; + } + + const uint8_t endereco = enderecoMuxAtivo; + const int canalAnterior = canalAtivo; + + uint32_t inicio = millis(); + + Wire.beginTransmission(endereco); + Wire.write((uint8_t)0x00); + int erro = Wire.endTransmission(); + + uint32_t duracao = (uint32_t)(millis() - inicio); + + MostrarLog( + "MUX", + "Desabilitar canais" + " | anterior=" + String(canalAnterior) + + " erro=" + String(erro) + + " duracao=" + String(duracao) + "ms" + + " SDA=" + String(digitalRead(_pinoSDA)) + + " SCL=" + String(digitalRead(_pinoSCL)) + + " RDY=" + String(TCAReadyOk() ? 1 : 0) + ); + + if (erro != 0) { + RegistrarFalhaTransacao("Falha ao desabilitar canais do MUX"); + return false; + } + + enderecoMuxAtivo = 0xFF; + canalAtivo = -1; + + RegistrarSucessoTransacao(); + return true; + } }; bool I2CService::DebugMode = false; diff --git a/Firmware/Modulos/ServoAgroModel_v3.h b/Firmware/Modulos/ServoAgroModel_v3.h new file mode 100644 index 000000000..db62f9f37 --- /dev/null +++ b/Firmware/Modulos/ServoAgroModel_v3.h @@ -0,0 +1,511 @@ +#ifndef ServoAgroModel_v3 +#define ServoAgroModel_v3 + +#include + +#include "SerialService.h" +#include "Pinout.h" +#include "Utils.h" +#include "IComponenteCAN.h" + +/* + * Controle de servo diretamente pelo periférico LEDC do ESP32-S3. + * + * Não depende da biblioteca ESP32Servo. + * + * Cada servo precisa utilizar um canal LEDC exclusivo. + * + * ESP32-S3: + * canais LEDC válidos: 0 a 7 + * resolução máxima: 14 bits + */ +class ServoAgro_v3 : public ComponenteCAN { + public: + String ID; + uint8_t ID_Num = 0; + bool Incremental = false; + bool Iniciado = false; + PinoModel _pino; + + ServoAgro_v3() {} + + ServoAgro_v3( + const String& id, + uint8_t idNum, + bool incremental, + int pino, + TiposBarramentos tipoBarramento, + uint8_t canalPwm, + uint16_t pulsoMinUs = 544, + uint16_t pulsoMaxUs = 2400 + ) : + ID(id), + ID_Num(idNum), + Incremental(incremental), + _pino(PinoModel(pino, tipoBarramento, _OUTPUT)), + _canalPwm(canalPwm), + _pulsoMinUs(pulsoMinUs), + _pulsoMaxUs(pulsoMaxUs) + { + } + + void Inicializar() { + MostrarLog( + "Entrou no Inicializar" + " | pino=" + String(_pino.num) + + " | canalPWM=" + String(_canalPwm) + + " | incremental=" + String(Incremental ? 1 : 0) + ); + + if (Iniciado) { + MostrarLog("Servo ja inicializado"); + return; + } + + if (!_pino.Definido()) { + MostrarLog("Servo nao iniciado | pino nao definido"); + return; + } + + if (_canalPwm >= QuantidadeCanaisLedc) { + MostrarLog( + "Servo nao iniciado | canal LEDC invalido: " + + String(_canalPwm) + ); + return; + } + + if (_pulsoMinUs >= _pulsoMaxUs) { + MostrarLog( + "Servo nao iniciado | intervalo de pulso invalido" + ); + return; + } + + if (!ReservarCanal(_canalPwm)) { + MostrarLog( + "Servo nao iniciado | canal LEDC ja reservado: " + + String(_canalPwm) + ); + return; + } + + _canalReservado = true; + + MostrarLog( + "Configurando LEDC" + " | frequencia=" + String(FrequenciaHz) + + "Hz resolucao=" + String(ResolucaoBits) + + "bits" + ); + + double frequenciaReal = ledcSetup( + _canalPwm, + FrequenciaHz, + ResolucaoBits + ); + + if (frequenciaReal <= 0.0) { + MostrarLog( + "Falha no ledcSetup" + " | canal=" + String(_canalPwm) + ); + + LiberarRecursos(); + return; + } + + _frequenciaRealHz = static_cast(frequenciaReal); + + MostrarLog( + "LEDC configurado" + " | frequencia_real=" + String(_frequenciaRealHz) + + "Hz" + ); + + ledcAttachPin( + static_cast(_pino.num), + _canalPwm + ); + + _pinoAnexado = true; + + MostrarLog( + "Pino anexado ao LEDC" + " | pino=" + String(_pino.num) + + " canal=" + String(_canalPwm) + ); + + /* + * Primeiro aplica um duty válido. + * + * No Arduino-ESP32 2.x, ledcReadFreq() retorna zero + * quando o duty atual do canal é zero. + */ + int anguloInicial = Incremental ? 0 : 180; + + if (!AplicarAngulo(anguloInicial, true)) { + MostrarLog("Falha ao aplicar angulo inicial"); + LiberarRecursos(); + return; + } + + /* + * Agora o canal já possui duty diferente de zero, + * então ledcReadFreq() consegue retornar a frequência. + */ + uint32_t frequenciaConfirmada = ledcReadFreq(_canalPwm); + if (frequenciaConfirmada != FrequenciaHz) { + LiberarRecursos(); + return; + } + + MostrarLog( + "PWM confirmado" + " | frequencia=" + String(frequenciaConfirmada) + "Hz" + + " duty=" + String(ledcRead(_canalPwm)) + + " angulo=" + String(_Angulo) + ); + + if (frequenciaConfirmada == 0) { + MostrarLog( + "Aviso: nao foi possivel ler a frequencia, " + "mas o ledcSetup retornou " + + String(_frequenciaRealHz) + "Hz" + ); + } + + Iniciado = true; + + MostrarLog( + "Servo iniciado" + " | angulo=" + String(_Angulo) + + " pulso=" + String(_pulsoAtualUs) + "us" + + " duty=" + String(_dutyAtual) + ); + } + + void Desligar() { + if (!Iniciado && !_pinoAnexado && !_canalReservado) { + MostrarLog("Servo nao esta inicializado"); + return; + } + + /* + * Duty zero interrompe os pulsos antes de desconectar o pino. + */ + if (_canalReservado) { + ledcWrite(_canalPwm, 0); + } + + delay(2); + + LiberarRecursos(); + + Iniciado = false; + + MostrarLog("Servo desligado"); + } + + void RequisitarDados() { + if (!Iniciado) { + return; + } + } + + void ProcessarComando(std::vector& data) { + if (!Iniciado) { + return; + } + + if (data.size() < 3) { + return; + } + + CanMessagePosicaoDados posicao = + static_cast(data[0]); + + uint8_t idNum = data[1]; + + if (idNum != ID_Num) { + return; + } + + switch (posicao) { + case CanMessagePosicaoDados::Command1: { + int novoAngulo = static_cast(data[2]); + AtualizarAngulo(novoAngulo); + break; + } + + default: + break; + } + } + + std::vector MontarMensagemCAN( + CanMessagePosicaoDados posicao + ) { + std::vector data; + + data.push_back( + static_cast(posicao) + ); + + data.push_back(ID_Num); + + switch (posicao) { + case CanMessagePosicaoDados::Status: { + data.push_back(Iniciado ? 1 : 0); + break; + } + + case CanMessagePosicaoDados::Dados1: { + data.push_back( + static_cast( + constrain(_Angulo, 0, 180) + ) + ); + break; + } + + default: + break; + } + + return data; + } + + int ObterAngulo() const { + return _Angulo; + } + + uint16_t ObterPulsoUs() const { + return _pulsoAtualUs; + } + + uint32_t ObterDuty() const { + return _dutyAtual; + } + + uint8_t ObterCanalPwm() const { + return _canalPwm; + } + + private: + /* + * O ESP32-S3 possui 8 canais LEDC. + */ + static constexpr uint8_t QuantidadeCanaisLedc = 8; + + /* + * O ESP32-S3 suporta no máximo 14 bits no LEDC. + */ + static constexpr uint8_t ResolucaoBits = 14; + + /* + * Servos convencionais utilizam período de 20 ms. + */ + static constexpr uint32_t FrequenciaHz = 50; + + bool DebugMode = false; + + uint8_t _canalPwm = 0xFF; + + uint16_t _pulsoMinUs = 544; + uint16_t _pulsoMaxUs = 2400; + + uint32_t _frequenciaRealHz = 0; + + bool _canalReservado = false; + bool _pinoAnexado = false; + + int _Angulo = 0; + + uint16_t _pulsoAtualUs = 0; + uint32_t _dutyAtual = 0; + bool _sinalAplicado = false; + + /* + * Um único mapa é compartilhado entre todas as instâncias + * de ServoAgro_v3. + */ + static bool* CanaisReservados() { + static bool canais[QuantidadeCanaisLedc] = { + false, false, false, false, + false, false, false, false + }; + + return canais; + } + + static bool ReservarCanal(uint8_t canal) { + if (canal >= QuantidadeCanaisLedc) { + return false; + } + + bool* canais = CanaisReservados(); + + if (canais[canal]) { + return false; + } + + canais[canal] = true; + return true; + } + + static void LiberarCanal(uint8_t canal) { + if (canal >= QuantidadeCanaisLedc) { + return; + } + + CanaisReservados()[canal] = false; + } + + static uint32_t DutyMaximo() { + return (1UL << ResolucaoBits) - 1UL; + } + + static uint32_t PeriodoUs() { + return 1000000UL / FrequenciaHz; + } + + uint16_t AnguloParaPulsoUs(int angulo) const { + angulo = constrain(angulo, 0, 180); + + uint32_t intervalo = + static_cast(_pulsoMaxUs - _pulsoMinUs); + + uint32_t pulso = + static_cast(_pulsoMinUs) + + ( + static_cast(angulo) * + intervalo + ) / 180UL; + + return static_cast(pulso); + } + + uint32_t PulsoUsParaDuty(uint16_t pulsoUs) const { + /* + * duty = pulso / período * duty máximo + * + * A soma de PeriodoUs()/2 realiza arredondamento + * inteiro em vez de simplesmente truncar. + */ + uint64_t numerador = + static_cast(pulsoUs) * + static_cast(DutyMaximo()); + + numerador += PeriodoUs() / 2UL; + + return static_cast( + numerador / PeriodoUs() + ); + } + + bool AplicarAngulo(int novoAngulo, bool confirmar = false) { + if (!_canalReservado || !_pinoAnexado) { + MostrarLog( + "Nao foi possivel atualizar angulo" + " | PWM nao inicializado" + ); + + return false; + } + + novoAngulo = constrain(novoAngulo, 0, 180); + + /* + * Só ignora uma posição repetida se um sinal já tiver + * sido realmente aplicado ao canal LEDC. + */ + if (_sinalAplicado && novoAngulo == _Angulo) { + return true; + } + + uint16_t pulsoUs = + AnguloParaPulsoUs(novoAngulo); + + uint32_t duty = + PulsoUsParaDuty(pulsoUs); + + ledcWrite(_canalPwm, duty); + + if (confirmar) { + /* + * Frequência de 50 Hz: cada período possui 20 ms. + */ + vTaskDelay(pdMS_TO_TICKS(25)); + + uint32_t dutyConfirmado = + ledcRead(_canalPwm); + + if (dutyConfirmado != duty) { + MostrarLog( + "Falha ao confirmar duty" + " | esperado=" + String(duty) + + " lido=" + String(dutyConfirmado) + ); + + return false; + } + } + + _Angulo = novoAngulo; + _pulsoAtualUs = pulsoUs; + _dutyAtual = duty; + _sinalAplicado = true; + + return true; + } + + void AtualizarAngulo(int novoAngulo) { + if (!Iniciado) { + MostrarLog( + "Comando de angulo ignorado" + " | servo nao iniciado" + ); + + return; + } + + AplicarAngulo(novoAngulo); + } + + void LiberarRecursos() { + if (_pinoAnexado && _pino.Definido()) { + ledcDetachPin( + static_cast(_pino.num) + ); + + /* + * Depois do detach deixa o sinal explicitamente baixo. + */ + pinMode(_pino.num, OUTPUT); + digitalWrite(_pino.num, LOW); + + _pinoAnexado = false; + } + + if (_canalReservado) { + LiberarCanal(_canalPwm); + _canalReservado = false; + } + + _frequenciaRealHz = 0; + _pulsoAtualUs = 0; + _dutyAtual = 0; + _sinalAplicado = false; + } + + void MostrarLog(const String& mensagem) { + if (DebugMode) { + PrintTela( + "[SRV " + ID + "] " + mensagem + ); + } + } +}; + +#endif \ No newline at end of file diff --git a/Firmware/Sensoriamento/Sensoriamento_v23/Sensoriamento_v23.ino b/Firmware/Sensoriamento/Sensoriamento_v23/Sensoriamento_v23.ino index 7537d7812..782c06591 100644 --- a/Firmware/Sensoriamento/Sensoriamento_v23/Sensoriamento_v23.ino +++ b/Firmware/Sensoriamento/Sensoriamento_v23/Sensoriamento_v23.ino @@ -9,7 +9,7 @@ #include "C:\ZendionInc\agrobot_base\Firmware\Modulos\I2CService.h" #include "C:\ZendionInc\agrobot_base\Firmware\Modulos\Utils.h" #include "C:\ZendionInc\agrobot_base\Firmware\Modulos\Pinout.h" -#include "C:\ZendionInc\agrobot_base\Firmware\Modulos\ServoAgroModel_v2.h" +#include "C:\ZendionInc\agrobot_base\Firmware\Modulos\ServoAgroModel_v3.h" #include "C:\ZendionInc\agrobot_base\Firmware\Modulos\ReleModel_v2.h" #include "C:\ZendionInc\agrobot_base\Firmware\Modulos\SinaleiroModel_v2.h" #include "C:\ZendionInc\agrobot_base\Firmware\Modulos\SensorTemperaturaNtcModel.h" @@ -62,10 +62,10 @@ void LedTask(void* pv) { #pragma endregion #pragma region SERVOS -ServoAgro_v2 sSRV_FreioET("FROET", 41, true, 45, TiposBarramentos::CONTROLADOR); -ServoAgro_v2 sSRV_FreioDT("FRODT", 43, false, 47, TiposBarramentos::CONTROLADOR); +ServoAgro_v3 sSRV_FreioET("FROET", 41, true, 45, TiposBarramentos::CONTROLADOR, 0); +ServoAgro_v3 sSRV_FreioDT("FRODT", 43, false, 47, TiposBarramentos::CONTROLADOR, 1); -ServoAgro_v2* ObterServoPorId(uint8_t idNum) { +ServoAgro_v3* ObterServoPorId(uint8_t idNum) { switch (idNum) { case 41: return &sSRV_FreioET; case 43: return &sSRV_FreioDT; @@ -75,8 +75,8 @@ ServoAgro_v2* ObterServoPorId(uint8_t idNum) { #pragma endregion #pragma region RELES -Rele_v2 sRLE_CoolerIn("RLCIN", 81, true, 19, TiposBarramentos::CONTROLADOR); -Rele_v2 sRLE_CoolerOut("RLCOUT", 82, true, 20, TiposBarramentos::CONTROLADOR); +Rele_v2 sRLE_CoolerIn("RLCIN", 81, true, 13, TiposBarramentos::CONTROLADOR); +Rele_v2 sRLE_CoolerOut("RLCOUT", 82, true, 14, TiposBarramentos::CONTROLADOR); Rele_v2* ObterRelePorId(uint8_t idNum) { switch (idNum) { @@ -152,7 +152,7 @@ void MostrarLog(String mensagem) { } void inicializarDependenciasI2C() { - I2CService::DefinirPinos(1, 2, 41, 42, 39, 40); + I2CService::DefinirPinos(1, 2, -1, -1, 39, 40); I2CService::IniciarI2C(); I2CService::IniciarADS(); I2CService::IniciarMUX(); @@ -193,6 +193,7 @@ void setup() { xTaskCreatePinnedToCore(LedTask, "LedTask", 3000, NULL, 1, NULL, tskNO_AFFINITY); // Registrar callback para processar mensagens CAN recebidas + MostrarLog("Registrando callback CAN..."); canService.setReceiveCallback([](int packetSize, int senderId, CanMessagePosicaoDados posicao, byte* data, int dataLength) { std::vector fullData; fullData.push_back(static_cast(posicao)); // Primeiro byte é sempre a posicao @@ -203,6 +204,7 @@ void setup() { ProcessarFrameCAN(posicao, fullData); }); + MostrarLog("Iniciando comunicação CAN..."); canService.begin(); } @@ -378,7 +380,7 @@ void ProcessarCmd(std::vector data) { break; } case sSRV: { - ServoAgro_v2* servo = ObterServoPorId(id); + ServoAgro_v3* servo = ObterServoPorId(id); if (servo == nullptr) return; servo->ProcessarComando(data);