Skip to content

Commit

Permalink
fix connecting in AP mode + fix async mode in esc driver + docs update
Browse files Browse the repository at this point in the history
  • Loading branch information
rtlopez committed Apr 8, 2024
1 parent 072efd7 commit ee129ae
Show file tree
Hide file tree
Showing 9 changed files with 61 additions and 78 deletions.
25 changes: 13 additions & 12 deletions README.md
Original file line number Diff line number Diff line change
Expand Up @@ -3,12 +3,13 @@ The mini, DIY, ~$5 cost, ESP8266/ESP32 based, high performance flight controller

## Features
* Frames (Quad X)
* Betaflight configuration tool compatible (v10.8 or v10.9)
* Betaflight configuration tool compatible (v10.8-v10.10)
* Receiver protocol (8 channel PPM)
* SBUS and CRSF Serial Rx protocols on ESP32 and RP2040
* SBUS and CRSF Serial Rx protocols
* Builtin ESP-NOW receiver
* ESC protocols (PWM, Oneshot125, Brushed, Dshot150, Dshot300, Dshot600)
* Configurable Gyro Filters (LPF, Notch, dTerm)
* Blackbox recording (OpenLog/Opelager serial)
* Configurable Gyro Filters (LPF, Notch, dTerm, RPM)
* Blackbox recording (OpenLog/OpenLager serial)
* In flight PID Tuning
* Flight modes (ACRO, ANGLE, AIRMODE, ARM)
* Up to 8kHz gyro/loop on ESP32 with SPI gyro
Expand All @@ -17,17 +18,17 @@ The mini, DIY, ~$5 cost, ESP8266/ESP32 based, high performance flight controller
* Resorce/Pin mapping
* Buzzer
* Lipo voltage monitor
* Failsafe
* Failsafe mode

## Requirements
Hardware:
* ESP8266 Wemos D1 Mini or ESP32 mini board
* MPU6050 I2C gyro (GY-88, GY-91, GY-521 or similar) or MPU9250 SPI on ESP32
* ESP32 mini board or ESP8266 Wemos D1 mini or similar
* MPU9250 SPI or MPU6050 I2C gyro (GY-88, GY-91, GY-521 or similar)
* PDB with 5V BEC
* Buzzer and some electronic components (optional).

Software:
* [Betaflight Configurator](https://github.com/betaflight/betaflight-configurator/releases) (v10.8 or v10.9)
* [Betaflight Configurator](https://github.com/betaflight/betaflight-configurator/releases) (v10.10 or v10.9 or v10.8)
* [CH340 usb-serial converter driver](https://sparks.gogo.co.nz/ch340.html)

## Flashing
Expand All @@ -38,6 +39,8 @@ Software:
5. Click "Program"
6. After success power cycle board

Note: only ESP32 and ESP8266 can be flashed in this way.

![ESP-FC Flashing](/docs/images/esptool-js-flash-connect.png)

## Setup
Expand Down Expand Up @@ -75,9 +78,8 @@ Here are more details about [how to setup](/docs/setup.md).
| MSP | Yes | Yes | Yes |
| CLI | Yes | Yes | Yes |
| PPM (IN) | Yes | Yes | Yes |
| SBUS | - | Yes | Yes |
| IBUS | - | - | - |
| CRSF (ELRS) | - | Yes | Yes |
| SBUS | Yes | Yes | Yes |
| CRSF (ELRS) | Yes | Yes | Yes |
| BLACKBOX | Yes | Yes | Yes |
| PWM (OUT) | Yes | Yes | Yes |
| ONESHOT125 | Yes | Yes | Yes |
Expand Down Expand Up @@ -124,7 +126,6 @@ You can also join to our [Discord Channel](https://discord.gg/jhyPPM5UEH)
* Serial Rx (IBUS)
* ELRS telemetry
* ESP32-S2/S3/C3 targets
* BiDir Dshot
* Baro (MS5611)
* GPS navigation

Expand Down
12 changes: 9 additions & 3 deletions docs/setup.md
Original file line number Diff line number Diff line change
Expand Up @@ -44,6 +44,13 @@ If you want to use Serial based receiver (SBUS,CRSF), you need to allocate UART

Then go to the `Receiver` tab, and select `Receiver mode` and `Serial Receiver Provider`.

To use Serial RX on ESP8266, you need to connect reciver to primary UART. In this case you lose ability to configure through UART.
To deal with it activate "SOFTSERIAL" feature. In this case if board stay in failsafe mode for 30 seconds (transmitter is turned off), then automatically WiFi access point will be started.
Default name is "ESP-FC". After connecting to this AP open configurator, select "manual" port and type port: `tcp://192.168.4.1:1111`.
You can also configure board to automatically connect existing wifi network. In this case set `wifi_ssid` and `wifi_pass` parameters in CLI.

To use ESP-NOW receiver, choose "SPI Rx (e.g. built-in Rx)" receiver mode in Receiver tab. You need compatible transmitter module.

## Motor setup

In `Motors` tab you must configure
Expand Down Expand Up @@ -92,7 +99,7 @@ This option allows to separate PWM frequency. If your pid loop is set to 1k, but

## Flight modes

Flight modes can be configured in modes tab
Flight modes can be configured in `Modes` tab

### Arm

Expand Down Expand Up @@ -125,8 +132,7 @@ Logging using serial device is possible, like [D-ronin OpenLager](https://github
Recommended settings

- To log with 1k rate, you need device that is capable to receive data with 250kbps.
- To log with 1k rate with debug, baro and mag data, 500kbps is recommended.
- To log with 1k rate, you need device that is capable to receive data with 500kbps.
- To log with 2k rate, you need 1Mbps

OpenLager can handle it easily. If you plan to use OpenLog, you might need to flash [blackbox-firmware](https://github.com/cleanflight/blackbox-firmware) to be able to handle more than 115.2kbps. Either 250kbps and 500kbps works well with modern SD cards up to 32GB size.
Expand Down
33 changes: 11 additions & 22 deletions lib/EscDriver/src/EscDriverEsp32.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -60,6 +60,16 @@ static void IRAM_ATTR _rmt_zero_mem(rmt_channel_t channel, size_t len)
}
}

// using map() in isr cause crash
static long IRAM_ATTR _map(long x, long in_min, long in_max, long out_min, long out_max)
{
const long in_range = in_max - in_min;
if(in_range == 0) return out_min;
const long out_range = out_max - out_min;
const long delta = x - in_min;
return (delta * out_range) / in_range + out_min;
}

bool EscDriverEsp32::_tx_end_installed = false;

EscDriverEsp32 *EscDriverEsp32::instances[] = {NULL, NULL, NULL, NULL, NULL, NULL, NULL, NULL};
Expand Down Expand Up @@ -324,40 +334,19 @@ void EscDriverEsp32::readTelemetry()
if(!_digital || !_dshot_tlm) continue;
if(!_channel[i].attached()) continue;

//HardwareSerial* s = (i == 0 && _timer.check()) ? &Serial : nullptr;

RingbufHandle_t rb = NULL;
if(ESP_OK != rmt_get_ringbuf_handle((rmt_channel_t)i, &rb)) continue;

size_t rmt_len = 0;
rmt_item32_t* data = (rmt_item32_t*)xRingbufferReceive(rb, &rmt_len, 0);
//if(s) { s->print((uint8_t)rmt_len); }
if (data)
{
/*for(size_t j = 0; j < rmt_len >> 2; j++)
{
if(!data[j].duration0) break;
if(s) s->print(' ');
if(s) s->print(data[j].level0);
if(s) s->print(':');
if(s) s->print(data[j].duration0);
if(!data[j].duration1) break;
if(s) s->print(' ');
if(s) s->print(data[j].level1);
if(s) s->print(':');
if(s) s->print(data[j].duration1);
}*/
uint32_t value = extractTelemetryGcr((uint32_t*)data, rmt_len >> 2, _channel[i].dshot_tlm_bit_len);

//if(s) s->print(' ');
//if(s) s->print(value, HEX);

vRingbufferReturnItem(rb, (void *)data);

_channel[i].telemetryValue = value;
}
//if(s) s->print('\n');
}
//PIN_DEBUG(LOW);
}
Expand All @@ -373,7 +362,7 @@ void EscDriverEsp32::writeAnalogCommand(uint8_t channel, int32_t pulse)
maxPulse = 2000;
}
pulse = constrain(pulse, minPulse, maxPulse);
int duration = map(pulse, 1000, 2000, slot.pulse_min, slot.pulse_max);
int duration = _map(pulse, 1000, 2000, slot.pulse_min, slot.pulse_max);

int count = 2;
if (_async)
Expand Down
2 changes: 1 addition & 1 deletion lib/Espfc/src/Cli.h
Original file line number Diff line number Diff line change
Expand Up @@ -877,7 +877,7 @@ class Cli
else if(strcmp_P(cmd.args[0], PSTR("wifi")) == 0)
{
s.print(F("ST IP4: tcp://"));
s.print(_model.state.localIp);
s.print(WiFi.localIP());
s.print(F(":"));
s.println(_model.config.wireless.port);
s.print(F("ST MAC: "));
Expand Down
8 changes: 4 additions & 4 deletions lib/Espfc/src/Input.h
Original file line number Diff line number Diff line change
Expand Up @@ -363,26 +363,26 @@ class Input
Device::InputDevice * getInputDevice()
{
Device::SerialDevice * serial = _model.getSerialStream(SERIAL_FUNCTION_RX_SERIAL);
if(serial && _model.isActive(FEATURE_RX_SERIAL) && _model.config.input.serialRxProvider == SERIALRX_SBUS)
if(serial && _model.isFeatureActive(FEATURE_RX_SERIAL) && _model.config.input.serialRxProvider == SERIALRX_SBUS)
{
_sbus.begin(serial);
_model.logger.info().logln(F("RX SBUS"));
return &_sbus;
}
if(serial && _model.isActive(FEATURE_RX_SERIAL) && _model.config.input.serialRxProvider == SERIALRX_CRSF)
if(serial && _model.isFeatureActive(FEATURE_RX_SERIAL) && _model.config.input.serialRxProvider == SERIALRX_CRSF)
{
_crsf.begin(serial);
_model.logger.info().logln(F("RX CRSF"));
return &_crsf;
}
else if(_model.isActive(FEATURE_RX_PPM) && _model.config.pin[PIN_INPUT_RX] != -1)
else if(_model.isFeatureActive(FEATURE_RX_PPM) && _model.config.pin[PIN_INPUT_RX] != -1)
{
_ppm.begin(_model.config.pin[PIN_INPUT_RX], _model.config.input.ppmMode);
_model.logger.info().log(F("RX PPM")).log(_model.config.pin[PIN_INPUT_RX]).logln(_model.config.input.ppmMode);
return &_ppm;
}
#if defined(ESPFC_ESPNOW)
else if(_model.isActive(FEATURE_RX_SPI))
else if(_model.isFeatureActive(FEATURE_RX_SPI))
{
int status = _espnow.begin();
_model.logger.info().log(F("RX ESPNOW")).logln(status);
Expand Down
6 changes: 3 additions & 3 deletions lib/Espfc/src/Msp/MspProcessor.h
Original file line number Diff line number Diff line change
Expand Up @@ -530,7 +530,7 @@ class MspProcessor
case MSP_CF_SERIAL_CONFIG:
for(int i = 0; i < SERIAL_UART_COUNT; i++)
{
if(_model.config.serial[i].id >= SERIAL_ID_SOFTSERIAL_1 && !_model.isActive(FEATURE_SOFTSERIAL)) continue;
if(_model.config.serial[i].id >= SERIAL_ID_SOFTSERIAL_1 && !_model.isFeatureActive(FEATURE_SOFTSERIAL)) continue;
r.writeU8(_model.config.serial[i].id); // identifier
r.writeU16(_model.config.serial[i].functionMask); // functionMask
r.writeU8(toBaudIndex(_model.config.serial[i].baud)); // msp_baudrateIndex
Expand All @@ -545,13 +545,13 @@ class MspProcessor
uint8_t count = 0;
for (int i = 0; i < SERIAL_UART_COUNT; i++)
{
if(_model.config.serial[i].id >= SERIAL_ID_SOFTSERIAL_1 && !_model.isActive(FEATURE_SOFTSERIAL)) continue;
if(_model.config.serial[i].id >= SERIAL_ID_SOFTSERIAL_1 && !_model.isFeatureActive(FEATURE_SOFTSERIAL)) continue;
count++;
}
r.writeU8(count);
for (int i = 0; i < SERIAL_UART_COUNT; i++)
{
if(_model.config.serial[i].id >= SERIAL_ID_SOFTSERIAL_1 && !_model.isActive(FEATURE_SOFTSERIAL)) continue;
if(_model.config.serial[i].id >= SERIAL_ID_SOFTSERIAL_1 && !_model.isFeatureActive(FEATURE_SOFTSERIAL)) continue;
r.writeU8(_model.config.serial[i].id); // identifier
r.writeU32(_model.config.serial[i].functionMask); // functionMask
r.writeU8(toBaudIndex(_model.config.serial[i].baud)); // msp_baudrateIndex
Expand Down
5 changes: 5 additions & 0 deletions lib/Espfc/src/Target/TargetESP32s3.h
Original file line number Diff line number Diff line change
Expand Up @@ -3,6 +3,11 @@
#include "Esp.h"
#include "Debug_Espfc.h"

// pins to avoid:
// strapping: 0, 3, 45, 46
// flash/psram: 26-37 (reserved)
// usb/jtag: 19, 20

#define ESPFC_INPUT
#define ESPFC_INPUT_PIN 6 // ppm

Expand Down
46 changes: 14 additions & 32 deletions lib/Espfc/src/Wireless.h
Original file line number Diff line number Diff line change
Expand Up @@ -17,7 +17,6 @@ class Wireless
{
enum Status {
STOPPED,
STARTING,
STARTED,
};
public:
Expand All @@ -27,7 +26,7 @@ class Wireless
{
WiFi.persistent(false);
#ifdef ESPFC_ESPNOW
if(_model.isActive(FEATURE_RX_SPI))
if(_model.isFeatureActive(FEATURE_RX_SPI))
{
startAp();
}
Expand All @@ -38,7 +37,7 @@ class Wireless
void startAp()
{
bool status = WiFi.softAP("ESP-FC");
_model.logger.info().log(F("WIFI AP")).logln(status);
_model.logger.info().log(F("WIFI AP START")).logln(status);
}

int connect()
Expand All @@ -65,41 +64,33 @@ class Wireless
{
startAp();
}
//delay(200);
_server.begin(_model.config.wireless.port);
_server.setNoDelay(true);
_model.logger.info().log(F("WIFI PORT")).log(WiFi.status()).logln(_model.config.wireless.port);
_model.state.serial[SERIAL_SOFT_0].stream = &_adapter;
_model.logger.info().log(F("WIFI SERVER PORT")).log(WiFi.status()).logln(_model.config.wireless.port);
return 1;
}

void wifiEventConnected(const String& ssid, int channel)
{
_model.logger.info().log(F("WIFI CONNECTED")).log(ssid).logln(channel);
_model.logger.info().log(F("WIFI STA CONN")).log(ssid).logln(channel);
}

void wifiEventApConnected(const uint8_t* mac)
{
char buf[20];
snprintf(buf, sizeof(buf), "%02x:%02x:%02x:%02x:%02x:%02x", mac[0], mac[1], mac[2], mac[3], mac[4], mac[5]);
_model.logger.info().log(F("WIFI AP CONNECTED")).logln(buf);
//_server.begin(_model.config.wireless.port);
//_model.logger.info().log(F("WIFI PORT")).log(WiFi.status()).logln(_model.config.wireless.port);
_model.logger.info().log(F("WIFI AP CONNECT")).logln(buf);
}

void wifiEventGotIp(const IPAddress& ip)
{
_model.state.localIp = ip;
//_status = STARTED;
_model.state.serial[SERIAL_SOFT_0].stream = &_adapter;
_model.logger.info().log(F("WIFI IP")).logln(_model.state.localIp.toString());
_model.logger.info().log(F("WIFI STA IP")).logln(ip.toString());
}

void wifiEventDisconnected()
{
_model.state.serial[SERIAL_SOFT_0].stream = nullptr;
_model.state.localIp = IPAddress(0,0,0,0);
//_status = STOPPED;
_model.logger.info().logln(F("WIFI DISCONNECTED"));
_model.logger.info().logln(F("WIFI STA DISCONNECT"));
}

int update()
Expand All @@ -109,30 +100,21 @@ class Wireless
switch(_status)
{
case STOPPED:
if(_model.state.rescueConfigMode == RESCUE_CONFIG_ACTIVE)
if(_model.state.rescueConfigMode == RESCUE_CONFIG_ACTIVE && _model.isFeatureActive(FEATURE_SOFTSERIAL))
{
_status = STARTING;
connect();
_status = STARTED;
return 1;
}
break;
case STARTING:
/*if(WiFi.status() == WL_CONNECTED)
{
_status = STARTED;
_server.begin(_model.config.wireless.port);
_model.logger.info().log(F("WIFI PORT")).log(WiFi.status()).logln(_model.config.wireless.port);
}*/
break;
case STARTED:
if(_server.hasClient())
{
_client = _server.accept();
}
break;
}

if(_server.hasClient())
{
_client = _server.accept();
}

return 1;
}

Expand Down
2 changes: 1 addition & 1 deletion platformio.ini
Original file line number Diff line number Diff line change
Expand Up @@ -51,7 +51,7 @@ build_flags =
extra_scripts = merge_firmware.py

[env:esp32s3]
board = lolin_s3
board = lolin_s3_mini
platform = espressif32
framework = arduino
board_build.f_cpu = 240000000L
Expand Down

0 comments on commit ee129ae

Please sign in to comment.