吳勻昌 (yulie.wu )

2018年12月24日星期一

ST7567 12864LCD Arduino 328 使用U8g2 空間不足的問題

328使用U8g2會有空間不足的問題

ST7567 用 ST7565 的Library 畫面的對比度過高

就算調到0 還是太黑

對比度粗調位置 0x24 /* range 0x20~0x27 */

下列為測試檔和修改過的library

Test PDE

#include "ST7565.h"

int ledPin = 13; // LED connected to digital pin 13

// the LCD backlight is connected up to a pin so you can turn it on & off

// pin 9 - Serial data out (SID)
// pin 8 - Serial clock out (SCLK)
// pin 7 - Data/Command select (RS or A0)
// pin 6 - LCD reset (RST)
// pin 5 - LCD chip select (CS)
//ST7565 glcd(9, 8, 7, 6, 5);

//ST7565 glcd(SDA, SCK, RS/DC, RST, CS);

ST7565 glcd(5, 6, 4, 3, 2);
#define Power_3v3 7
#define Power_Gnd 8
#define BACKLIGHT_LED 9

//ST7565 glcd(9, 8, 7, 6, 5);
#define LOGO16_GLCD_HEIGHT 16
#define LOGO16_GLCD_WIDTH 16

// a bitmap of a 16x16 fruit icon
const static unsigned char __attribute__ ((progmem)) logo16_glcd_bmp[]={
0x30, 0xf0, 0xf0, 0xf0, 0xf0, 0x30, 0xf8, 0xbe, 0x9f, 0xff, 0xf8, 0xc0, 0xc0, 0xc0, 0x80, 0x00,
0x20, 0x3c, 0x3f, 0x3f, 0x1f, 0x19, 0x1f, 0x7b, 0xfb, 0xfe, 0xfe, 0x07, 0x07, 0x07, 0x03, 0x00, };

// The setup() method runs once, when the sketch starts
void setup() {
Serial.begin(115200);

pinMode(Power_3v3, OUTPUT); digitalWrite(Power_3v3, HIGH);
pinMode(Power_Gnd, OUTPUT); digitalWrite(Power_Gnd, LOW);
// pinMode(BACKLIGHT_LED, OUTPUT);
// analogWrite(BACKLIGHT_LED, 64);

#ifdef __AVR__
Serial.print(freeRam());
#endif

// turn on backlight

// initialize and set the contrast to 0x18
glcd.begin(0x08);
delay(200);
glcd.st7565_set_pre_contrast(0x20);/*0x20~0x27*/
// glcd.st7565_set_brightness(0x00);

glcd.display(); // show splashscreen
delay(2000);
glcd.clear();

// draw a single pixel
glcd.setpixel(10, 10, BLACK);
glcd.display(); // show the changes to the buffer
delay(2000);
glcd.clear();

// draw many lines
testdrawline();
glcd.display(); // show the lines
delay(2000);
glcd.clear();

// draw rectangles
testdrawrect();
glcd.display();
delay(2000);
glcd.clear();

// draw multiple rectangles
testfillrect();
glcd.display();
delay(2000);
glcd.clear();

// draw mulitple circles
testdrawcircle();
glcd.display();
delay(2000);
glcd.clear();

// draw a black circle, 10 pixel radius, at location (32,32)
glcd.fillcircle(32, 32, 10, BLACK);
glcd.display();
delay(2000);
glcd.clear();

// draw the first ~120 characters in the font
testdrawchar();
glcd.display();
delay(2000);
glcd.clear();

// draw a string at location (0,0)
glcd.drawstring(0, 0, "Lorem ipsum dolor sit amet, consectetur adipisicing elit, sed do eiusmod tempor incididunt ut labore et dolore magna aliqua. Ut enim ad minim veniam, quis nostrud exercitation");
glcd.display();
delay(2000);
glcd.clear();

// draw a bitmap icon and 'animate' movement
testdrawbitmap(logo16_glcd_bmp, LOGO16_GLCD_HEIGHT, LOGO16_GLCD_WIDTH);
}

void loop()
{}

#ifdef __AVR__
// this handy function will return the number of bytes currently free in RAM, great for debugging!
int freeRam(void)
{
extern int __bss_end;
extern int *__brkval;
int free_memory;
if((int)__brkval == 0) {
free_memory = ((int)&free_memory) - ((int)&__bss_end);
}
else {
free_memory = ((int)&free_memory) - ((int)__brkval);
}
return free_memory;
}
#endif

#define NUMFLAKES 10
#define XPOS 0
#define YPOS 1
#define DELTAY 2

void testdrawbitmap(const uint8_t *bitmap, uint8_t w, uint8_t h) {
uint8_t icons[NUMFLAKES][3];
randomSeed(666); // whatever seed

// initialize
for (uint8_t f=0; f< NUMFLAKES; f++) {
icons[f][XPOS] = random(128);
icons[f][YPOS] = 0;
icons[f][DELTAY] = random(5) + 1;
}

while (1) {
// draw each icon
for (uint8_t f=0; f< NUMFLAKES; f++) {
glcd.drawbitmap(icons[f][XPOS], icons[f][YPOS], logo16_glcd_bmp, w, h, BLACK);
}
glcd.display();
delay(200);

// then erase it + move it
for (uint8_t f=0; f< NUMFLAKES; f++) {
glcd.drawbitmap(icons[f][XPOS], icons[f][YPOS], logo16_glcd_bmp, w, h, 0);
// move it
icons[f][YPOS] += icons[f][DELTAY];
// if its gone, reinit
if (icons[f][YPOS] > 64) {
icons[f][XPOS] = random(128);
icons[f][YPOS] = 0;
icons[f][DELTAY] = random(5) + 1;
}
}
}
}

void testdrawchar(void) {
for (uint8_t i=0; i < 168; i++) {
glcd.drawchar((i % 21) * 6, i/21, i);
}
}

void testdrawcircle(void) {
for (uint8_t i=0; i<64; i+=2) {
glcd.drawcircle(63, 31, i, BLACK);
}
}

void testdrawrect(void) {
for (uint8_t i=0; i<64; i+=2) {
glcd.drawrect(i, i, 128-i, 64-i, BLACK);
}
}

void testfillrect(void) {
for (uint8_t i=0; i<64; i++) {
// alternate colors for moire effect
glcd.fillrect(i, i, 128-i, 64-i, i%2);
}
}

void testdrawline() {
for (uint8_t i=0; i<128; i+=4) {
glcd.drawline(0, 0, i, 63, BLACK);
}
for (uint8_t i=0; i<64; i+=4) {
glcd.drawline(0, 0, 127, i, BLACK);
}

glcd.display();
delay(1000);

for (uint8_t i=0; i<128; i+=4) {
glcd.drawline(i, 63, 0, 0, WHITE);
}
for (uint8_t i=0; i<64; i+=4) {
glcd.drawline(127, i, 0, 0, WHITE);
}
}

ST7565.h

/*
$Id:$

ST7565 LCD library!

Copyright (C) 2010 Limor Fried, Adafruit Industries

This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later version.

This library 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
Lesser General Public License for more details.

You should have received a copy of the GNU Lesser General Public
License along with this library; if not, write to the Free Software
Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA

// some of this code was written by <cstone@pobox.com> originally; it is in the public domain.
*/

#if ARDUINO >= 100
#include "Arduino.h"
#else
#include "WProgram.h"
#endif

#define swap(a, b) { uint8_t t = a; a = b; b = t; }

#define BLACK 1
#define WHITE 0

#define LCDWIDTH 128
#define LCDHEIGHT 64

#define CMD_DISPLAY_OFF 0xAE
#define CMD_DISPLAY_ON 0xAF

#define CMD_SET_DISP_START_LINE 0x40
#define CMD_SET_PAGE 0xB0

#define CMD_SET_COLUMN_UPPER 0x10
#define CMD_SET_COLUMN_LOWER 0x00

#define CMD_SET_ADC_NORMAL 0xA0
#define CMD_SET_ADC_REVERSE 0xA1

#define CMD_SET_DISP_NORMAL 0xA6
#define CMD_SET_DISP_REVERSE 0xA7

#define CMD_SET_ALLPTS_NORMAL 0xA4
#define CMD_SET_ALLPTS_ON 0xA5
#define CMD_SET_BIAS_9 0xA2
#define CMD_SET_BIAS_7 0xA3

#define CMD_RMW 0xE0
#define CMD_RMW_CLEAR 0xEE
#define CMD_INTERNAL_RESET 0xE2
#define CMD_SET_COM_NORMAL 0xC0
#define CMD_SET_COM_REVERSE 0xC8
#define CMD_SET_POWER_CONTROL 0x28
#define CMD_SET_RESISTOR_RATIO 0x20
#define CMD_SET_PRE_CONTRAST 0x24 /*2018-12-19 add range 0x20~0x27*/
#define CMD_SET_VOLUME_FIRST 0x81
#define CMD_SET_VOLUME_SECOND 0
#define CMD_SET_STATIC_OFF 0xAC
#define CMD_SET_STATIC_ON 0xAD
#define CMD_SET_STATIC_REG 0x0
#define CMD_SET_BOOSTER_FIRST 0xF8
#define CMD_SET_BOOSTER_234 0
#define CMD_SET_BOOSTER_5 1
#define CMD_SET_BOOSTER_6 3
#define CMD_NOP 0xE3
#define CMD_TEST 0xF0

class ST7565 {
public:
ST7565(int8_t SID, int8_t SCLK, int8_t A0, int8_t RST, int8_t CS) :sid(SID), sclk(SCLK), a0(A0), rst(RST), cs(CS) {}
ST7565(int8_t SID, int8_t SCLK, int8_t A0, int8_t RST) :sid(SID), sclk(SCLK), a0(A0), rst(RST), cs(-1) {}

void st7565_init(void);
void begin(uint8_t contrast);
void st7565_command(uint8_t c);
void st7565_data(uint8_t c);
void st7565_set_brightness(uint8_t val);
void st7565_set_pre_contrast(uint8_t pval); /* 2018-12-19 add */
void clear_display(void);
void clear();
void display();

void setpixel(uint8_t x, uint8_t y, uint8_t color);
uint8_t getpixel(uint8_t x, uint8_t y);
void fillcircle(uint8_t x0, uint8_t y0, uint8_t r,
uint8_t color);
void drawcircle(uint8_t x0, uint8_t y0, uint8_t r,
uint8_t color);
void drawrect(uint8_t x, uint8_t y, uint8_t w, uint8_t h,
uint8_t color);
void fillrect(uint8_t x, uint8_t y, uint8_t w, uint8_t h,
uint8_t color);
void drawline(uint8_t x0, uint8_t y0, uint8_t x1, uint8_t y1,
uint8_t color);
void drawchar(uint8_t x, uint8_t line, char c);
void drawstring(uint8_t x, uint8_t line, char *c);
void drawstring_P(uint8_t x, uint8_t line, const char *c);

void drawbitmap(uint8_t x, uint8_t y,
const uint8_t *bitmap, uint8_t w, uint8_t h,
uint8_t color);

private:
int8_t sid, sclk, a0, rst, cs;
void spiwrite(uint8_t c);

void my_setpixel(uint8_t x, uint8_t y, uint8_t color);

//uint8_t buffer[128*64/8];
};

ST7665.cpp

$Id:$

ST7565 LCD library!

This library is free software; you can redistribute it and/or

modify it under the terms of the GNU Lesser General Public

License as published by the Free Software Foundation; either

version 2.1 of the License, or (at your option) any later version.

This library 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

Lesser General Public License for more details.

You should have received a copy of the GNU Lesser General Public

License along with this library; if not, write to the Free Software

Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA

// some of this code was written by <cstone@pobox.com> originally; it is in the public domain.

//#include <Wire.h>

#ifdef __AVR__

#include <avr/pgmspace.h>

#include <util/delay.h>

#endif

#ifndef _delay_ms

#define _delay_ms(t) delay(t)

#endif

#ifndef _BV

#define _BV(bit) (1<<(bit))

#endif

#include <stdlib.h>

#include "ST7565.h"

#define ST7565_STARTBYTES 1

uint8_t is_reversed = 0;

// a handy reference to where the pages are on the screen

//const uint8_t pagemap[] = { 3, 2, 1, 0, 7, 6, 5, 4 };
const uint8_t pagemap[] = { 7, 6, 5, 4, 3, 2, 1, 0 }; /* 2019-01-20 add */

// a 5x7 font table

const extern uint8_t PROGMEM font[];

// the memory buffer for the LCD

uint8_t st7565_buffer[1024] = {

0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,

0x0, 0x0, 0x0, 0x3, 0x7, 0xF, 0x1F, 0x1F, 0x3F, 0x3F, 0x3F, 0x3F, 0x7, 0x0, 0x0, 0x0,

0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,

0x0, 0x0, 0x7F, 0x3F, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,

0x0, 0x0, 0x1F, 0x3F, 0x70, 0x70, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,

0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x6, 0x6, 0x0, 0x0, 0x0, 0x3, 0x3,

0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,

0x1F, 0x1F, 0x1F, 0x1F, 0x1F, 0x1F, 0x1F, 0x1F, 0x1F, 0x1F, 0x1F, 0x1F, 0x1F, 0xF, 0x7, 0x7,

0x7, 0x3F, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFE, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x3E, 0x0, 0x0,

0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0xF, 0x3F,

0x70, 0x60, 0x60, 0x60, 0x60, 0x30, 0x7F, 0x3F, 0x0, 0x0, 0x1F, 0x3F, 0x70, 0x60, 0x60, 0x60,

0x60, 0x39, 0xFF, 0xFF, 0x0, 0x6, 0x1F, 0x39, 0x60, 0x60, 0x60, 0x60, 0x30, 0x3F, 0x7F, 0x0,

0x0, 0x60, 0xFF, 0xFF, 0x60, 0x60, 0x0, 0x7F, 0x7F, 0x70, 0x60, 0x60, 0x40, 0x0, 0x7F, 0x7F,

0x0, 0x0, 0x0, 0x0, 0x7F, 0x7F, 0x0, 0x0, 0x0, 0x7F, 0x7F, 0x0, 0x0, 0x60, 0xFF, 0xFF,

0x60, 0x60, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,

0x80, 0xF8, 0xFC, 0xFE, 0xFE, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xEF, 0xE7, 0xE7, 0xE3,

0xF3, 0xF9, 0xFF, 0xFF, 0xFF, 0xF7, 0x7, 0x1F, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x7F, 0xFF,

0x7F, 0x7F, 0x7F, 0x7F, 0x7F, 0x7F, 0x3F, 0x3F, 0x1F, 0xF, 0x7, 0x3, 0x0, 0x0, 0x0, 0xC0,

0xE0, 0x60, 0x20, 0x20, 0x60, 0xE0, 0xE0, 0xE0, 0x0, 0x0, 0x80, 0xC0, 0xE0, 0x60, 0x20, 0x60,

0x60, 0xE0, 0xE0, 0xE0, 0x0, 0x0, 0x80, 0xC0, 0x60, 0x60, 0x20, 0x60, 0x60, 0xE0, 0xE0, 0x0,

0x0, 0x0, 0xE0, 0xE0, 0x0, 0x0, 0x0, 0xE0, 0xE0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x80, 0xE0,

0x60, 0x60, 0x60, 0x60, 0xE0, 0x80, 0x0, 0x0, 0x0, 0xE0, 0xE0, 0x0, 0x0, 0x0, 0xE0, 0xE0,

0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,

0x0, 0x0, 0x0, 0x3, 0x7, 0x1F, 0x9F, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFD, 0xF1, 0xE3,

0xE3, 0xCF, 0xFF, 0xFF, 0xFF, 0xFF, 0xF0, 0xFC, 0x7F, 0x3F, 0x3F, 0x3F, 0x3F, 0x7F, 0xFF, 0xFF,

0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFE, 0xFC, 0xF0, 0xE0, 0x80, 0x0, 0x0, 0x0, 0xC,

0x1C, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,

0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x7F, 0x7F, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,

0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x7, 0x7, 0x0,

0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x1C, 0xC, 0x0, 0x0, 0x0, 0x0, 0x0,

0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,

0x0, 0x7, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFE, 0xFE, 0xFE, 0xFE, 0xFC, 0xF8,

0xF8, 0xF0, 0xFE, 0xFF, 0xFF, 0xFF, 0x7F, 0x3F, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x1F,

0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0xFF,

0xFF, 0x0, 0x0, 0x0, 0xFF, 0xFF, 0xE0, 0xC0, 0xC0, 0xC0, 0xFF, 0x7F, 0x0, 0x0, 0x1E, 0x7F,

0xE1, 0xC0, 0xC0, 0xC0, 0xC0, 0x61, 0xFF, 0xFF, 0x0, 0x0, 0xFE, 0xFF, 0x1, 0x0, 0x0, 0x0,

0xFF, 0xFF, 0x0, 0x0, 0x21, 0xF9, 0xF8, 0xDC, 0xCC, 0xCF, 0x7, 0x0, 0xC0, 0xFF, 0xFF, 0xC0,

0x80, 0x0, 0xFF, 0xFF, 0xC0, 0xC0, 0x80, 0x0, 0x0, 0xFF, 0xFF, 0x0, 0x0, 0x1F, 0x7F, 0xF9,

0xC8, 0xC8, 0xC8, 0xC8, 0x79, 0x39, 0x0, 0x0, 0x71, 0xF9, 0xD8, 0xCC, 0xCE, 0x47, 0x3, 0x0,

0x0, 0x0, 0x0, 0x0, 0x80, 0x80, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,

0x0, 0x0, 0x0, 0x80, 0xC0, 0xE0, 0xF0, 0xF8, 0xF8, 0xFC, 0xFC, 0xFC, 0xFC, 0xF8, 0xF0, 0xC0,

0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0xC0,

0xC0, 0x0, 0x0, 0x0, 0xC0, 0xC0, 0x0, 0x0, 0x0, 0x0, 0xC0, 0xC0, 0x0, 0x0, 0x0, 0x80,

0xC0, 0xC0, 0xC0, 0xC0, 0xC0, 0x80, 0xC0, 0xC0, 0x0, 0x0, 0x0, 0x80, 0xC0, 0xC0, 0xC0, 0xC0,

0xC0, 0x80, 0x0, 0x0, 0x80, 0xC0, 0xC0, 0xC0, 0xC0, 0xC0, 0x0, 0x0, 0x0, 0xC0, 0xC0, 0x0,

0x0, 0x0, 0xC0, 0x80, 0x0, 0x0, 0x0, 0x0, 0x0, 0xC0, 0xC0, 0x0, 0x0, 0x0, 0x80, 0xC0,

0xC0, 0xC0, 0xC0, 0xC0, 0x80, 0x80, 0x0, 0x0, 0x80, 0xC0, 0xC0, 0xC0, 0xC0, 0x80, 0x0, 0x0,

0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,

0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,};

// reduces how much is refreshed, which speeds it up!

// originally derived from Steve Evans/JCW's mod but cleaned up and

// optimized

#define enablePartialUpdate

#ifdef enablePartialUpdate

static uint8_t xUpdateMin, xUpdateMax, yUpdateMin, yUpdateMax;

#endif

static void updateBoundingBox(uint8_t xmin, uint8_t ymin, uint8_t xmax, uint8_t ymax) {

#ifdef enablePartialUpdate

if (xmin < xUpdateMin) xUpdateMin = xmin;

if (xmax > xUpdateMax) xUpdateMax = xmax;

if (ymin < yUpdateMin) yUpdateMin = ymin;

if (ymax > yUpdateMax) yUpdateMax = ymax;

#endif

}

void ST7565::drawbitmap(uint8_t x, uint8_t y,

const uint8_t *bitmap, uint8_t w, uint8_t h,

uint8_t color) {

for (uint8_t j=0; j<h; j++) {

for (uint8_t i=0; i<w; i++ ) {

if (pgm_read_byte(bitmap + i + (j/8)*w) & _BV(j%8)) {

my_setpixel(x+i, y+j, color);

}

updateBoundingBox(x, y, x+w, y+h);

}

void ST7565::drawstring(uint8_t x, uint8_t line, char *c) {

while (c[0] != 0) {

drawchar(x, line, c[0]);

c++;

x += 6; // 6 pixels wide

if (x + 6 >= LCDWIDTH) {

x = 0; // ran out of this line

line++;

}

if (line >= (LCDHEIGHT/8))

return; // ran out of space :(

}

void ST7565::drawstring_P(uint8_t x, uint8_t line, const char *str) {

while (1) {

char c = pgm_read_byte(str++);

if (! c)

return;

drawchar(x, line, c);

x += 6; // 6 pixels wide

if (x + 6 >= LCDWIDTH) {

x = 0; // ran out of this line

line++;

}

if (line >= (LCDHEIGHT/8))

return; // ran out of space :(

}

void ST7565::drawchar(uint8_t x, uint8_t line, char c) {

for (uint8_t i =0; i<5; i++ ) {

st7565_buffer[x + (line*128) ] = pgm_read_byte(font+(c*5)+i);

x++;

}

updateBoundingBox(x, line*8, x+5, line*8 + 8);

}

// bresenham's algorithm - thx wikpedia

void ST7565::drawline(uint8_t x0, uint8_t y0, uint8_t x1, uint8_t y1,

uint8_t color) {

uint8_t steep = abs(y1 - y0) > abs(x1 - x0);

if (steep) {

swap(x0, y0);

swap(x1, y1);

}

if (x0 > x1) {

swap(x0, x1);

swap(y0, y1);

}

// much faster to put the test here, since we've already sorted the points

updateBoundingBox(x0, y0, x1, y1);

uint8_t dx, dy;

dx = x1 - x0;

dy = abs(y1 - y0);

int8_t err = dx / 2;

int8_t ystep;

if (y0 < y1) {

ystep = 1;

} else {

ystep = -1;}

for (; x0<=x1; x0++) {

if (steep) {

my_setpixel(y0, x0, color);

} else {

my_setpixel(x0, y0, color);

}

err -= dy;

if (err < 0) {

y0 += ystep;

err += dx;

}

// filled rectangle

void ST7565::fillrect(uint8_t x, uint8_t y, uint8_t w, uint8_t h,

uint8_t color) {

// stupidest version - just pixels - but fast with internal buffer!

for (uint8_t i=x; i<x+w; i++) {

for (uint8_t j=y; j<y+h; j++) {

my_setpixel(i, j, color);

}

updateBoundingBox(x, y, x+w, y+h);

}

// draw a rectangle

void ST7565::drawrect(uint8_t x, uint8_t y, uint8_t w, uint8_t h,

uint8_t color) {

// stupidest version - just pixels - but fast with internal buffer!

for (uint8_t i=x; i<x+w; i++) {

my_setpixel(i, y, color);

my_setpixel(i, y+h-1, color);

}

for (uint8_t i=y; i<y+h; i++) {

my_setpixel(x, i, color);

my_setpixel(x+w-1, i, color);

}

updateBoundingBox(x, y, x+w, y+h);

}

// draw a circle outline

void ST7565::drawcircle(uint8_t x0, uint8_t y0, uint8_t r,

uint8_t color) {

updateBoundingBox(x0-r, y0-r, x0+r, y0+r);

int8_t f = 1 - r;

int8_t ddF_x = 1;

int8_t ddF_y = -2 * r;

int8_t x = 0;

int8_t y = r;

my_setpixel(x0, y0+r, color);

my_setpixel(x0, y0-r, color);

my_setpixel(x0+r, y0, color);

my_setpixel(x0-r, y0, color);

while (x<y) {

if (f >= 0) {

y--;

ddF_y += 2;

f += ddF_y;

}

x++;

ddF_x += 2;

f += ddF_x;

my_setpixel(x0 + x, y0 + y, color);

my_setpixel(x0 - x, y0 + y, color);

my_setpixel(x0 + x, y0 - y, color);

my_setpixel(x0 - x, y0 - y, color);

my_setpixel(x0 + y, y0 + x, color);

my_setpixel(x0 - y, y0 + x, color);

my_setpixel(x0 + y, y0 - x, color);

my_setpixel(x0 - y, y0 - x, color);

}

void ST7565::fillcircle(uint8_t x0, uint8_t y0, uint8_t r,

uint8_t color) {

updateBoundingBox(x0-r, y0-r, x0+r, y0+r);

int8_t f = 1 - r;

int8_t ddF_x = 1;

int8_t ddF_y = -2 * r;

int8_t x = 0;

int8_t y = r;

for (uint8_t i=y0-r; i<=y0+r; i++) {

my_setpixel(x0, i, color);

}

while (x<y) {

if (f >= 0) {

y--;

ddF_y += 2;

f += ddF_y;

}

x++;

ddF_x += 2;

f += ddF_x;

for (uint8_t i=y0-y; i<=y0+y; i++) {

my_setpixel(x0+x, i, color);

my_setpixel(x0-x, i, color);

}

for (uint8_t i=y0-x; i<=y0+x; i++) {

my_setpixel(x0+y, i, color);

my_setpixel(x0-y, i, color);

}

void ST7565::my_setpixel(uint8_t x, uint8_t y, uint8_t color) {

if ((x >= LCDWIDTH) || (y >= LCDHEIGHT))

return;

// x is which column

if (color)

st7565_buffer[x+ (y/8)*128] |= _BV(7-(y%8));

else

st7565_buffer[x+ (y/8)*128] &= ~_BV(7-(y%8));

}

// the most basic function, set a single pixel

void ST7565::setpixel(uint8_t x, uint8_t y, uint8_t color) {

if ((x >= LCDWIDTH) || (y >= LCDHEIGHT))

return;

// x is which column

if (color)

st7565_buffer[x+ (y/8)*128] |= _BV(7-(y%8));

else

st7565_buffer[x+ (y/8)*128] &= ~_BV(7-(y%8));

updateBoundingBox(x,y,x,y);

}

// the most basic function, get a single pixel

uint8_t ST7565::getpixel(uint8_t x, uint8_t y) {

if ((x >= LCDWIDTH) || (y >= LCDHEIGHT))

return 0;

return (st7565_buffer[x+ (y/8)*128] >> (7-(y%8))) & 0x1;

}

void ST7565::begin(uint8_t contrast) {

st7565_init();

st7565_command(CMD_DISPLAY_ON);

st7565_command(CMD_SET_ALLPTS_NORMAL);

st7565_set_brightness(contrast);

}

void ST7565::st7565_init(void) {

// set pin directions

pinMode(sid, OUTPUT);

pinMode(sclk, OUTPUT);

pinMode(a0, OUTPUT);

pinMode(rst, OUTPUT);

pinMode(cs, OUTPUT);

// toggle RST low to reset; CS low so it'll listen to us

if (cs > 0)

digitalWrite(cs, LOW);

digitalWrite(rst, LOW);

_delay_ms(500);

digitalWrite(rst, HIGH);

// LCD bias select

st7565_command(CMD_SET_BIAS_7);

// ADC select

st7565_command(CMD_SET_ADC_NORMAL);

// SHL select

st7565_command(CMD_SET_COM_NORMAL);

// Initial display line

st7565_command(CMD_SET_DISP_START_LINE);

// turn on voltage converter (VC=1, VR=0, VF=0)

st7565_command(CMD_SET_POWER_CONTROL | 0x4);

// wait for 50% rising

_delay_ms(50);

// turn on voltage regulator (VC=1, VR=1, VF=0)

st7565_command(CMD_SET_POWER_CONTROL | 0x6);

// wait >=50ms

_delay_ms(50);

// turn on voltage follower (VC=1, VR=1, VF=1)

st7565_command(CMD_SET_POWER_CONTROL | 0x7);

// wait

_delay_ms(10);

// set lcd operating voltage (regulator resistor, ref voltage resistor)

st7565_command(CMD_SET_RESISTOR_RATIO | 0x6);

// initial display line

// set page address

// set column address

// write display data

// set up a bounding box for screen updates

updateBoundingBox(0, 0, LCDWIDTH-1, LCDHEIGHT-1);

}

inline void ST7565::spiwrite(uint8_t c) {

#if not defined (_VARIANT_ARDUINO_DUE_X_) && not defined (_VARIANT_ARDUINO_ZERO_)

shiftOut(sid, sclk, MSBFIRST, c);

#else

int8_t i;

for (i=7; i>=0; i--) {

digitalWrite(sclk, LOW);

delayMicroseconds(5); //need to slow down the data rate for Due and Zero

if (c & _BV(i))

digitalWrite(sid, HIGH);

else

digitalWrite(sid, LOW);

// delayMicroseconds(5); //need to slow down the data rate for Due and Zero

digitalWrite(sclk, HIGH);

}

#endif

int8_t i;

for (i=7; i>=0; i--) {

SCLK_PORT &= ~_BV(SCLK);

if (c & _BV(i))

SID_PORT |= _BV(SID);

else

SID_PORT &= ~_BV(SID);

SCLK_PORT |= _BV(SCLK);

}

// loop unwrapped! too fast doesnt work :(

SCLK_PORT &= ~_BV(SCLK);

if (c & _BV(7))

SID_PORT |= _BV(SID);

else

SID_PORT &= ~_BV(SID);

SCLK_PORT |= _BV(SCLK);

SCLK_PORT &= ~_BV(SCLK);

if (c & _BV(6))

SID_PORT |= _BV(SID);

else

SID_PORT &= ~_BV(SID);

SCLK_PORT |= _BV(SCLK);

SCLK_PORT &= ~_BV(SCLK);

if (c & _BV(5))

SID_PORT |= _BV(SID);

else

SID_PORT &= ~_BV(SID);

SCLK_PORT |= _BV(SCLK);

SCLK_PORT &= ~_BV(SCLK);

if (c & _BV(4))

SID_PORT |= _BV(SID);

else

SID_PORT &= ~_BV(SID);

SCLK_PORT |= _BV(SCLK);

SCLK_PORT &= ~_BV(SCLK);

if (c & _BV(3))

SID_PORT |= _BV(SID);

else

SID_PORT &= ~_BV(SID);

SCLK_PORT |= _BV(SCLK);

SCLK_PORT &= ~_BV(SCLK);

if (c & _BV(2))

SID_PORT |= _BV(SID);

else

SID_PORT &= ~_BV(SID);

SCLK_PORT |= _BV(SCLK);

SCLK_PORT &= ~_BV(SCLK);

if (c & _BV(1))

SID_PORT |= _BV(SID);

else

SID_PORT &= ~_BV(SID);

SCLK_PORT |= _BV(SCLK);

SCLK_PORT &= ~_BV(SCLK);

if (c & _BV(0))

SID_PORT |= _BV(SID);

else

SID_PORT &= ~_BV(SID);

SCLK_PORT |= _BV(SCLK);

}

void ST7565::st7565_command(uint8_t c) {

digitalWrite(a0, LOW);

spiwrite(c);

}

void ST7565::st7565_data(uint8_t c) {

digitalWrite(a0, HIGH);

spiwrite(c);

}

void ST7565::st7565_set_brightness(uint8_t val) {

st7565_command(CMD_SET_VOLUME_FIRST);

st7565_command(CMD_SET_VOLUME_SECOND | (val & 0x3f));

}

/*2018-12-19 add range 0x20~0x27*/

void ST7565::st7565_set_pre_contrast(uint8_t pval) {

st7565_command(CMD_SET_PRE_CONTRAST);

}

void ST7565::display(void) {

uint8_t col, maxcol, p;

Serial.print("Refresh ("); Serial.print(xUpdateMin, DEC);

Serial.print(", "); Serial.print(xUpdateMax, DEC);

Serial.print(","); Serial.print(yUpdateMin, DEC);

Serial.print(", "); Serial.print(yUpdateMax, DEC); Serial.println(")");

for(p = 0; p < 8; p++) {

putstring("new page! ");

uart_putw_dec(p);

putstring_nl("");

#ifdef enablePartialUpdate

// check if this page is part of update

if ( yUpdateMin >= ((p+1)*8) ) {

continue; // nope, skip it!

}

if (yUpdateMax < p*8) {

break;

}

#endif

st7565_command(CMD_SET_PAGE | pagemap[p]);

#ifdef enablePartialUpdate

col = xUpdateMin;

maxcol = xUpdateMax;

#else

// start at the beginning of the row

col = 0;

maxcol = LCDWIDTH-1;

#endif

st7565_command(CMD_SET_COLUMN_LOWER | ((col+ST7565_STARTBYTES) & 0xf));

st7565_command(CMD_SET_COLUMN_UPPER | (((col+ST7565_STARTBYTES) >> 4) & 0x0F));

st7565_command(CMD_RMW);

for(; col <= maxcol; col++) {

//uart_putw_dec(col);

//uart_putchar(' ');

st7565_data(st7565_buffer[(128*p)+col]);

}

#ifdef enablePartialUpdate

xUpdateMin = LCDWIDTH - 1;

xUpdateMax = 0;

yUpdateMin = LCDHEIGHT-1;

yUpdateMax = 0;

#endif

}

// clear everything

void ST7565::clear(void) {

memset(st7565_buffer, 0, 1024);

updateBoundingBox(0, 0, LCDWIDTH-1, LCDHEIGHT-1);

}

// this doesnt touch the buffer, just clears the display RAM - might be handy

void ST7565::clear_display(void) {

uint8_t p, c;

for(p = 0; p < 8; p++) {

putstring("new page! ");

uart_putw_dec(p);

putstring_nl("");

st7565_command(CMD_SET_PAGE | p);

for(c = 0; c < 129; c++) {

//uart_putw_dec(c);

//uart_putchar(' ');

st7565_command(CMD_SET_COLUMN_LOWER | (c & 0xf));

st7565_command(CMD_SET_COLUMN_UPPER | ((c >> 4) & 0xf));

st7565_data(0x0);

}

//============================================================
// othet test

//#include "./ST7565-LCD-master/ST7565.h"
#include "ST7565.h"
int ledPin = 13; // LED connected to digital pin 13

// the LCD backlight is connected up to a pin so you can turn it on & off

// pin 9 - Serial data out (SID)
// pin 8 - Serial clock out (SCLK)
// pin 7 - Data/Command select (RS or A0)
// pin 6 - LCD reset (RST)
// pin 5 - LCD chip select (CS)
//ST7565 glcd(9, 8, 7, 6, 5);

//ST7565 glcd(SDA, SCK, RS/DC, RST, CS);
/*
ST7565 glcd(5, 6, 4, 3, 2);
#define Power_3v3 7
#define Power_Gnd 8
#define BACKLIGHT_LED 9
*/
ST7565 glcd(8, 9, 7, 6, 5);
#define LOGO16_GLCD_HEIGHT 16
#define LOGO16_GLCD_WIDTH 16

// a bitmap of a 16x16 fruit icon
const static unsigned char __attribute__ ((progmem)) logo16_glcd_bmp[] = {
0x30, 0xf0, 0xf0, 0xf0, 0xf0, 0x30, 0xf8, 0xbe, 0x9f, 0xff, 0xf8, 0xc0, 0xc0, 0xc0, 0x80, 0x00,
0x20, 0x3c, 0x3f, 0x3f, 0x1f, 0x19, 0x1f, 0x7b, 0xfb, 0xfe, 0xfe, 0x07, 0x07, 0x07, 0x03, 0x00,
};

// The setup() method runs once, when the sketch starts
void setup() {
Serial.begin(115200);

// initialize and set the contrast to 0x18
glcd.begin(0x08);
delay(200);
glcd.st7565_set_pre_contrast(0x20);/*0x20~0x27*/ // for ST7567 only
// glcd.st7565_set_brightness(0x00);
}

void loop() {
glcd.drawstring(0, 0, "test");
glcd.drawchar( 0, 1, 'a'); //a
glcd.drawchar( 6, 1, 'b'); //b
glcd.drawchar(12, 1, 'c'); //c

// glcd.drawstring(0, 6, 'a1');

// ---------- show A0 ------------------
String string1 = String(analogRead(0), DEC); // A0 to string串流 (使用 int 變數和指定的進位表示法) (十進位)
char charString1[4]; // 宣告字元陣列 4個字元
string1.getBytes(charString1, 4); // 將string1串流轉換成 charString字元陣列,4個字元
//--------- 結構指標 ----------------- // char *charString = "qazwsx"; //for test
for (int i = 0; i < 4; i++) {
glcd.drawchar(i * 6, 2, charString1[i]);
}

// ---------- show Sec ------------------
String string2 = String(millis() / 1000 ); // A0 to string串流 (使用 int 變數和指定的進位表示法) (十進位)
char charString2[4]; // 宣告字元陣列 4個字元
string2.getBytes(charString2, 4); // 將string2串流轉換成 charString2字元陣列,4個字元
for (int i = 0; i < 4; i++) {
glcd.drawchar(i * 6, 3, charString2[i]);
}

glcdVal(0, 5, 123, 4);
glcdVal(6, 5, analogRead(7), 4);
glcdVal_F(11, 5, millis() / 1000, 4);
glcdVal_F(0, 6, millis() / 1000, 4);
glcdVal_F(0, 7, millis() / 1000 * -1, 4);

glcd.display();
delay(100);
glcd.clear();
}

void glcdVal_F(byte gx, byte gy, int val, byte cun) { // 字元起始x,y,要顯示的值,顯示的位數
String stringOne = String(val); // val to string串流
char charString[cun]; // 宣告字元陣列 4個字元
stringOne.getBytes(charString, cun); // 將stringOne串流轉換成 charString字元陣列,4個字元
//--------- 結構指標 -----------------
int i; byte gx1; // char *charString = "qazwsx"; //for test
for ( i = 0; i < cun; i++) {
if (val >= 1000 && val <= 9999 )gx1 = cun - 4 ;
if (val >= 100 && val <= 999 )gx1 = cun - 3 ;
if (val >= 10 && val <= 99 )gx1 = cun - 2 ;
if (val >= 0 && val <= 9 )gx1 = cun - 1 ;
if (val >= -9 && val <= -1 )gx1 = cun - 2 ;
if (val >= -99 && val <= -10 )gx1 = cun - 3 ;
if (val >= -999 && val <= -100 )gx1 = cun - 4 ;
if (val >= -9999 && val <= -1000 )gx1 = cun - 5 ;
glcd.drawchar(((i + gx + gx1) * 6), gy, charString[i]);
}
}

void glcdVal(byte gx, byte gy, int val, byte cun) { // 字元起始x,y,要顯示的值,顯示的位數
String stringOne = String(val); // val to string串流
char charString[cun]; // 宣告字元陣列 4個字元
stringOne.getBytes(charString, cun); // 將stringOne串流轉換成 charString字元陣列,4個字元
//--------- 結構指標 -----------------
int i; // char *charString = "qazwsx"; //for test
for ( i = 0; i < cun; i++) {
glcd.drawchar((i + gx) * 6, gy, charString[i]);
}
}

2018年11月28日星期三

GY-271 磁力計上的晶片差異 HMC5883 & QMC 5883 Library 並不相容 & 電子羅盤轉方向

先講結論 : ESP32 or ESP8266 請使用HMC5883L (L883) 的晶片，可以省掉不少麻煩

groups.io 的討論

https://github.com/DFRobot/DFRobot_QMC5883
可自動判別HMC or QMC
QMC數據會飄
HMC正常
ESP32 可編譯上傳，但載入執行失敗

https://github.com/dthain/QMC5883L
這個QMC 專用，簡單穩定，在328上運行良好，佔用空間小 (ESP8266 ,ESP32 會編譯錯誤)

由於銲在PCB版的晶片不是所需方向時，調整順時鐘轉90度

/*
2020-06-18 測試轉方向 OK
*/

#include <QMC5883L.h>
QMC5883L compass;

void setup()
{
compass.init();
compass.setSamplingRate(100); // 10,50,100,200
//compass.setOversampling(64); // 64,128,256,512 設置採樣率
compass.setRange(8); // 2 or 8 量程
compass.resetCalibration();

Serial.begin(115200);
// Serial.println("QMC5883L Compass Demo");
// Serial.println("Turn compass in all directions to calibrate....");
}

void loop()
{

static int16_t x, y, z, t;
compass.readRaw(&x, &y, &z, &t);
Serial.print("x: "); Serial.print(x);
Serial.print(" y: "); Serial.print(y);
// Serial.print(" z: "); Serial.print(z);

// -------------- 順時鐘轉90度 (銲在PCB版的晶片不是所需方向時)------------------
// 就是把xy座標順時中轉90度
static int16_t xx , yy;
xx = x; yy = y;
if (xx >= 0 )y = xx * -1; // x -> -y
if (xx < -1)y = xx * -1; // -x -> y
if (yy >= 0 )x = yy; // y -> x
if (yy < -1)x = yy; // -y -> -x

Serial.print(" x: "); Serial.print(x);
Serial.print(" y: "); Serial.print(y);
// Serial.print(" z: "); Serial.print(z);
// -------------- End ---------------------------------------------------

/* Update the observed boundaries of the measurements */
static int16_t xhigh, xlow;
static int16_t yhigh, ylow;
if (x < xlow) xlow = x; if (x > xhigh) xhigh = x;
if (y < ylow) ylow = y; if (y > yhigh) yhigh = y;

/* Bail out if not enough data is available. */
if ( xlow == xhigh || ylow == yhigh ) return 0;

/* Recenter the measurement by subtracting the average */
x -= (xhigh + xlow) / 2;
y -= (yhigh + ylow) / 2;

/* Rescale the measurement to the range observed. */
float fx = (float)x / (xhigh - xlow); Serial.print(F(" fx: ")); Serial.print(fx);
float fy = (float)y / (yhigh - ylow); Serial.print(F(" fy: ")); Serial.print(fy);

static int heading;
heading = (atan2(fy, fx) * 180.0) / PI;

if (heading <= 0) heading += 360;

Serial.print(F(" Heading: ")); Serial.print(heading);
Serial.println(); delay(1000);
}

所有QMC都在芯片上有“DA”，而HMC卻沒有。
左邊是HMC，右邊是QMC。

ESP8266 ,ESP32 QMC5883L 用底下這個，測試過讀取數據OK ，但該作者提供的library 測試轉換磁方位不成功
https://github.com/mechasolution/Mecha_QMC5883L/blob/master/test.ino

#include <Wire.h> //I2C Arduino Library
#define addr 0x0D //I2C Address for The QMC5883

void setup() {
Serial.begin(115200);
Wire.begin();
Wire.beginTransmission(addr); //start talking
Wire.write(0x0B); // Tell the QMC5883 to Continuously Measure
Wire.write(0x01); // Set the Register
Wire.endTransmission();
Wire.beginTransmission(addr); //start talking
Wire.write(0x09); // Tell the QMC5883 to Continuously Measure
Wire.write(0x1D); // Set the Register
Wire.endTransmission();
}

void loop() {

int x, y, z, t; //triple axis data
Wire.beginTransmission(addr); //Tell the QMC what regist to begin writing data into
Wire.write(0x00); //start with register 3.
Wire.endTransmission();
Wire.requestFrom(addr, 6); //Read the data.. 2 bytes for each axis.. 6 total bytes
if (6 <= Wire.available()) {
x = Wire.read(); //MSB x
x |= Wire.read() << 8; //LSB x
y = Wire.read(); //MSB y
y |= Wire.read() << 8; //LSB y
z = Wire.read(); //MSB z
z |= Wire.read() << 8; //LSB z
}

// Show Values
Serial.print("X Value: ");
Serial.println(x);
Serial.print("Y Value: ");
Serial.println(y);
Serial.print("Z Value: ");
Serial.println(z);
Serial.println();

delay(500);
}

校正
http://wukcsoft.blogspot.com/2014/06/read-gy-80.html

2018年11月9日星期五

Arduino 計算兩個衛星定位點之間的距離和相對角度

/*
* 2018-11-09 Test OK
*
* 計算兩個衛星定位點之間的距離和相對角度
* 這個函式是從TinyGPSPlus-0.92 Libraty 擷取出來的
*
*
*/

void setup() {
Serial.begin(115200, SERIAL_8N1);

//distanceBetween(24.1365, 121.6588,24.1403, 121.64723);

Serial.println(distanceBetween(24.1365, 121.6588,24.1403, 121.64723));
Serial.println(courseTo(24.1365, 121.6588,24.1403, 121.64723));

}

void loop() {

}

/* static */
double distanceBetween(double lat1, double long1, double lat2, double long2){
/*
// returns distance in meters between two positions, both specified
// as signed decimal-degrees latitude and longitude. Uses great-circle
// distance computation for hypothetical sphere of radius 6372795 meters.
// Because Earth is no exact sphere, rounding errors may be up to 0.5%.
// Courtesy of Maarten Lamers
//返回指定的兩個位置之間的距離（以米為單位）
//作為帶符號的十進制度緯度和經度。使用大圓
//半徑為6372795米的假想球體的距離計算。
//因為地球不是精確的球體，所以舍入誤差可能高達0.5％。
//由Maarten Lamers提供
*/
double delta = radians(long1 - long2);
double sdlong = sin(delta);
double cdlong = cos(delta);
lat1 = radians(lat1);
lat2 = radians(lat2);
double slat1 = sin(lat1);
double clat1 = cos(lat1);
double slat2 = sin(lat2);
double clat2 = cos(lat2);
delta = (clat1 * slat2) - (slat1 * clat2 * cdlong);
delta = sq(delta);
delta += sq(clat2 * sdlong);
delta = sqrt(delta);
double denom = (slat1 * slat2) + (clat1 * clat2 * cdlong);
delta = atan2(delta, denom);
return delta * 6372795;
}

double courseTo(double lat1, double long1, double lat2, double long2){
/*
// returns course in degrees (North=0, West=270) from position 1 to position 2,
// both specified as signed decimal-degrees latitude and longitude.
// Because Earth is no exact sphere, calculated course may be off by a tiny fraction.
// Courtesy of Maarten Lamers
//從位置1到位置2以度數（北= 0，西= 270）返迴路線，
//均指定為帶符號的十進制度緯度和經度。
//因為地球不是精確的球體，計算過程可能會受到很小的影響。
//由Maarten Lamers提供
*/
double dlon = radians(long2 - long1);
lat1 = radians(lat1);
lat2 = radians(lat2);
double a1 = sin(dlon) * cos(lat2);
double a2 = sin(lat1) * cos(lat2) * cos(dlon);
a2 = cos(lat1) * sin(lat2) - a2;
a2 = atan2(a1, a2);
if (a2 < 0.0)
{
a2 += TWO_PI;
}
return degrees(a2);
}