チャリコンの制作 あの坂を登れ
-
プロローグ
自転車趣味の分野で「獲得高」というのがあります。これは標高でなく「坂を上った高度の累積」です。ツーリングでどれだけ頑張ったかの目安になります。これを計測したいと思います。
それと自転車で山越時に緩い坂、急な坂を上り下りしてると次第に上っているのか下っているのかわからなくなる時があります。乗車中に「勾配の計測」が欲しいと思いました。
巷にはサイクルコンピューターなる商品があるようですが自作する事にします。 -
要求仕様
・坂の計測機能
角度でなく勾配率%で示したい
(%とは例えば100m先が10m高くなっていたら10%の勾配という事になります)
・獲得高計測機能
上っている時の高さのみ累積したい。
・トリップメータ機能
ツーリング中の距離は計測したい。
・スピード表示機能
速度も出ると嬉しい。ただしなるべくシンプルに。
・防水である事
自転車に積むので、風雨にさらされて壊れてはいけません。
・夜間に表示が見える事
ツーリングをしていると夜になる事があります。夜間に表示が光るとよいです。
・電池の持ちが良い事
朝から晩までのツーリングを考慮し、12時間程度は動作してほしい。
・電池は2本で動作する事。
3本以上だと重くなりますし、不経済です。 -
ソリューション
・「勾配の計測」→Gセンサー(加速度センサー)で計測する事にします。
・「獲得高の計測」→高度を得る為に当初GPS測位の結果のNMEAに含まれる高度を利用しましたが、精度に難がある事が解り気圧から求めます。
・「トリップメータ」→GPS測位の結果のNMEAに含まれる、緯度、経度から求めます。
・「スピード表示機能」→GPS測位の結果のNMEAに含まれる速度を利用します。
・「防水である事」→弁当箱を防水ケース代りにします。操作は内側に張り付けたリードスイッチを磁石で操作します。
・「夜間に表示が見える事」→バックライト付きのLCDモジュールを使います。
・「電池の持ちが良い事」→GPSモジュールの電源をMOSFETを使って不要な時にOFF出来るようにします。LCDのバックライトも周囲の明るさに応じて光るようにします。
・「電池は2本で動作する事」→昇圧DCDCコンバータを用いる事にします。 -
主要構成部品
マイコン
NXP KL46Z
低消費電力な ARM Coretex-M0+ をコアにもつマイコンです。加速度センサーをオンボードで搭載していますので都合が良いです。
このマイコンに合わせてチャリコンに必要な部品を載せたシールドを作成していきます。
ヘッダソケットのピンアサインはArduino Uno 3と同じになっています。
気圧センサー、GPSモジュール、バックライト付きLCD
左から「気圧センサ(BMP180)」、「GPSモジュール(AE-GYSFDMAXB)」、「LCD(AQM1602Y-FLW-FBW)」
リードスイッチ
ケース内部に配線しやすいように、リードスイッチをワイヤーハーネス化します。
DCDCコンバータ(AE-XCL102D503CR-G)と電池BOX
電池ボックスの単3電池2本の電圧から5Vを発生させるように、アッセンブリを作ります。
※リードスイッチはDCDCコンバータにつながり電源を筐体の外からOFF/ON出来ます
ケース
100均の弁当箱にリフレクタの固定機構の一部を流用し
-
回路図
気圧センサ、GPSモジュールをマイコンに載せるシールドとして作成します
全部組み付けると以下のような感じになります。
-
ソフトウェア
マイコンがmbed対応なので、mbed OS5 を使う事します。
以下のように3スレッドで設計します。
「メインスレッド」
LCD表示が主な責務です。GPSスレッドのNMEA受信のイベントを待っています。
GPSの受信イベントが起きると、センサースレッドの計測値(気圧高度、勾配率)を取り込み「獲得高」を計算、測位情報(速度、高度)とともに、LCDに表示します。
「GPSスレッド」
GPSモジュールからNMEAを受信、それをパースをし、時刻、高度、緯度、経度、速度 を取り出します。
ひとおり受信出来たら、メインスレッドにイベントをセットします。
※GPSは1秒毎に新しいNMEAを送ってきて更新します。
「センサースレッド」
二つのセンサーの値をI2Cを使って傾斜、高度を求めます。
a.加速度センサーY軸(前方向)とZ軸(下方向)の加速度を得て、水平に対する角度を求めます。
角度からtanを用いて勾配率を求めます。振動等の一時的乱れを排除する為平均化処理をします。
b.気圧から高度を求めます。 -
動作の様子
LCDの上段はNMEAの値から「進行方向」、「速度」を示しています。
LCDの下段はNMEAの値から「高度」、Gセンサーの値から求めた「勾配率」気圧センサーから求めた「獲得高」を示しています。
-
ソースコード
メインスレッド
メインスレッド
#include "resources.h"
#include "sens_thread.h"
#include "gps_thread.h"
#include "debug.h"
#include "mbed.h"
#include "SB1602E.h"
DigitalOut led( LED1 );
AnalogIn lightSense(PTE22);
SB1602E lcd(I2C_SDA, I2C_SCL);
uint32_t num = 0;
PwmOut LCDbackLight(D9);
DigitalOut GPSPower(D7);
PwmOut Buzzer(D6);
InterruptIn SwitchHg(D2,PullUp);
InterruptIn SwitchA(D3,PullUp);
InterruptIn SwitchB(D5,PullUp);
int mainLoopConter = 0;
#define BLINKING_RATE 100
const char * directionCharTbl[17]={"N ","NE","NE","E ","E ","SE","SE","S ","S ","SW","SW","W ","W ","NW","NW","N "," "};
static int getHight = 0;
static int releaseHight = 0;
uint8_t pos;
int dispFunction=0;
time_t rtcSeconds = time(NULL);
#define RAD_DEG (180.0 / M_PI)
double Lastlongitude=0;
double Lastlatitude=0;
double DeltaDistance=0;
float TripMeter=0;
bool SwitchHg_act=false;
void SwitchA_push(void)
{
Buzzer.period(1.0/2200);
Buzzer.write(0.5f);
}
void SwitchA_release(void)
{
Buzzer.write(0.0f);
}
void SwitchB_push(void)
{
Buzzer.period(1.0/3000);
Buzzer.write(0.5f);
}
void SwitchB_release(void)
{
Buzzer.write(0.0f);
}
void SwitchHg_push(void)
{
}
void SwitchHg_release(void)
{
}
int main()
{
float latestPressHight= -999;
int freeDispNum=0;
char freeDispFn=0x20;
nmeaBufferInitialize();
LCDbackLight = 1.0f;
GPSPower = 1;
SwitchHg.fall(&SwitchHg_push);
SwitchHg.rise(&SwitchHg_release);
SwitchA.fall(&SwitchA_push);
SwitchA.rise(&SwitchA_release);
SwitchB.fall(&SwitchB_push);
SwitchB.rise(&SwitchB_release);
thread_sens.start(mbed::callback(thread_sens_func));
thread_gps.start(mbed::callback(thread_gps_func));
lcd.printf(0, "Hello world!\r");
lcd.printf(1, "TENAGA Labo(^-^)\r");
ThisThread::sleep_for(1000);
while ( true ) {
uint32_t flag = event_flags.wait_any(FLAG_PLAY);
unsigned int directionIndex = (unsigned int)floor(rxVTG.tCource/22.5);
const char *directionChar = directionCharTbl[16];
LCDbackLight = lightSense;
if (SwitchA.read() == 0) {
dispFunction++;
if (dispFunction >= 6)
dispFunction = 0;
}
if (SwitchB.read() == 0) {
TripMeter = 0;
getHight=0;
releaseHight=0;
}
// Calcurate Get Hight every 10 Second
if ((mainLoopConter % 10) == 0) {
// Prssure
if( latestPressHight > -999 ) {
if (latestPressHight < PressHight) {
getHight += (int) floor(PressHight - latestPressHight);
} else {
releaseHight += (int) floor(latestPressHight - PressHight);
}
latestPressHight = PressHight;
}else if(PressHight > -999){
latestPressHight = PressHight;
}
// Get Distance
if(Lastlatitude > 0 && Lastlongitude > 0 && rxGGA.latitude > 0 && rxGGA.longitude > 0){
// https://keisan.casio.jp/exec/system/1257670779
DeltaDistance = 6378.137 * acos (
sin(RAD_DEG * Lastlatitude) * sin(RAD_DEG * rxGGA.latitude) +
cos(RAD_DEG * Lastlatitude) * cos(RAD_DEG * rxGGA.latitude) * cos(RAD_DEG * (rxGGA.longitude - Lastlongitude))
);
if (rxVTG.kmet > 1.0) {
if ((((rxVTG.kmet / 3.6) * 10.0f)*1.1) > DeltaDistance) {
TripMeter += DeltaDistance;
}
}
Lastlatitude = rxGGA.latitude;
Lastlongitude = rxGGA.longitude;
}else if (rxGGA.latitude > 0 && rxGGA.longitude> 0 ){
Lastlatitude = rxGGA.latitude;
Lastlongitude = rxGGA.longitude;
}
}
/* GPS Heading to directio string */
if(directionIndex < 16) directionChar = directionCharTbl[directionIndex];
pos = tsi.readDistance();
if (pos == 0) {
// nop
} else if (pos <= TS_THRESHOLD) {
dispFunction = 1;
} else if (pos <= TS_THRESHOLD * 2) {
dispFunction = 2;
} else if (pos <= TS_THRESHOLD * 3) {
dispFunction = 3;
} else if (pos <= TS_THRESHOLD * 4) {
dispFunction = 4;
} else if (pos <= TS_THRESHOLD * 5) {
dispFunction = 5;
}else{
dispFunction = 6;//function non
}
char functionString[17]={0};
switch (dispFunction){
case 0:
freeDispNum = (int)floor(PressHight);
freeDispFn = 'P';
sprintf(functionString,"%c%3dm\r",freeDispFn, freeDispNum );
break;
case 1:
freeDispFn = 'T';
sprintf(functionString,"%02d:%02d:%02d\r",((rxZDA.hh)+9)%24, rxZDA.mm, rxZDA.ss );
break;
case 2:
freeDispNum = TripMeter;
freeDispFn = 'M';
sprintf(functionString,"%2.3fKm\r",(TripMeter/1000.0f) );
break;
case 3:
freeDispFn = 'd';
sprintf(functionString,"%c%2.1fm\r",freeDispFn, DeltaDistance );
break;
case 4:
freeDispFn = 'D';
sprintf(functionString,"%c%4dm\r",freeDispFn, (int)floor(releaseHight) );
break;
case 5:
freeDispFn = 'U';
sprintf(functionString,"%c%4dm\r",freeDispFn, (int)floor(getHight) );
break;
default :
break;
}
lcd.printf(1, "H%3.0fm%c%2d%% %c%04d\r", rxGGA.alt,0x20, GradientRate, freeDispFn, freeDispNum);
if (flag == FLAG_PLAY) {
lcd.printf(0, "%s%3.0f%c %2.0fKm/h\r", directionChar, rxVTG.tCource ,0xdf,rxVTG.kmet); // line# (0 or 1), string
lcd.printf(1, "%3.0fm%2d%% %s", rxGGA.alt, GradientRate, functionString);
}
led = !led;
mainLoopConter ++;
}
thread_sens.join();
thread_gps.join();
}
GPSスレッド
#include "resources.h"
#include "gps_thread.h"
#include "debug.h"
bool status_play = false;
GPSrxBufferDouble_t Nmea;
gpGGA_t rxGGA;
float longitude;
float latitude;
float gtime;
int sats;
float hdop;
float alt;
float geoid;
char ns, ew, unit;
int lock;
gpVTG_t rxVTG;
float tCource;
float mCource;
float knot;
float kmet;
char tc;
char mc;
char knc;
char kmc;
char nmode;
float zdatime;
int zdaday;
int zdamonth;
int zdayear;
gpZDA_t rxZDA;
void nmeaBufferInitialize(void)
{
// int initializeLoop;
for(int initializeLoop = 0;RXNMEA_NUMBER > initializeLoop; initializeLoop++){
memset(Nmea.rxNmea[initializeLoop].RxBuffer ,0, sizeof(Nmea.rxNmea[initializeLoop].RxBuffer));
Nmea.rxNmea[initializeLoop].RxCount = 0;
Nmea.rxNmea[initializeLoop].isAvailable = false;
}
Nmea.writeableSelect = 0;
}
DigitalOut led2( LED2 );
static RawSerial GPSserial_port(D1, D0, 9600);
bool isRecive=false;
unsigned char interrupuRxCount=0;
void dev_recv() {
bool isRxdone = false;
while (GPSserial_port.readable()) {
char rxChar = GPSserial_port.getc();
if ('$' == rxChar) {
if (isRecive == false
&& Nmea.rxNmea[Nmea.writeableSelect].isAvailable == false) {
isRecive = true;
interrupuRxCount = 0;
Nmea.rxNmea[Nmea.writeableSelect].RxBuffer[0] = 0x00;
Nmea.rxNmea[Nmea.writeableSelect].RxCount = 0;
}
} else if (0x0d == rxChar) {
if (isRecive
&& Nmea.rxNmea[Nmea.writeableSelect].isAvailable == false) {
isRecive = false;
Nmea.rxNmea[Nmea.writeableSelect].RxBuffer[interrupuRxCount] =
0x00;
Nmea.rxNmea[Nmea.writeableSelect].RxCount = interrupuRxCount;
Nmea.rxNmea[Nmea.writeableSelect].isAvailable = true;
isRxdone = true;
if ((Nmea.writeableSelect + 1) == RXNMEA_NUMBER) {
Nmea.writeableSelect = 0;
} else {
Nmea.writeableSelect++;
}
}
} else {
if (isRecive
&& Nmea.rxNmea[Nmea.writeableSelect].isAvailable == false) {
if (interrupuRxCount < (RXNMEA_BUFFER_SIZE - 1)) {
Nmea.rxNmea[Nmea.writeableSelect].RxBuffer[interrupuRxCount] =
rxChar;
interrupuRxCount++;
}
}
}
}
if (isRxdone) {
event_flags.set(FLAG_BUTTON_0);
}
}
void thread_gps_func(void) {
unsigned int readableSelect = 0;
char *msg = Nmea.rxNmea[readableSelect].RxBuffer;
GPSserial_port.attach(&dev_recv, Serial::RxIrq);
while (true) {
uint32_t flag = event_flags.wait_any(FLAG_ALL);
led2 = !led2;
msg = Nmea.rxNmea[readableSelect].RxBuffer;
if (sscanf(msg, "GPGGA,%f,%f,%c,%f,%c,%d,%d,%f,%f,%c,%f", >ime,
&latitude, &ns, &longitude, &ew, &lock, &sats, &hdop, &alt,
&unit, &geoid) >= 1) {
if (ns == 'S') {
latitude *= -1.0;
}
if (ew == 'W') {
longitude *= -1.0;
}
float degrees = trunc(latitude / 100.0f);
float minutes = latitude - (degrees * 100.0f);
latitude = degrees + minutes / 60.0f;
degrees = trunc(longitude / 100.0f);
minutes = longitude - (degrees * 100.0f);
longitude = degrees + minutes / 60.0f;
rxGGA.alt = alt;
rxGGA.geoid = geoid;
rxGGA.latitude = latitude;
rxGGA.longitude = longitude;
} else if (sscanf(msg, "GPVTG,%f,%c,,%c,%f,%c,%f,%c,%c", &tCource, &tc,
&mc, &knot, &knc, &kmet, &kmc, &nmode) >= 1) {
//GPVTG,169.01,T,,M,0.05,N,0.08,K,A*3F
rxVTG.tCource = tCource;
rxVTG.mCource = mCource;
rxVTG.knot = knot;
rxVTG.kmet = kmet;
event_flags.set(FLAG_PLAY);
} else if (sscanf(msg, "GPZDA,%f,%d,%d,%d", &zdatime, &zdaday,
&zdamonth, &zdayear) >= 1) {
rxZDA.hh = (char) ((int) floor(zdatime) / 10000);
rxZDA.mm = (char) (((int) floor(zdatime) / 100) % 100);
rxZDA.ss = (char) ((int) floor(zdatime) % 100);
rxZDA.day = (char) zdaday;
rxZDA.month = (char) zdamonth;
rxZDA.year = zdayear;
}
Nmea.rxNmea[readableSelect].isAvailable = false;
if ((readableSelect + 1) == RXNMEA_NUMBER) {
readableSelect = 0;
} else {
readableSelect++;
}
}
}
センサースレッド
#include "resources.h"
#include "sens_thread.h"
#include "MMA8451Q.h"
#include "MAG3110.h"
#include "BMP180.h"
#include <math.h>
#include "debug.h"
#define PI314F 3.14159265f
#define RAD_DEG (180.0f / PI314F)
#define N_SAMPLES 250
#define SAMPLE_WAIT 0.02f
#define BLINKING_RATE 20
#if defined (TARGET_KL25Z) || defined (TARGET_KL46Z)
PinName const SDA = PTE25;
PinName const SCL = PTE24;
#elif defined (TARGET_KL05Z)
PinName const SDA = PTB4;
PinName const SCL = PTB3;
#elif defined (TARGET_K20D50M)
PinName const SDA = PTB1;
PinName const SCL = PTB0;
#else
#error TARGET NOT DEFINED
#endif
#define MMA8451_I2C_ADDRESS (0x1d<<1)
InterruptIn sw3(PTC12);
DigitalIn sw2in(PTC12);
char buffer[32];
int sw_status=0;
float yAdj=0;
float zAdj=0;
float yMul=0;
float zMul=0;
int GradientRate;
float MagHeading;
float PressHight=-999;
float PressAVG;
int PressAVGcount;
void sw3_push(void)
{
sw_status=1;
yAdj=0;
zAdj=0;
}
void sw3_release(void)
{
sw_status=0;
}
MMA8451Q acc(SDA, SCL, MMA8451_I2C_ADDRESS);
MAG3110 mag(PTE25, PTE24);
I2C i2c(D14, D15);
BMP180 bmp180(&i2c);
void thread_sens_func(void)
{
float x, y, z, per, zPer, yRad, zRad, yDeg, zDeg, yGraAve, zGraAve;
float sampleG[N_SAMPLES]={0.0f};
float sampleGz[N_SAMPLES]={0.0f};
unsigned int sampleMtxIndex=0;
unsigned int sampleLoop=0;
signed char temperature;
sw3.fall(&sw3_push);
sw3.rise(&sw3_release);
sw_status = 1;
int sampleCount=0;
//********** SW1 MAG CAL **********
// mag.calXY(PTC3, 0);
//********** SW1 MAG CAL **********
while (1) {
if (bmp180.init() != 0) {
// error
} else {
// Success
break;
}
ThisThread::sleep_for(BLINKING_RATE);
}
while (true) {
int iy;
y = acc.getAccY(); // use
z = acc.getAccZ(); // use
sampleG[sampleMtxIndex]=y; // y ACC
sampleGz[sampleMtxIndex]=z; // z ACC
sampleMtxIndex++;
if(sampleMtxIndex >= N_SAMPLES) {
sampleMtxIndex=0;
yGraAve=0;
zGraAve=0;
for(sampleLoop=0; sampleLoop < N_SAMPLES; sampleLoop++){
yGraAve = yGraAve + sampleG[sampleLoop];
zGraAve = zGraAve + sampleGz[sampleLoop];
}
yGraAve = yGraAve / N_SAMPLES;
zGraAve = zGraAve / N_SAMPLES;
if(sw_status == 1){
yAdj = yGraAve;
zAdj = zGraAve;
sw_status=0;
}
if(yGraAve > 1.0f) yGraAve = 1.0f; // fix arc:sin/cos "nan"
if(zGraAve > 1.0f) zGraAve = 1.0f; // fix arc:sin/cos "nan"
yRad=asin(yGraAve);
zRad=acos(zGraAve);
yMul = 1.0f/( ( (PI314F/2)-asin(yAdj) )/(PI314F/2) );
yRad=yRad-asin(yAdj);
yRad=yRad*yMul;
zMul = 1.0f/( ( (PI314F/2)-acos(zAdj) )/(PI314F/2) );
zRad=zRad-acos(zAdj);
zRad=zRad*zMul;
yDeg=yRad*RAD_DEG;
zDeg=zRad*RAD_DEG;
per=tan(yRad);
zPer=tan(zRad);
per=per*100;
zPer=zPer*100;
iy=(int)floor(per);
GradientRate = iy;
}
MagHeading = mag.getHeading();
// Atmospheric pressure and Temperature
bmp180.startTemperature();
ThisThread::sleep_for(5);
float Temperature;
if(bmp180.getTemperature(&Temperature) != 0) {
//error
}
bmp180.startPressure(BMP180::ULTRA_LOW_POWER);
ThisThread::sleep_for(10);
int pressure;
if(bmp180.getPressure(&pressure) != 0) {
//error
}
float tmpCALC = (float)pressure;
PressAVG += ( ( pow((101325.0f/tmpCALC), (1/5.275f)) - 1 ) * (Temperature+273.15)) / 0.0065f;
PressAVGcount++;
if( PressAVGcount >= 50){
PressHight = PressAVG/PressAVGcount;
PressAVGcount = 0;
PressAVG = 0;
}
ThisThread::sleep_for(20);
}
}
-
Shimanacchan
さんが
2021/02/28
に
編集
をしました。
(メッセージ: 初版)
-
Shimanacchan
さんが
2022/02/05
に
編集
をしました。
ログインしてコメントを投稿する