前回投稿したＲＴＣで動作するソーラートラッカーをＧＰＳ対応にしてみた。
STM32F103+ServoMotorでSolarTrackerを作ってみた。
ＧＰＳモジュールは秋月から購入したがシリアル経由でＮＭＥＡ(ＲＭＣ)メッセージを受信するだけなのでボーレート設定を除き他のＧＰＳモジュールでもそのまま使えると思う。すべてのＮＭＥＡメッセージを受信すると凄い量となり受信に時間がかかってしまうが購入したＧＰＳモジュールは受信メッセージを選択できるのが良い。ちなみに１ＰＰＳ出力は使っていない。
GPS受信機 シリアル出力タイプ(先バラ) みちびき2機(194/195)対応 1PPS出力付 GT-502MGG-N
ＧＰＳからの緯度経度情報とＵＴＣ時刻により太陽位置計算するようにしたので標準時の経度情報も必要なくなってほぼメンテナンス・フリーとなった。(/・ω・)/
その他、ライブラリをより使いやすくするために最適化を行なった。
【スケッチ】
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230 231 232 233 234 235 236 237 238 239 240 241 242 243 244 245 246 247 248 249 250 251 252 253 254 255 256 257 258 259 260 261 262 263 264 265 266 267 268 269 270 271 272 273 274 275 276 277 278 279 280 281 282 283 284 285 286 287 288 289 290 291 292 293 294 295 296 297 298 299 300 301 302 303 304 305 306 307 308 309 310 311 312 313 314 315 316 317 318 319 320 321 322 323 324 325 326 327 328 329 330 331 332 333 334 335 336 337 338 339 340 341 342 343 344 345 346 347 348 349 350 351 352 353 354 355 356 357 358 359 360 361 362 363 364 365 366 367 368 369 370 |
/* SolarTracker.ino - Solar Tracking Control Program STM32 MCU based boards by STMicroelectronics [2.8.1] url : https://github.com/stm32duino/BoardManagerFiles/raw/main/package_stmicroelectronics_index.json Board: Generic STM32F1 series Board part number: "Generic F103RCTx" Upload method : "STM32QubeProgrammer (Serial)" U(S)ART support : "Enabled (no generic 'Serial')" Copyright (c) 2024 Sasapea's Lab. All right reserved. This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or at your option) any later version. This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. You should have received a copy of the GNU General Public License along with this program. If not, see <https://www.gnu.org/licenses/>. */ #include <string> #include <stdio.h> #include <stdlib.h> #include <stdint.h> #include <stdbool.h> #include <string.h> #include <math.h> #include <time.h> #include <EEPROM.h> #include "solartracker.h" #include "servo24.h" #include "nmea.h" #define PIN_LED PC13 HardwareSerial Serial(USART1); //HardwareSerial Serial3(PC_11, PC_10); HardwareSerial Serial5(PD_2, PC_12); DECLARE_NMEA(Serial5) nmea; class SolarTracker : public SolarTrackerBase { public: SolarTracker(void) : SolarTrackerBase() { _debug = false; _turnmode = 0; _initmode = 3; } void begin(void) { /* Load Config Data from EEPROM */ loadConfig(); /* Init NMEA (MEDIATEK MT33xx GNSS PMTK) */ Serial5.begin(9600); nmea.begin(); nmea.setCallback(nmea.SENTENCE_RMC, [this](size_t sentence, const char **values, size_t count){ this->rmc(sentence, values, count); }); nmea.write("PMTK314,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0"); /* Init LED */ initLED(); /* Setup Servo */ for (size_t i = 0; i < lengthof(specs); ++i) { _servo[i].begin((Servo24::SERVO)_config.servo[i].ch, _config.servo[i].range, _config.servo[i].min, _config.servo[i].max); _servo[i].turn(_servo[i].anglerange() >> 1); } ::delay(1000); /* Setup SolarTracker */ SolarTrackerBase::begin(); SolarTrackerBase::delay(SERVO_DELAY); } uint16_t turn(const struct tm &tmbuf, float h, float a, uint16_t delay) override { /* debug print */ if (_debug) { Serial.printf("%02d-%02d-%02dT%02d:%02d:%02dZ, h = %s, a = %s\n", tmbuf.tm_year + 1900, tmbuf.tm_mon + 1, tmbuf.tm_mday, tmbuf.tm_hour, tmbuf.tm_min, tmbuf.tm_sec, to_string(h, 2).c_str(), to_string(a, 2).c_str()); } /* 太陽位置が追尾範囲内なら回転させる (南は180度) */ a -= 180 - (_servo[0].anglerange() >> 1); if ((h >= 0) && (h <= _servo[1].anglerange()) && (a >= 0) && (a <= _servo[0].anglerange())) { _servo[0].turn(a, delay); _servo[1].turn(h, delay); return 0; } /* 故障や保守のため太陽位置が追尾範囲外になったら中立位置に戻しておく */ else if (delay == 0) { _servo[0].turn(_servo[0].anglerange() >> 1, SERVO_DELAY); _servo[1].turn(_servo[1].anglerange() >> 1, SERVO_DELAY); } /* 翌日の日の出時にゆっくり回転させるための回転時間(ミリ秒)を指定 */ /* 次回の呼び出し以降、引数delayにこの値が設定される */ return SERVO_DELAY; } void handle(void) { config(); nmea.handle(); } private: void tracking(void) { if (_initmode) { if (!_servo[0].busy() && !_servo[1].busy()) { if (_initmode) turn(--_initmode + 1); } } else if (_turnmode == 0) SolarTrackerBase::tracking(_tmbuf); } void config(void) { static char cmd[128]; static size_t cnt; while (Serial.available()) { char c = Serial.read(); if ((c == '\r') || (c == '\n')) { if (cnt) { char *arg; cmd[cnt] = 0; if ((arg = strchr(cmd, ' '))) { *arg++ = 0; while (*arg == ' ') arg++; } else arg = &cmd[cnt]; cnt = 0; if (strncasecmp(cmd, "servo", 5) == 0) { char *part; size_t ch = strtol(cmd + 5, &part, 10); if ((ch > 0) && (ch <= lengthof(specs))) { --ch; if (strcasecmp(part, ".ch") == 0) _config.servo[ch].range = (Servo24::SERVO)atoi(arg); else if (strcasecmp(part, ".range") == 0) _config.servo[ch].range = atoi(arg); else if (strcasecmp(part, ".min") == 0) { _config.servo[ch].min = atoi(arg); _servo[ch].pulse(_config.servo[ch].min, _config.servo[ch].max, true); } else if (strcasecmp(part, ".max") == 0) { _config.servo[ch].max = atoi(arg); _servo[ch].pulse(_config.servo[ch].min, _config.servo[ch].max, true); } else Serial.printf("-- invalid command --\n"); } else Serial.printf("-- invalid command --\n"); } else if (strcasecmp(cmd, "load") == 0) loadConfig(); else if (strcasecmp(cmd, "save") == 0) saveConfig(); else if (strcasecmp(cmd, "reset") == 0) __NVIC_SystemReset(); else if (strcasecmp(cmd, "config") == 0) { Serial.printf("[configurations]\n"); for (size_t i = 0; i < lengthof(specs); ++i) Serial.printf(" servo%d { .ch = %d, .range = %d, .min = %d, .max = %d }\n", i + 1, _config.servo[i].ch, _config.servo[i].range, _config.servo[i].min, _config.servo[i].max); } else if (strcasecmp(cmd, "debug") == 0) _debug = atoi(arg); else if (strcasecmp(cmd, "turn") == 0) { if (strcasecmp(arg, "auto") == 0) { if (_turnmode) { SolarTrackerBase::delay(SERVO_DELAY); SolarTrackerBase::tracking(_tmbuf, true); } _turnmode = 0; } else if (strcasecmp(arg, "center") == 0) turn(_turnmode = 1); else if (strcasecmp(arg, "max") == 0) turn(_turnmode = 2); else if (strcasecmp(arg, "min") == 0) turn(_turnmode = 3); else Serial.printf("-- invalid command --\n"); } else if (cmd[0]) Serial.printf("-- invalid command --\n"); } } else if (cnt < sizeof(cmd) - 1) cmd[cnt++] = c; } } void turn(uint8_t mode) { switch (mode) { case 1: _servo[1].turn(_servo[1].anglerange() >> 1, SERVO_DELAY); _servo[0].turn(_servo[0].anglerange() >> 1, SERVO_DELAY); break; case 2: _servo[1].turn(_servo[1].anglerange(), SERVO_DELAY); _servo[0].turn(_servo[0].anglerange(), SERVO_DELAY); break; case 3: _servo[1].turn(0, SERVO_DELAY); _servo[0].turn(0, SERVO_DELAY); break; } } void loadConfig(void) { const uint16_t MARKER = 0xDEFA; EEPROM.get<typeof(_config)>(0, _config); if (_config.marker != MARKER) { _config.marker = MARKER; for (size_t i = 0; i < lengthof(specs); ++i) { _config.servo[i].ch = specs[i].ch; _config.servo[i].range = specs[i].range; _config.servo[i].min = specs[i].min; _config.servo[i].max = specs[i].max; } saveConfig(); } } void saveConfig(void) { EEPROM.put<typeof(_config)>(0, _config); } void rmc(size_t sentence, const char **values, size_t count) { (void)sentence; (void)count; int utc_time = strtoul(values[nmea.RMC_UTC_time], nullptr, 10); int utc_date = strtoul(values[nmea.RMC_UTC_date], nullptr, 10); _tmbuf = { .tm_sec = (utc_time / 1 % 100), .tm_min = (utc_time / 100 % 100), .tm_hour = (utc_time / 10000 % 100), .tm_mday = (utc_date / 10000 % 100), .tm_mon = (utc_date / 100 % 100) - 1, .tm_year = (utc_date / 1 % 100) + 100, /* [TODO] */ .tm_wday = 0, .tm_yday = 0, .tm_isdst = 0 }; float lat = nmea.todd(atof(values[nmea.RMC_Latitude])); float lon = nmea.todd(atof(values[nmea.RMC_Longitude])); #if NMEA_DEBUG & 1 Serial.printf("%02d-%02d-%02dT%02d:%02d:%02dZ (%s, %s)\n", _tmbuf.tm_year + 1900, _tmbuf.tm_mon + 1, _tmbuf.tm_mday, _tmbuf.tm_hour, _tmbuf.tm_min, _tmbuf.tm_sec, to_string(lat, 4).c_str(), to_string(lon, 4).c_str()); #endif turnLED(_tmbuf.tm_sec & 1); SolarTrackerBase::location(lat, lon); tracking(); } static std::string to_string(float value, uint8_t precision = 6) { char buf[32]; if (precision > 6) precision = 6; bool sign = value < 0; value = abs(value); unsigned long decimal = pow(10, precision); snprintf(buf, sizeof(buf), "%s%lu.%0*lu", sign ? "-" : "", (unsigned long)value, precision, (unsigned long)(value * decimal) % decimal); return std::string(buf); } static void initLED(void) { #if defined(PIN_LED) pinMode(PIN_LED, OUTPUT); #endif } static void turnLED([[maybe_unused]] bool on) { #if defined(PIN_LED) digitalWrite(PIN_LED, on ? HIGH : LOW); #endif } static constexpr uint16_t SERVO_DELAY = 3000; static constexpr struct { uint16_t ch; uint16_t range; uint16_t min; uint16_t max; } specs[] = { { Servo24::SERVO_1, 270, 2500, 500 }, { Servo24::SERVO_2, 180, 500, 2500 }, }; typedef struct { uint32_t marker; struct { uint16_t ch; uint16_t range; uint16_t min; uint16_t max; } servo[lengthof(specs)]; } config_t; uint8_t _turnmode; uint8_t _initmode; bool _debug; struct tm _tmbuf; config_t _config; Servo24 _servo[lengthof(specs)]; }; SolarTracker solar; bool status; void setup() { Serial.begin(115200); solar.begin(); } void loop() { solar.handle(); } |
【ライブラリ】
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 |
/* solartracker.h - Sun Tracking Control with Servo Motor Copyright (c) 2024 Sasapea's Lab. All right reserved. This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or at your option) any later version. This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. You should have received a copy of the GNU General Public License along with this program. If not, see <https://www.gnu.org/licenses/>. */ #pragma once #include <stdint.h> #include <stdbool.h> #include "sun.h" class SolarTrackerBase { public: void begin(void) { _sun.begin(); _minutes = -1; _start = 0; _delay = 0; } void location(float lat, float lon, float lons = 0) { _lat = lat; _lon = lon; _lons = lons; } void tracking(struct tm &tmbuf, bool force = false) { if (_start) { if (millis() - _start < _delay) return; _start = 0; _delay = 0; } if (force || (_minutes != tmbuf.tm_min)) { _minutes = tmbuf.tm_min; _sun.pos(tmbuf, _lat, _lon, _lons); uint16_t rv = turn(tmbuf, _sun.degh(), _sun.dega(), _delay); if (rv) _delay = rv; else _start = millis(); } } void delay(uint32_t millis) { _delay = millis; } virtual uint16_t turn(const struct tm &tmbuf, float h, float a, uint16_t delay) = 0; private: float _lat; float _lon; float _lons; int _minutes; uint32_t _start; uint32_t _delay; Sun _sun; }; |
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 |
/* servo24.h - 24 Channel Servo Motor Library for STM32 Family 2024-08-02: First Version for STM32F103RCT6 Copyright (c) 2024 Sasapea's Lab. All right reserved. This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or at your option) any later version. This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. You should have received a copy of the GNU General Public License along with this program. If not, see <https://www.gnu.org/licenses/>. */ #pragma once #include <stdint.h> #include <stdbool.h> #include "ticktimer.h" #include "HardwareTimer.h" #define lengthof(a) (sizeof(a) / sizeof(a[0])) class Servo24 { public: typedef enum { SERVO_1 , SERVO_2 , SERVO_3 , SERVO_4 , SERVO_5 , SERVO_6 , SERVO_7 , SERVO_8 , SERVO_9 , SERVO_10, SERVO_11, SERVO_12, SERVO_13, SERVO_14, SERVO_15, SERVO_16, SERVO_17, SERVO_18, SERVO_19, SERVO_20, SERVO_21, SERVO_22, SERVO_23, SERVO_24, } SERVO; Servo24(void) : _instance(nullptr) { _anglerange = 0; _angle = 0; } virtual ~Servo24(void) { end(); } /* The argument period can only be changed on a per timer. */ bool begin(SERVO channel, uint16_t anglerange = 180, uint16_t pulsemin = 500, uint16_t pulsemax = 2500, uint16_t period = 20000) { if (_instance) return false; _index = channel / lengthof(TIMMAP[0].pinname); _channel = channel % lengthof(TIMMAP[0].pinname); _anglerange = anglerange; _pulsemin = pulsemin; _pulsemax = pulsemax; _delay = 0; _count = 0; _angle = (uint16_t)-1; _angleto = 0; _anglefrom = 0; const timmap_t &map = TIMMAP[_index]; timobj_t &obj = timobj[_index]; if (obj.servo[_channel]) return false; obj.servo[_channel] = this; if (obj.instance == nullptr) { obj.period = period; obj.instance = new HardwareTimer(map.instance); obj.instance->setOverflow(period, MICROSEC_FORMAT); obj.instance->resume(); /* start TIMx clock */ } _period = obj.period; _instance = obj.instance; _overflow = _instance->getOverflow(TICK_FORMAT); _instance->setMode(_channel + 1, TIMER_OUTPUT_COMPARE_PWM1, map.pinname[_channel]); _instance->setCaptureCompare(_channel + 1, 0, TICK_COMPARE_FORMAT); /* no signal */ _instance->resumeChannel(_channel + 1); /* start TIMx channel */ _ticktimer.begin(_period > 2000 ? _period / 2000 : 1, [this](void){ this->tick(); }); return true; } void end(void) { if (_instance) { unregister(); _instance = nullptr; } } bool turn(uint16_t angle, uint16_t delay = 0, bool wait = false) { if (angle > _anglerange) return false; if (angle == _angleto) return true; _ticktimer.stop(); _delay = 0; _count = 0; _anglefrom = (_angle != (uint16_t)-1 ? _angle : angle); _angleto = angle; if (delay && (_anglefrom != _angleto)) { _delay = delay; _ticktimer.start(); if (wait) ::delay(delay); } else turn0(angle); return true; } void pulse(uint16_t min, uint16_t max, bool turn = false) { _pulsemin = min; _pulsemax = max; if (turn && (_angle != (uint16_t)-1)) turn0(_angle, true); } uint16_t anglerange(void) { return _anglerange; } bool busy(void) { return _ticktimer.running(); } private: void unregister(void) { _ticktimer.stop(); timobj_t &obj = timobj[_index]; obj.servo[_channel] = nullptr; for (uint32_t i = 0; i < lengthof(obj.servo); ++i) { if (obj.servo[i]) return; } delete obj.instance; obj.instance = nullptr; } void turn0(uint16_t angle, bool force = false) { if (force || (_angle != angle)) { _angle = angle; _instance->setCaptureCompare(_channel + 1, _overflow * (_pulsemin + (int32_t)(int16_t)(_pulsemax - _pulsemin) * angle / _anglerange) / _period, TICK_COMPARE_FORMAT); } } void tick(void) { if ((_count += _ticktimer.millis()) < _delay) turn0(_anglefrom + (int32_t)(int16_t)(_angleto - _anglefrom) * _count / _delay); else { _ticktimer.stop(); turn0(_angleto); } } HardwareTimer *_instance; uint16_t _index; uint16_t _channel; uint16_t _anglerange; uint16_t _pulsemin; uint16_t _pulsemax; uint16_t _period; uint32_t _overflow; uint16_t _delay; uint16_t _count; uint16_t _angle; uint16_t _angleto; uint16_t _anglefrom; TickTimer _ticktimer; typedef struct { TIM_TypeDef *instance; PinName pinname[4]; } timmap_t; /* STM32F103RCT6: Yahboom YB_ESV01 24Ch Servo Board */ static constexpr timmap_t TIMMAP[] = { { TIM5, { PA_0_ALT2 , PA_1_ALT2, PA_2_ALT2 , PA_3_ALT2 } }, { TIM3, { PB_4 , PB_5 , PB_0_ALT2 , PB_1_ALT2 } }, { TIM2, { PA_15_ALT1, PB_3_ALT1, PB_10_ALT1, PB_11_ALT1 } }, { TIM4, { PB_6 , PB_7 , PB_8 , PB_9 } }, { TIM8, { PC_6_ALT1 , PC_7_ALT1, PC_8_ALT1 , PC_9_ALT1 } }, { TIM1, { PA_8 , PB_14 , PB_15 , PA_11 } } }; typedef struct { HardwareTimer *instance; Servo24 *servo[lengthof(TIMMAP[0].pinname)]; uint16_t period; } timobj_t; static timobj_t timobj[lengthof(TIMMAP)]; }; |
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 |
/* servo24.cpp - 24 Channel Servo Motor Library for STM32 Family Copyright (c) 2024 Sasapea's Lab. All right reserved. This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or at your option) any later version. This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. You should have received a copy of the GNU General Public License along with this program. If not, see <https://www.gnu.org/licenses/>. */ #include "servo24.h" Servo24::timobj_t Servo24::timobj[]; |
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 |
/* ticktimer.h - SysTick Timer Library for ARDUINO (AVR/STM32) Copyright (c) 2024 Sasapea's Lab. All right reserved. This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or at your option) any later version. This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. You should have received a copy of the GNU General Public License along with this program. If not, see <https://www.gnu.org/licenses/>. */ #pragma once #include <stdint.h> #include <stdbool.h> #include <vector> #include "cpu.h" #if !defined(ARDUINO_ARCH_AVR) #include <functional> #endif class TickTimer { public: #if defined(ARDUINO_ARCH_AVR) typedef void (*callback_t)(void); #else typedef std::function<void(void)> callback_t; #endif void begin(uint32_t millis, callback_t callback) { if (!initialized) { initialized = true; instances.clear(); initOnce(); } _millis = millis; _callback = callback; _running = false; } void start(void) { if (!_running && _millis && _callback) { Cpu::state_t save = Cpu::disableInterrupts(); instances.push_back(this); Cpu::restoreInterrupts(save); _count = 0; _running = true; } } void stop(void) { if (_running) { _running = false; auto it = std::find(instances.begin(), instances.end(), this); if (it != instances.end()) { Cpu::state_t save = Cpu::disableInterrupts(); instances.erase(it); Cpu::restoreInterrupts(save); } } } bool running(void) { return _running; } uint32_t millis(void) { return _millis; } private: void tick(void) { if (_running && (++_count >= _millis)) { _count = 0; if (_callback) _callback(); } } static void tick0(void) { for (TickTimer *inst: instances) inst->tick(); } static void initOnce(void); /* timer initialize */ friend void TickTimer_Tick(void); static bool initialized; static std::vector<TickTimer *> instances; uint32_t _millis; callback_t _callback; bool _running; uint32_t _count; }; |
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 |
/* ticktimer.cpp - SysTick Timer Library for ARDUINO (AVR/STM32) Copyright (c) 2024 Sasapea's Lab. All right reserved. This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or at your option) any later version. This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. You should have received a copy of the GNU General Public License along with this program. If not, see <https://www.gnu.org/licenses/>. */ #include "ticktimer.h" bool TickTimer::initialized = false; std::vector<TickTimer *> TickTimer::instances; inline void TickTimer_Tick(void) { TickTimer::tick0(); } #if defined(ARDUINO_ARCH_AVR) #if defined(TIFR) #define TIFR0 TIFR #endif #if defind(TIMSK) #define TIMSK0 TIMSK #endif #if (F_CPU == 16000000) || (F_CPU == 16500000) #define PERIOD_US 4 #elif (F_CPU == 8000000) #define PERIOD_US 8 #else #error "System Clock Not Supported. (8MHz or 16MHz)" #endif ISR(TIMER0_COMPA_vect) { OCR0A += (1000 / PERIOD_US); TickTimer_Tick(); } void TickTimer::initOnce(void) { TCCR0A = 0; // standard mode OCR0A = TCNT0 + (1000 / PERIOD_US); TIFR0 = _BV(OCF0A); TIMSK0 |= _BV(OCIE0A); } #elif defined(ARDUINO_ARCH_ESP8266) #error "ESP8266 Not Supported" #elif defined(ARDUINO_ARCH_ESP32) #error "ESP32 Not Supported" #elif defined(ARDUINO_ARCH_SAMD) #error "SAMD Not Supported" #elif defined(ARDUINO_ARCH_NRF5) #error "NRF52 Not Supported" #elif defined(ARDUINO_ARCH_RP2040) #error "RP2040 Not Supported" #elif defined(ARDUINO_ARCH_STM32) // && defined(ARDUINO_GENERIC_F103RCTX) /* for STM32 based boards by STMicroelectronics */ extern "C" void HAL_SYSTICK_Callback(void) { TickTimer_Tick(); } void TickTimer::initOnce(void) { } #else #error "CPU Not Supported" #endif |
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 |
/* cpu.h - CPU Library for ARDUINO (AVR/ESP8266/ESP32/ARM) Copyright (c) 2024 Sasapea's Lab. All right reserved. This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or at your option) any later version. This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. You should have received a copy of the GNU General Public License along with this program. If not, see <https://www.gnu.org/licenses/>. */ #pragma once #include "Arduino.h" #if defined(ARDUINO_ARCH_AVR) #include <avr/interrupt.h> class Cpu { public: typedef uint8_t state_t; static inline state_t disableInterrupts(void) { state_t save = SREG; cli(); return save; } static inline void restoreInterrupts(state_t save) { SREG = save; } }; #elif defined(ARDUINO_ARCH_ESP8266) class Cpu { public: typedef size_t state_t; static inline state_t disableInterrupts(void) { return noInterrupts(); } static inline void restoreInterrupts(state_t save) { xt_wsr_ps(save); } }; #elif defined(ARDUINO_ARCH_ESP32) class Cpu { public: typedef size_t state_t; static inline state_t disableInterrupts(void) { return portSET_INTERRUPT_MASK_FROM_ISR(); } static inline void restoreInterrupts(state_t save) { portCLEAR_INTERRUPT_MASK_FROM_ISR(save); } }; #elif defined(ARDUINO_ARCH_STM32) \ || defined(ARDUINO_ARCH_SAMD) \ || defined(ARDUINO_ARCH_NRF5) \ || defined(ARDUINO_ARCH_RP2040) class Cpu { public: typedef size_t state_t; static inline state_t disableInterrupts(void) { state_t save = __get_PRIMASK(); __disable_irq(); return save; } static inline void restoreInterrupts(state_t save) { __set_PRIMASK(save); } }; #else class Cpu { public: typedef size_t state_t; static inline state_t disableInterrupts(void) { noInterrupts(); return 0; } static inline void restoreInterrupts(state_t save) { (void)save; interrupts(); } }; #endif |
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 |
/* nmea.h - NMEA Library for Arduino Copyright (c) 2024 Sasapea's Lab. All right reserved. This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or at your option) any later version. This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. You should have received a copy of the GNU General Public License along with this program. If not, see <https://www.gnu.org/licenses/>. */ #pragma once #include <functional> #include <stdio.h> #include <stdint.h> #include <stdbool.h> #include <string.h> #include "Arduino.h" #define NMEA_DEBUG 0 #if NMEA_DEBUG extern HardwareSerial Serial; #endif #define DECLARE_NMEA(instance) NMEA<typeof(instance), instance> template<typename T, T& UART> class NMEA { public: typedef std::function<void(size_t sentence, const char **values, size_t count)> callback_t; typedef enum { SENTENCE_ALL, SENTENCE_RMC, SENTENCE_GGA, SENTENCE_GSA, } SENTENCE; /* RMC Message (Recommended minimum data）*/ typedef enum { RMC_Name, /* TrackerID + Sentence Name */ RMC_UTC_time, /* the format is hhmmss.sss */ RMC_Positioning_status, /* A=effective positioning, V=invalid positioning */ RMC_Latitude, /* the format is ddmm.mmmmmmm */ RMC_Latitude_hemisphere, /* N or S (north latitude or south latitude) */ RMC_Longitude, /* the format is dddmm.mmmmmmm */ RMC_Longitude_hemisphere, /* E or W (east longitude or west longitude) */ RMC_Ground_speed, /* Ground speed */ RMC_Ground_heading, /* take true north as the reference datum */ RMC_UTC_date, /* the format is ddmmyy (day, month, year) */ RMC_Magnetic_declination, /* 000.0~180.0 degrees */ RMC_Magnetic_direction, /* E (east) or W (west) */ RMC_Mode_indication, /* A=autonomous positioning, D=differential, E=estimation, N=invalid data */ } RMC; /* CGA Message (Global positioning system fix data) */ typedef enum { GGA_Name, /* TrackerID + Sentence Name */ GGA_UTC_time, /* the format is hhmmss.sss */ GGA_Latitude, /* the format is ddmm.mmmmmmm */ GGA_Latitude_hemisphere, /* N or S (north latitude or south latitude) */ GGA_Longitude, /* the format is dddmm.mmmmmmm */ GGA_Longitude_hemisphere, /* E or W (east longitude or west longitude) */ GGA_GNSS_positioning_status, /* 0 not positioned, 1 single point positioning, 2 differential GPS fixed solution, 4 fixed solution, 5 floating point solution */ GGA_Number_of_satellites_used, /* Number of satellites used */ GGA_HDOP, /* level precision factor, */ GGA_Altitude, /* Altitude */ GGA_Altitude_label, /* Altitude label */ GGA_Geoid, /* The height of the earth ellipsoid relative to the geoid */ GGA_Geoid_label, /* Geoid label */ GGA_Differential_time, /* Differential time */ } GGA; /* GSA Message (GNSS DOP and active satellites）*/ typedef enum { GSA_Name, /* TrackerID + Sentence Name */ GSA_Mode, /* M=Manual, A=Auto */ GSA_Positioning_type, /* 1=not positioned, 2=two-dimensional positioning, 3=three-dimensional positioning */ GSA_PRN_code, /* Pseudo Random Noise Code, channels 1 to 12, up to 12 */ GSA_PDOP, /* position precision factor */ GSA_HDOP, /* level precision factor */ GSA_VDOP, /* vertical precision factor */ GSA_GNSS_system_ID, /* 0(QZSS), 1(GPS), 2(GLONASS), 3(GALILEO), 4(BeiDou), 5(BEIDOU) */ } GSA; static void begin(void) { UART.setTimeout(0); } static void setCallback(SENTENCE sentence, callback_t callback) { _callbacks[sentence] = callback; } static void handle(void) { if (_count >= sizeof(_buffer)) _count = 0; int n = UART.readBytes(_buffer + _count, sizeof(_buffer) - _count); if (n > 0) { #if NMEA_DEBUG & 2 Serial.printf("%.*s", n, _buffer + _count); #endif _count += n; char *p, *q; while ((p = (char *)memchr(q = _buffer, '\n', _count))) { if (((size_t)(p - q) >= 6) && (*q == '$') && (p[-4] == '*')) { char cs = strtol(p - 3, nullptr, 16); for (++q; q < p - 4; ) cs ^= *q++; *q = 0; if (cs == 0) parse(_buffer + 1); } ++p; memcpy(_buffer, p, _count -= (size_t)(p - _buffer)); } } } static void write(const char *cmd) { char buf[256], cs = 0; for (const char *p = cmd; *p; ) cs ^= *p++; UART.write(buf, snprintf(buf, sizeof(buf), "$%s*%02X\r\n", cmd, cs)); } static float todd(float d) { /* return to decimal degree */ d /= 100; return (int)d + (d - (int)d) / 0.6; } private: static void parse(char *p) { char *values[32] = {}; values[0] = p; size_t count = 1; while ((count < sizeof(values) / sizeof(values[0])) && (p = strchr(p, ','))) { *p = 0; values[count++] = ++p; } for (size_t i = 0; i < sizeof(SENTENCES) / sizeof(SENTENCES[0]); ++i) { if ((i == 0) || (strcmp(SENTENCES[i], values[0] + 2) == 0)) { if (_callbacks[i]) _callbacks[i](i, (const char **)values, count); if (i) break; } } } /* static inline is C++17 and later */ static inline constexpr const char *SENTENCES[] = { "", /* all */ "RMC", "GGA", "GSA", }; static inline callback_t _callbacks[sizeof(SENTENCES)/sizeof(SENTENCES[0])]; static inline size_t _count; static inline char _buffer[256]; }; |
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230 231 232 233 234 235 236 237 238 239 240 241 242 243 244 245 246 247 248 249 250 251 252 253 254 255 256 257 258 259 260 261 262 263 264 265 266 267 268 269 270 271 272 273 274 275 276 277 278 279 280 281 282 283 284 285 286 287 288 289 290 291 292 293 294 295 296 297 298 299 300 301 302 303 304 305 306 307 308 309 310 311 312 313 314 315 316 317 318 319 320 321 322 323 324 325 326 327 328 329 |
/* sun.h - Sun Time and Position Library Copyright (c) 2024 Sasapea's Lab. All right reserved. This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or at your option) any later version. This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. You should have received a copy of the GNU General Public License along with this program. If not, see <https://www.gnu.org/licenses/>. */ #pragma once #ifndef _POSIX_THREAD_SAFE_FUNCTIONS #define _POSIX_THREAD_SAFE_FUNCTIONS #endif #include <math.h> #include <time.h> #pragma push_macro("DEG") #pragma push_macro("RAD") #undef DEG #define DEG (180 / M_PI) #undef RAD #define RAD (M_PI / 180) class Sun { public: void begin(void) { _rise = -1; _set = -1; _degh = -1; _dega = -1; _in0 = -1; } /************************************* 現在日の出/日の入り時刻を計算する *************************************/ void time(float lat, float lon) { time(lat, lon, timeZoneOffset()); } void time(float lat, float lon, int tzoff) { struct tm tm; time_t now = ::time(nullptr); if (tzoff) ::localtime_r(&now, &tm); else ::gmtime_r(&now, &tm); time(tm, lat, lon, tzoff); } /************************************* 指定日の日の出/日の入り時刻を計算する *************************************/ void time(const struct tm &date, float lat, float lon) { time(date, lat, lon, timeZoneOffset()); } void time(const struct tm &date, float lat, float lon, int tzoff) { int n = dayOfYear(date); int m = isLeapYear(date.tm_year + 1900) ? 366 : 365; suntime(lat, lon, n, m, tzoff, _rise, _set); } /************************************* 日の出時刻を取得 *************************************/ int rise(void) { return _rise; } /************************************* 日の入り時刻を取得 *************************************/ int set(void) { return _set; } /************************************* 時間値の抽出 *************************************/ static void timepart(int value, int &hour, int &minute, int &second) { hour = value / 3600 % 24; minute = value / 60 % 60; second = value % 60; } /************************************* 現在時刻の太陽の位置を計算する *************************************/ void pos(float lat, float lon, float lons = 0) { struct tm tm; time_t now = ::time(nullptr); if (lons) ::localtime_r(&now, &tm); else ::gmtime_r(&now, &tm); pos(tm, lat, lon, lons); } /************************************* 指定時刻の太陽の位置を計算する *************************************/ void pos(const struct tm &date, float lat, float lon, float lons = 0) { float t, dlt; sunpos(date, lon, lons, t, dlt, _in0); horisys(lat, t, dlt, _degh, _dega); } /************************************* 太陽高度(度)を取得 *************************************/ float degh(void) { return _degh; } /************************************* 太陽方位角(北:0度、東:90度、南:180度、西:270度)を取得 *************************************/ float dega(void) { return _dega; } /************************************* 大気外法線面日射量を取得 *************************************/ float in0(void) { return _in0; } protected: static int timeZoneOffset(void) { static int tzoff = 1; if (tzoff == 1) { struct tm l, g; time_t t = ::time(nullptr); tzoff = mktime(localtime_r(&t, &l)) - mktime(gmtime_r(&t, &g)); } return tzoff; } static bool isLeapYear(int year) { return ((year % 4 == 0) && ((year % 100 != 0) || (year % 400 == 0))); } static int dayOfYear(const struct tm &date) { const int YDAYS[] = { 0, 31, 59, 90, 120, 151, 181, 212, 243, 273, 304, 334 }; return YDAYS[date.tm_mon] + date.tm_mday + ((date.tm_mon >= 2) && isLeapYear(date.tm_year + 1900)) - 1; } /************************************************************************* Original Source: http://radiopench.blog96.fc2.com/blog-entry-735.html Arduinoで日の出・日の入り時刻を計算 近似式を使って日の出・日の入り時刻を求める。 by ラジオペンチ, 2017/5/10, http://radiopench.blog96.fc2.com/ 参考サイト http://k-ichikawa.blog.enjoy.jp/etc/HP/js/sunRise/srs.html http://www.iot-kyoto.com/satoh/2016/01/22/post-99/ *************************************************************************/ static void suntime(float lat, float lon, int nday, int mday, int tzsec, int &rise, int &set) { lat *= RAD; rise = sunrise(lon, lat, nday, mday) * 3600 + tzsec; if (rise < 0) rise += 86400; set = sunset(lon, lat, nday, mday) * 3600 + tzsec; if (set < 0) set += 86400; } static float sunrise(float x, float y, int n, int m) // 日の出時刻を求める関数 { float d = dcalc(n, m); // 太陽赤緯を求める float e = ecalc(n, m); // 均時差を求める // 太陽の時角幅を求める (視半径、大気差などを補正 (-0.899度)) float t = acos((sin(-0.899 * RAD) - sin(d) * sin(y)) / (cos(d) * cos(y))) * DEG; return (-t + 180 - x) / (360 / 24) - e; // 日の出時刻を返す } static float sunset(float x, float y, int n, int m) // 日の入り時刻を求める関数 { float d = dcalc(n, m); // 太陽赤緯を求める float e = ecalc(n, m); // 均時差を求める // 太陽の時角幅を求める (視半径、大気差などを補正 (-0.899度)) float t = acos((sin(-0.899 * RAD) - sin(d) * sin(y)) / (cos(d) * cos(y))) * DEG; return (t + 180 - x) / (360 / 24) - e; // 日の入り時刻を返す } static float dcalc(int n, int m) // 近似式で太陽赤緯を求める { float w = (n + 0.5) * 2 * M_PI / m; // 日付をラジアンに変換 float d = + 0.33281 - 22.984 * cos(w) - 0.34990 * cos(2 * w) - 0.13980 * cos(3 * w) + 3.7872 * sin(w) + 0.03250 * sin(2 * w) + 0.07187 * sin(3 * w); return d * RAD; // 赤緯を返す（単位はラジアン） } static float ecalc(int n, int m) // 近似式で均時差を求める { float w = (n + 0.5) * 2 * M_PI / m; // 日付をラジアンに換算 float e = + 0.0072 * cos(w) - 0.0528 * cos(2 * w) - 0.0012 * cos(3 * w) - 0.1229 * sin(w) - 0.1565 * sin(2 * w) - 0.0041 * sin(3 * w); return e; // 均一時差を返す（単位は時） } int _rise; int _set; /************************************************************************* Original Source: 年差を考慮した太陽位置の簡易計算 (TE_Simplified_SP_230724.pdf) 202306 AKASAKA Hiroshi 太陽位置を計算するプログラム 地点緯度lat, 経度lon, 標準時経度lons, 計算対象日付dateを指定する。 株式会社気象データシステム（https://metds.co.jp/) 資料一覧: https://metds.co.jp/documents/ea/ *************************************************************************/ static void sunpos(const struct tm &date, float lon, float lons, float &t, float &dlt, float &in0) { const float J0 = 1.37; int year = date.tm_year + 1900; // // 黄道傾斜角の計算 // float dlt0 = -23.4393 + 0.013 * (year - 2000.0) / 100; // // 地方標準時（日本ならJST)1月1日基準の通算計算対象時刻nday(日)の計算 // float nday = dayOfYear(date) + date.tm_hour / 24.0 + date.tm_min / (24.0 * 60.0) + date.tm_sec / (24.0 * 60.0 * 60.0) - lons / (24.0 * 15.0); // // 平均近点角mの計算 // float n = year - 1968.0; float m = 0.9856 * (nday - (3.71 + 0.2596 * n - (int)((n + 3.0) / 4.0))); // // 冬至点と近日点がなす角度eps、真近点角vの計算 // float eps = 12.3901 + 0.0172 * (n + m / 360.0); float v = m + 1.918 * sin(m * RAD) + 0.02 * sin(2.0 * m * RAD); // // 均時差et(°)の計算 // float veps = (v + eps) * RAD; float et = (m - v) - atan(0.043 * sin(2.0 * veps) / (1.0 - 0.043 * cos(2.0 * veps))) / RAD; // // 大気外法線面日射量IN0の計算 // in0 = J0 * (1.0 + 0.033 * cos(RAD * v)); // // 視赤緯dlt(°)の計算 // float sindlt = cos(veps) * sin(RAD * dlt0); dlt = asin(sindlt) / RAD; // // 時角t(°)の計算 // t = 15.0 * (date.tm_hour + date.tm_min / 60.0 + date.tm_sec / 3600.0 - 12.0) + (lon - lons) + et; } /*************************************************************************************** 緯度lat(°)、時角t(°)、視赤緯dlt(°)、均時差et(°)を受け、 太陽高度degh(°)、太陽方位角dega(°)を返すサブプログラム ****************************************************************************************/ static void horisys(float lat, float t, float dlt, float °h, float °a) { float sinh = sin(RAD * lat) * sin(RAD * dlt) + cos(RAD * lat) * cos(RAD * dlt) * cos(RAD * t); float cosh = sqrt(1.0 - sinh * sinh); float sina = cos(RAD * dlt) * sin(RAD * t) / cosh; float cosa = (sinh * sin(RAD * lat) - sin(RAD * dlt)) / (cosh * cos(RAD * lat)); // degh = asin(sinh) / RAD; if (sina > 0) dega = 180 + 90.0 - atan(cosa / sina) / RAD; else if (sina == 0) dega = 0.0; else dega = 180 - 90.0 - atan(cosa / sina) / RAD; } float _degh; float _dega; float _in0; }; #pragma pop_macro("RAD") #pragma pop_macro("DEG") |