Skip to content

Commit

Permalink
Add Clip180 servo mode
Browse files Browse the repository at this point in the history
  • Loading branch information
@CapnBry
CapnBry committed Jan 13, 2024
1 parent 7944861 commit 7d10eb5
Show file tree
Hide file tree
Showing 7 changed files with 104 additions and 33 deletions.
1 change: 1 addition & 0 deletions html/scan.js
View file
Original file line number Diff line number Diff line change
Expand Up @@ -65,6 +65,7 @@ function updateAatConfig(config)

// AAT
_('servosmoo').value = config.aat.servosmoo;
_('servomode').value = config.aat.servomode;
_('azim_center').value = config.aat.azim_center;
_('azim_min').value = config.aat.azim_min;
_('azim_max').value = config.aat.azim_max;
Expand Down
29 changes: 20 additions & 9 deletions html/vrx_index.html
View file
Original file line number Diff line number Diff line change
Expand Up @@ -125,16 +125,27 @@ <h2>RTC Update via NTP</h2>
</datalist>
</fieldset>
</div>
<div class="mui-panel">
<fieldset><legend>Servo mode</legend>
<div>Set the mechanical servo mode to match the tracker's design.</div>
<div class="mui-select mui-col-xs-12">
<select id="servomode" name="servomode">
<option value="0">2:1 geared - Servo geared for 360 degree movement</option>
<option value="1">Clip 180 - Standard direct driven servo limited to 180 degrees</option>
</select>
</div>
</fieldset>
</div>
<div class="mui-panel">
<fieldset><legend>Azimuth servo</legend>
Enter the micrsecond (us) values for the min and max position of the horizontal servo, using the slider to test positions.
<input type="range" id="bear" name="bear" class="aatlive" list="azim_markers" value="0" min="-180" max="180" step="90" style="width: 100%;"/>
<input type="range" id="bear" name="bear" class="aatlive" list="azim_markers" value="0" min="-180" max="180" step="45" style="width: 100%;"/>
<datalist id="bear_markers">
<option value="-180" label="S"></option>
<option value="-90" label="W"></option>
<option value="0" label="N"></option>
<option value="90" label="E"></option>
<option value="180" label="S"></option>
<option value="-180"></option>
<option value="-90"></option>
<option value="0"></option>
<option value="90"></option>
<option value="180"></option>
</datalist>
<div class="mui-textfield mui-col-xs-3">
<input id="azim_min" type="number" name="azim_min" min="500" max="2500"/>
Expand All @@ -143,9 +154,9 @@ <h2>RTC Update via NTP</h2>
<div class="mui-select mui-col-xs-6">
<select id="azim_center" name="azim_center">
<option value="0">N</option>
<option value="1">E</option>
<option value="2">S</option>
<option value="3">W</option>
<option value="2">E</option>
<option value="4">S</option>
<option value="6">W</option>
</select>
<label for="azim_center">Servo center direction</label>
</div>
Expand Down
5 changes: 5 additions & 0 deletions lib/config/config.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -236,6 +236,11 @@ VrxBackpackConfig::SetAatCenterDir(uint8_t val)
CONFIG_MOD_CHECK(m_config.aat.centerDir, val);
}

void VrxBackpackConfig::SetAatServoMode(uint8_t val)
{
CONFIG_MOD_CHECK(m_config.aat.servoMode, val);
}

#endif /* defined(AAT_BACKPACK) */

#endif /* defined(TARGET_VRX_BACKPACK) */
6 changes: 4 additions & 2 deletions lib/config/config.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -64,10 +64,10 @@ typedef struct {
struct __attribute__((packed)) tagAatConfig {
uint8_t satelliteHomeMin; // minimum number of satellites to establish home
uint8_t servoSmooth; // 0-9 for min smoothing to most smoothing
uint8_t centerDir; // Direction servo points at center position 0=N 1=E 2=S 3=W
uint8_t centerDir; // Direction servo points at center position 0=N 2=E 4=S 6=W (can hold 45 degrees but only 90 is supported)
uint8_t project; // FUTURE: 0=none, 1=projectAzim, 2=projectElev, 3=projectBoth
uint8_t units; // FUTURE: 0=meters, anything else=also meters :-D
uint8_t servoMode; // FUTURE: reserved to declare 2:1, 180+flip servo, or 180 clipped
uint8_t servoMode; // 0=2:1, 1=clip180, FUTURE: 180+flip servo
// Also maybe invertAzim / invertElev servo bit or just swap low/high
struct __attribute__((packed)) tagServoEndoint {
uint16_t low;
Expand Down Expand Up @@ -109,6 +109,8 @@ class VrxBackpackConfig
uint8_t GetAatSatelliteHomeMin() const { return m_config.aat.satelliteHomeMin; }
uint8_t GetAatServoSmooth() const { return m_config.aat.servoSmooth; }
void SetAatServoSmooth(uint8_t val);
uint8_t GetAatServoMode() const { return m_config.aat.servoMode; }
void SetAatServoMode(uint8_t val);
uint8_t GetAatProject() const { return m_config.aat.project; }
uint8_t GetAatCenterDir() const { return m_config.aat.centerDir; }
void SetAatCenterDir(uint8_t val);
Expand Down
3 changes: 3 additions & 0 deletions src/devwifi_proxy_aat.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,6 +8,7 @@ void WebAatAppendConfig(ArduinoJson::JsonDocument &json)
auto aat = json["config"].createNestedObject("aat");
aat["satmin"] = config.GetAatSatelliteHomeMin();
aat["servosmoo"] = config.GetAatServoSmooth();
aat["servomode"] = config.GetAatServoMode();
aat["project"] = config.GetAatProject();
aat["azim_center"] = config.GetAatCenterDir();
aat["azim_min"] = config.GetAatServoLow(0);
Expand All @@ -27,6 +28,8 @@ void WebAatConfig(AsyncWebServerRequest *request)
// Servos
if (request->hasArg("servosmoo"))
config.SetAatServoSmooth(request->arg("servosmoo").toInt());
if (request->hasArg("servomode"))
config.SetAatServoMode(request->arg("servomode").toInt());
if (request->hasArg("azim_center"))
config.SetAatCenterDir(request->arg("azim_center").toInt());
if (request->hasArg("azim_min"))
Expand Down
86 changes: 66 additions & 20 deletions src/module_aat.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -237,10 +237,14 @@ int32_t AatModule::calcProjectedAzim(uint32_t now)
return _targetAzim;
}

const int32_t AatModule::azimCenterInverse() const
/**
* @brief: Calculate the bearing from centerdir to azim
* @return: -180 to +179 bearing
*/
const int32_t AatModule::azimToBearing(int32_t azim) const
{
const int32_t AZIM_CENTER_INVERSE[] = { 180, 90, 0, 270 };
return AZIM_CENTER_INVERSE[config.GetAatCenterDir()];
int32_t center = 45 * config.GetAatCenterDir();
return ((azim - center + 540) % 360) - 180; // -180 to +179
}

#if defined(PIN_OLED_SDA)
Expand Down Expand Up @@ -305,27 +309,52 @@ void AatModule::displayGpsIdle(uint32_t now)
_display.display();
}

void AatModule::displayAzimuthExtent(int32_t y)
{
switch ((ServoMode)config.GetAatServoMode())
{
// Just a line on the left / right of the azim line
default: /* fallthrough */
case ServoMode::TwoToOne:
_display.drawFastVLine(0, y, 5, SSD1306_WHITE);
_display.drawFastVLine(SCREEN_WIDTH-1, y, 5, SSD1306_WHITE);
break;

// 180 mode put the lines at the extent of the servo rotation
case ServoMode::Clip180:
//_display.drawFastVLine(SCREEN_WIDTH/4, y, 5, SSD1306_WHITE);
//_display.drawFastVLine(3*SCREEN_WIDTH/4, y, 5, SSD1306_WHITE);
// Dotted line all the way up the elevation field
for (int32_t dotY=0; dotY<(33+5); dotY+=2)
{
_display.drawPixel(SCREEN_WIDTH/4, dotY+16, SSD1306_WHITE);
_display.drawPixel(3*SCREEN_WIDTH/4, dotY+16, SSD1306_WHITE);
}
break;
}
}

void AatModule::displayAzimuth(int32_t projectedAzim)
{
int32_t y = SCREEN_HEIGHT - FONT_H + 1 - 6;

// horizon line + arrow + unprojected azim line
// |-----/\---|-|
_display.drawFastVLine(0, y, 5, SSD1306_WHITE);
_display.drawFastHLine(0, y+2, SCREEN_WIDTH-1, SSD1306_WHITE);
_display.drawFastVLine(SCREEN_WIDTH-1, y, 5, SSD1306_WHITE);
displayAzimuthExtent(y);

// Unprojected azimuth line
int32_t azimPos = map((_targetAzim + azimCenterInverse()) % 360, 0, 360, 0, SCREEN_WIDTH);
int32_t azimPos = map(azimToBearing(_targetAzim), -180, 180, 0, SCREEN_WIDTH);
_display.drawFastVLine(azimPos, y+1, 3, SSD1306_WHITE);
// Projected Azimuth arrow
azimPos = map((projectedAzim + azimCenterInverse()) % 360, 0, 360, 0, SCREEN_WIDTH);
azimPos = map(azimToBearing(projectedAzim), -180, 180, 0, SCREEN_WIDTH);
const uint8_t oledim_arrowup[] = { 0x10, 0x38, 0x7c }; // 'arrowup', 7x3px
_display.drawBitmap(azimPos-7/2, y+1, oledim_arrowup, 7, 3, SSD1306_WHITE, SSD1306_BLACK);

// S W N E S under that
y += 6;
const char AZIM_LABELS[] = "SWNES" "WNESW" "NESWN" "ESWNE"; // 5 characters for each direction, 3rd character = center
const char *labels = &AZIM_LABELS[5*config.GetAatCenterDir()];
const char *labels = &AZIM_LABELS[config.GetAatCenterDir()*5/2];
_display.drawChar(0, y, labels[0], SSD1306_WHITE, SSD1306_BLACK, 1);
_display.drawChar(SCREEN_WIDTH/4-FONT_W/2, y, labels[1], SSD1306_WHITE, SSD1306_BLACK, 1);
_display.drawChar(SCREEN_WIDTH/2-FONT_W/2+1, y, labels[2], SSD1306_WHITE, SSD1306_BLACK, 1);
Expand Down Expand Up @@ -367,23 +396,25 @@ void AatModule::displayTargetCircle(int32_t projectedAzim)
// yellow-blue OLED have 16 pixels of yellow, start below that
const int32_t elevOff = 16;
// Dotted line to separate top of screen from tracking area
for (int32_t x=0; x<SCREEN_WIDTH; x+=2)
for (int32_t x=0; x<SCREEN_WIDTH; x+=3)
_display.drawPixel(x, elevOff, SSD1306_WHITE);

int32_t elevPos = map(_targetElev, 0, 90, elevH, 0) + elevOff;
//elevPos = 0 + elevOff;
// X for projectedAzim
int32_t azimPos = map((projectedAzim + azimCenterInverse()) % 360, 0, 360, 0, SCREEN_WIDTH);
int32_t azimPos = map(azimToBearing(projectedAzim), -180, 180, 0, SCREEN_WIDTH);
_display.drawPixel(azimPos-1, elevPos-1, SSD1306_WHITE);
_display.drawPixel(azimPos+1, elevPos-1, SSD1306_WHITE);
_display.drawPixel(azimPos, elevPos, SSD1306_WHITE);
_display.drawPixel(azimPos-1, elevPos+1, SSD1306_WHITE);
_display.drawPixel(azimPos+1, elevPos+1, SSD1306_WHITE);

// circle/rectangle cage for _servoPos
int32_t servoXMin = ((ServoMode)config.GetAatServoMode() == ServoMode::Clip180) ? (SCREEN_WIDTH/4) : 0;
int32_t servoXMax = ((ServoMode)config.GetAatServoMode() == ServoMode::Clip180) ? (3*SCREEN_WIDTH/4) : SCREEN_WIDTH;
int32_t servoX = map(_servoPos[IDX_AZIM],
config.GetAatServoLow(IDX_AZIM), config.GetAatServoHigh(IDX_AZIM),
0, SCREEN_WIDTH);
servoXMin, servoXMax);
int32_t servoY = map(_servoPos[IDX_ELEV],
config.GetAatServoLow(IDX_ELEV), config.GetAatServoHigh(IDX_ELEV),
elevH, 0) + elevOff;
Expand Down Expand Up @@ -477,6 +508,28 @@ void AatModule::displayGpsIntervalBar(uint32_t now)
}
#endif /* defined(PIN_OLED_SDA) */

void AatModule::servoApplyMode(int32_t azim, int32_t elev, int32_t newServoPos[])
{
int32_t bearing = azimToBearing(azim);

// 2-to-1 reduction can do 360 so the input and output is the same
if ((ServoMode)config.GetAatServoMode() == ServoMode::TwoToOne)
{
newServoPos[IDX_AZIM] = map(bearing, -180, 179, config.GetAatServoLow(IDX_AZIM), config.GetAatServoHigh(IDX_AZIM));
newServoPos[IDX_ELEV] = map(elev, 0, 90, config.GetAatServoLow(IDX_ELEV), config.GetAatServoHigh(IDX_ELEV));
return;
}

// Clip180 limits azim to 90 degrees left/right from center
if ((ServoMode)config.GetAatServoMode() == ServoMode::Clip180)
{
bearing = constrain(bearing, -90, 90);
newServoPos[IDX_AZIM] = map(bearing, -90, 90, config.GetAatServoLow(IDX_AZIM), config.GetAatServoHigh(IDX_AZIM));
newServoPos[IDX_ELEV] = map(elev, 0, 90, config.GetAatServoLow(IDX_ELEV), config.GetAatServoHigh(IDX_ELEV));
return;
}
}

void AatModule::servoUpdate(uint32_t now)
{
uint32_t interval = now - _lastServoUpdateMs;
Expand All @@ -485,15 +538,8 @@ void AatModule::servoUpdate(uint32_t now)
_lastServoUpdateMs = now;

int32_t projectedAzim = calcProjectedAzim(now);
int32_t transformedAzim = projectedAzim;
int32_t transformedElev = _targetElev;

// For 1:2 gearing on the azim servo to allow 360 rotation
// For Elev servos that only go 0-90 and the azim does 360
transformedAzim = (transformedAzim + azimCenterInverse()) % 360;
int32_t newServoPos[IDX_COUNT];
newServoPos[IDX_AZIM] = map(transformedAzim, 0, 360, config.GetAatServoLow(IDX_AZIM), config.GetAatServoHigh(IDX_AZIM));
newServoPos[IDX_ELEV] = map(transformedElev, 0, 90, config.GetAatServoLow(IDX_ELEV), config.GetAatServoHigh(IDX_ELEV));
servoApplyMode(projectedAzim, _targetElev, newServoPos);

for (uint32_t idx=IDX_AZIM; idx<IDX_COUNT; ++idx)
{
Expand Down Expand Up @@ -556,7 +602,7 @@ void AatModule::overrideTargetCommon(int32_t azimuth, int32_t elevation)
void AatModule::overrideTargetBearing(int32_t bearing)
{
// bearing is -180 to +180, azimuth 0-360
int32_t azim = (360 + bearing + (90 * config.GetAatCenterDir())) % 360;
int32_t azim = (360 + bearing + (45 * config.GetAatCenterDir())) % 360;
// The furthest right a servo can go is +179 degrees, +180 goes to the -180 position
if (bearing == 180)
azim = (azim == 0) ? 359 : azim - 1;
Expand Down
7 changes: 5 additions & 2 deletions src/module_aat.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -58,21 +58,24 @@ class AatModule : public CrsfModuleBase
void overrideTargetCommon(int32_t azimuth, int32_t elevation);
virtual void onCrsfPacketIn(const crsf_header_t *pkt);
private:
enum tagServoIndex { IDX_AZIM, IDX_ELEV, IDX_COUNT };
enum ServoIndex { IDX_AZIM, IDX_ELEV, IDX_COUNT };
enum ServoMode { TwoToOne, Clip180 };

void displayInit();
void updateGpsInterval(uint32_t interval);
uint8_t calcGpsIntervalPct(uint32_t now);
int32_t calcProjectedAzim(uint32_t now);
void servoApplyMode(int32_t azim, int32_t elev, int32_t newServoPos[]);
void processGps(uint32_t now);
void servoUpdate(uint32_t now);
const int32_t azimCenterInverse() const;
const int32_t azimToBearing(int32_t azim) const;

#if defined(PIN_OLED_SDA)
void displayState();
void displayGpsIdle(uint32_t now);
void displayActive(uint32_t now, int32_t projectedAzim);
void displayGpsIntervalBar(uint32_t now);
void displayAzimuthExtent(int32_t y);
void displayAzimuth(int32_t projectedAzim);
void displayAltitude(int32_t azimPos, int32_t elevPos);
void displayTargetCircle(int32_t projectedAzim);
Expand Down

0 comments on commit 7d10eb5

Please sign in to comment.