Subversion Repositories Projects

Compare Revisions

Ignore whitespace Rev 1701 → Rev 1702

/C-OSD/arducam-osd/ArduCAM_OSD/ArduCAM_OSD.ino
0,0 → 1,216
/*
 
Copyright (c) 2011. All rights reserved.
An Open Source Arduino based OSD and Camera Control project.
Program : ArduCAM-OSD (Supports the variant: minimOSD)
Version : V1.9, 14 February 2012
Author(s): Sandro Benigno
Coauthor(s):
Jani Hirvinen (All the EEPROM routines)
Michael Oborne (OSD Configutator)
Mike Smith (BetterStream and Fast Serial libraries)
Special Contribuitor:
Andrew Tridgell by all the support on MAVLink
Doug Weibel by his great orientation since the start of this project
Contributors: James Goppert, Max Levine
and all other members of DIY Drones Dev team
Thanks to: Chris Anderson, Jordi Munoz
 
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 <http://www.gnu.org/licenses/>
*/
/* ************************************************************ */
/* **************** MAIN PROGRAM - MODULES ******************** */
/* ************************************************************ */
 
#undef PROGMEM
#define PROGMEM __attribute__(( section(".progmem.data") ))
 
#undef PSTR
#define PSTR(s) (__extension__({static prog_char __c[] PROGMEM = (s); &__c[0];}))
 
#define MAVLINK10
 
/* **********************************************/
/* ***************** INCLUDES *******************/
 
//#define membug
//#define FORCEINIT // You should never use this unless you know what you are doing
 
 
// AVR Includes
#include <FastSerial.h>
#include <AP_Common.h>
#include <AP_Math.h>
#include <math.h>
#include <inttypes.h>
#include <avr/pgmspace.h>
// Get the common arduino functions
#if defined(ARDUINO) && ARDUINO >= 100
#include "Arduino.h"
#else
#include "wiring.h"
#endif
#include <EEPROM.h>
#include <SimpleTimer.h>
#include <GCS_MAVLink.h>
 
#ifdef membug
#include <MemoryFree.h>
#endif
 
// Configurations
#include "OSD_Config.h"
#include "ArduCam_Max7456.h"
#include "OSD_Vars.h"
#include "OSD_Func.h"
 
/* *************************************************/
/* ***************** DEFINITIONS *******************/
 
//OSD Hardware
//#define ArduCAM328
#define MinimOSD
 
#define TELEMETRY_SPEED 57600 // How fast our MAVLink telemetry is coming to Serial port
#define BOOTTIME 2000 // Time in milliseconds that we show boot loading bar and wait user input
 
// Objects and Serial definitions
FastSerialPort0(Serial);
OSD osd; //OSD object
 
SimpleTimer mavlinkTimer;
 
 
/* **********************************************/
/* ***************** SETUP() *******************/
 
void setup()
{
#ifdef ArduCAM328
pinMode(10, OUTPUT); // USB ArduCam Only
#endif
pinMode(MAX7456_SELECT, OUTPUT); // OSD CS
 
Serial.begin(TELEMETRY_SPEED);
// setup mavlink port
mavlink_comm_0_port = &Serial;
#ifdef membug
Serial.println(freeMem());
#endif
 
// Prepare OSD for displaying
unplugSlaves();
osd.init();
// Start
startPanels();
delay(500);
 
// OSD debug for development (Shown at start)
#ifdef membug
osd.setPanel(1,1);
osd.openPanel();
osd.printf("%i",freeMem());
osd.closePanel();
#endif
 
// Just to easy up development things
#ifdef FORCEINIT
InitializeOSD();
#endif
 
 
// Check EEPROM to see if we have initialized it already or not
// also checks if we have new version that needs EEPROM reset
if(readEEPROM(CHK1) + readEEPROM(CHK2) != VER) {
osd.setPanel(6,9);
osd.openPanel();
osd.printf_P(PSTR("Missing/Old Config"));
osd.closePanel();
InitializeOSD();
}
// Get correct panel settings from EEPROM
readSettings();
// Show bootloader bar
loadBar();
 
// Startup MAVLink timers
mavlinkTimer.Set(&OnMavlinkTimer, 120);
 
// House cleaning, clear display and enable timers
osd.clear();
mavlinkTimer.Enable();
} // END of setup();
 
 
 
/* ***********************************************/
/* ***************** MAIN LOOP *******************/
 
// Mother of all happenings, The loop()
// As simple as possible.
void loop()
{
 
if(enable_mav_request == 1){//Request rate control
osd.clear();
osd.setPanel(3,10);
osd.openPanel();
osd.printf_P(PSTR("Requesting DataStreams..."));
osd.closePanel();
for(int n = 0; n < 3; n++){
request_mavlink_rates();//Three times to certify it will be readed
delay(50);
}
enable_mav_request = 0;
delay(2000);
osd.clear();
waitingMAVBeats = 0;
lastMAVBeat = millis();//Preventing error from delay sensing
}
read_mavlink();
mavlinkTimer.Run();
}
 
/* *********************************************** */
/* ******** functions used in main loop() ******** */
void OnMavlinkTimer()
{
setHeadingPatern(); // generate the heading patern
 
osd_battery_pic_A = setBatteryPic(osd_battery_remaining_A); // battery A remmaning picture
//osd_battery_pic_B = setBatteryPic(osd_battery_remaining_B); // battery B remmaning picture
setHomeVars(osd); // calculate and set Distance from home and Direction to home
 
writePanels(); // writing enabled panels (check OSD_Panels Tab)
}
 
 
void unplugSlaves(){
//Unplug list of SPI
#ifdef ArduCAM328
digitalWrite(10, HIGH); // unplug USB HOST: ArduCam Only
#endif
digitalWrite(MAX7456_SELECT, HIGH); // unplug OSD
}
/C-OSD/arducam-osd/ArduCAM_OSD/ArduCam_Max7456.cpp
0,0 → 1,295
 
#include <FastSerial.h>
 
#include "ArduCam_Max7456.h"
// Get the common arduino functions
#if defined(ARDUINO) && ARDUINO >= 100
#include "Arduino.h"
#else
#include "wiring.h"
#endif
#include "Spi.h"
 
volatile int x;
volatile int font_count;
volatile byte character_bitmap[0x40];
 
OSD::OSD()
{
}
 
//------------------ init ---------------------------------------------------
 
void OSD::init()
{
pinMode(MAX7456_SELECT,OUTPUT);
pinMode(MAX7456_VSYNC, INPUT);
digitalWrite(MAX7456_VSYNC,HIGH); //enabling pull-up resistor
 
detectMode();
 
digitalWrite(MAX7456_SELECT,LOW);
//read black level register
Spi.transfer(MAX7456_OSDBL_reg_read);//black level read register
byte osdbl_r = Spi.transfer(0xff);
Spi.transfer(MAX7456_VM0_reg);
Spi.transfer(MAX7456_RESET | video_mode);
delay(50);
//set black level
byte osdbl_w = (osdbl_r & 0xef); //Set bit 4 to zero 11101111
Spi.transfer(MAX7456_OSDBL_reg); //black level write register
Spi.transfer(osdbl_w);
 
// set all rows to same charactor white level, 90%
for (x = 0; x < MAX7456_screen_rows; x++)
{
Spi.transfer(x + 0x10);
Spi.transfer(MAX7456_WHITE_level_120);
}
// define sync (auto,int,ext) and
// making sure the Max7456 is enabled
control(1);
}
 
//------------------ Detect Mode (PAL/NTSC) ---------------------------------
 
void OSD::detectMode()
{
digitalWrite(MAX7456_SELECT,LOW);
//read STAT and auto detect Mode PAL/NTSC
Spi.transfer(MAX7456_STAT_reg_read);//status register
byte osdstat_r = Spi.transfer(0xff);
 
if ((B00000001 & osdstat_r) == 1){
setMode(1);
}
else if((B00000010 & osdstat_r) == 1){
setMode(0);
}
#ifdef MinimOSD
else if (digitalRead(3) == 1){
setMode(1);
}
#endif
else setMode(0);
digitalWrite(MAX7456_SELECT,LOW);
}
 
//------------------ Set Mode (PAL/NTSC) ------------------------------------
 
void OSD::setMode(int themode)
{
switch(themode){
case 0:
video_mode = MAX7456_MODE_MASK_NTCS;
video_center = MAX7456_CENTER_NTSC;
break;
case 1:
video_mode = MAX7456_MODE_MASK_PAL;
video_center = MAX7456_CENTER_PAL;
break;
}
}
 
//------------------ Get Mode (PAL 0/NTSC 1) --------------------------------
 
int OSD::getMode()
{
switch(video_mode){
case MAX7456_MODE_MASK_NTCS:
return 0;
break;
case MAX7456_MODE_MASK_PAL:
return 1;
break;
}
return 0;
}
 
//------------------ Get Center (PAL/NTSC) ----------------------------------
 
int OSD::getCenter()
{
return video_center; //first line for center panel
}
 
//------------------ plug ---------------------------------------------------
 
void OSD::plug()
{
digitalWrite(MAX7456_SELECT,LOW);
}
 
//------------------ clear ---------------------------------------------------
 
void OSD::clear()
{
// clear the screen
digitalWrite(MAX7456_SELECT,LOW);
Spi.transfer(MAX7456_DMM_reg);
Spi.transfer(MAX7456_CLEAR_display);
digitalWrite(MAX7456_SELECT,HIGH);
}
 
//------------------ set panel -----------------------------------------------
 
void
OSD::setPanel(uint8_t st_col, uint8_t st_row){
start_col = st_col;
start_row = st_row;
col = st_col;
row = st_row;
}
 
//------------------ open panel ----------------------------------------------
 
void
OSD::openPanel(void){
unsigned int linepos;
byte settings, char_address_hi, char_address_lo;
//find [start address] position
linepos = row*30+col;
// divide 16 bits into hi & lo byte
char_address_hi = linepos >> 8;
char_address_lo = linepos;
 
//Auto increment turn writing fast (less SPI commands).
//No need to set next char address. Just send them
settings = MAX7456_INCREMENT_auto; //To Enable DMM Auto Increment
digitalWrite(MAX7456_SELECT,LOW);
Spi.transfer(MAX7456_DMM_reg); //dmm
Spi.transfer(settings);
 
Spi.transfer(MAX7456_DMAH_reg); // set start address high
Spi.transfer(char_address_hi);
 
Spi.transfer(MAX7456_DMAL_reg); // set start address low
Spi.transfer(char_address_lo);
//Serial.printf("setPos -> %d %d\n", col, row);
}
 
//------------------ close panel ---------------------------------------------
 
void
OSD::closePanel(void){
Spi.transfer(MAX7456_DMDI_reg);
Spi.transfer(MAX7456_END_string); //This is needed "trick" to finish auto increment
digitalWrite(MAX7456_SELECT,HIGH);
//Serial.println("close");
row++; //only after finish the auto increment the new row will really act as desired
}
 
//------------------ write single char ---------------------------------------------
 
void
OSD::openSingle(uint8_t x, uint8_t y){
unsigned int linepos;
byte char_address_hi, char_address_lo;
//find [start address] position
linepos = y*30+x;
// divide 16 bits into hi & lo byte
char_address_hi = linepos >> 8;
char_address_lo = linepos;
digitalWrite(MAX7456_SELECT,LOW);
Spi.transfer(MAX7456_DMAH_reg); // set start address high
Spi.transfer(char_address_hi);
 
Spi.transfer(MAX7456_DMAL_reg); // set start address low
Spi.transfer(char_address_lo);
//Serial.printf("setPos -> %d %d\n", col, row);
}
 
//------------------ write ---------------------------------------------------
 
size_t
OSD::write(uint8_t c){
if(c == '|'){
closePanel(); //It does all needed to finish auto increment and change current row
openPanel(); //It does all needed to re-enable auto increment
}
else{
Spi.transfer(MAX7456_DMDI_reg);
Spi.transfer(c);
}
return 1;
}
 
//---------------------------------
 
void
OSD::control(uint8_t ctrl){
digitalWrite(MAX7456_SELECT,LOW);
Spi.transfer(MAX7456_VM0_reg);
switch(ctrl){
case 0:
Spi.transfer(MAX7456_DISABLE_display | video_mode);
break;
case 1:
//Spi.transfer((MAX7456_ENABLE_display_vert | video_mode) | MAX7456_SYNC_internal);
//Spi.transfer((MAX7456_ENABLE_display_vert | video_mode) | MAX7456_SYNC_external);
Spi.transfer((MAX7456_ENABLE_display_vert | video_mode) | MAX7456_SYNC_autosync);
break;
}
digitalWrite(MAX7456_SELECT,HIGH);
}
 
void
OSD::write_NVM(int font_count, uint8_t *character_bitmap)
{
byte x;
byte char_address_hi, char_address_lo;
byte screen_char;
 
char_address_hi = font_count;
char_address_lo = 0;
//Serial.println("write_new_screen");
 
// disable display
digitalWrite(MAX7456_SELECT,LOW);
Spi.transfer(MAX7456_VM0_reg);
Spi.transfer(MAX7456_DISABLE_display);
 
Spi.transfer(MAX7456_CMAH_reg); // set start address high
Spi.transfer(char_address_hi);
 
for(x = 0; x < NVM_ram_size; x++) // write out 54 (out of 64) bytes of character to shadow ram
{
screen_char = character_bitmap[x];
Spi.transfer(MAX7456_CMAL_reg); // set start address low
Spi.transfer(x);
Spi.transfer(MAX7456_CMDI_reg);
Spi.transfer(screen_char);
}
 
// transfer a 54 bytes from shadow ram to NVM
Spi.transfer(MAX7456_CMM_reg);
Spi.transfer(WRITE_nvr);
// wait until bit 5 in the status register returns to 0 (12ms)
while ((Spi.transfer(MAX7456_STAT_reg_read) & STATUS_reg_nvr_busy) != 0x00);
 
Spi.transfer(MAX7456_VM0_reg); // turn on screen next vertical
Spi.transfer(MAX7456_ENABLE_display_vert);
digitalWrite(MAX7456_SELECT,HIGH);
}
 
//------------------ pure virtual ones (just overriding) ---------------------
 
int OSD::available(void){
return 0;
}
int OSD::read(void){
return 0;
}
int OSD::peek(void){
return 0;
}
void OSD::flush(void){
}
 
/C-OSD/arducam-osd/ArduCAM_OSD/ArduCam_Max7456.h
0,0 → 1,105
 
#ifndef ArduCam_Max7456_h
#define ArduCam_Max7456_h
 
/******* FROM DATASHEET *******/
 
#define MAX7456_SELECT 6//SS
#define MAX7456_VSYNC 2//INT0
 
#define NTSC 0
#define PAL 1
#define MAX7456_MODE_MASK_PAL 0x40 //PAL mask 01000000
#define MAX7456_CENTER_PAL 0x8
 
#define MAX7456_MODE_MASK_NTCS 0x00 //NTSC mask 00000000 ("|" will do nothing)
#define MAX7456_CENTER_NTSC 0x6
 
//MAX7456 reg read addresses
#define MAX7456_OSDBL_reg_read 0xec //black level
#define MAX7456_STAT_reg_read 0xa0 //0xa[X] Status
 
//MAX7456 reg write addresses
#define MAX7456_VM0_reg 0x00
#define MAX7456_VM1_reg 0x01
#define MAX7456_DMM_reg 0x04
#define MAX7456_DMAH_reg 0x05
#define MAX7456_DMAL_reg 0x06
#define MAX7456_DMDI_reg 0x07
#define MAX7456_OSDM_reg 0x0c //not used. Is to set mix
#define MAX7456_OSDBL_reg 0x6c //black level
 
//MAX7456 reg write addresses to recording NVM process
#define MAX7456_CMM_reg 0x08
#define MAX7456_CMAH_reg 0x09
#define MAX7456_CMAL_reg 0x0a
#define MAX7456_CMDI_reg 0x0b
 
//DMM commands
#define MAX7456_CLEAR_display 0x04
#define MAX7456_CLEAR_display_vert 0x06
 
#define MAX7456_INCREMENT_auto 0x03
#define MAX7456_SETBG_local 0x20 //00100000 force local BG to defined brightness level VM1[6:4]
 
#define MAX7456_END_string 0xff
 
//VM0 commands mixed with mode NTSC or PAL mode
#define MAX7456_ENABLE_display_vert 0x0c //mask with NTSC/PAL
#define MAX7456_RESET 0x02 //mask with NTSC/PAL
#define MAX7456_DISABLE_display 0x00 //mask with NTSC/PAL
 
//VM0 command modifiers
#define MAX7456_SYNC_autosync 0x10
#define MAX7456_SYNC_internal 0x30
#define MAX7456_SYNC_external 0x20
//VM1 command modifiers
#define MAX7456_WHITE_level_80 0x03
#define MAX7456_WHITE_level_90 0x02
#define MAX7456_WHITE_level_100 0x01
#define MAX7456_WHITE_level_120 0x00
 
#define NVM_ram_size 0x36
#define WRITE_nvr 0xa0
#define STATUS_reg_nvr_busy 0x20
 
//If PAL
#ifdef isPAL
#define MAX7456_screen_size 480 //16x30
#define MAX7456_screen_rows 15
#else
#define MAX7456_screen_size 390 //13x30
#define MAX7456_screen_rows 12
#endif
 
//------------------ the OSD class -----------------------------------------------
 
class OSD: public BetterStream
{
public:
OSD(void);
void init(void);
void clear(void);
void plug(void);
void setPanel(uint8_t start_col, uint8_t start_row);
void openPanel(void);
void closePanel(void);
void control(uint8_t ctrl);
void detectMode(void);
void setMode(int mode);
void openSingle(uint8_t x, uint8_t y);
int getMode(void);
int getCenter(void);
virtual int available(void);
virtual int read(void);
virtual int peek(void);
virtual void flush(void);
virtual size_t write(uint8_t c);
void write_NVM(int font_count, uint8_t *character_bitmap);
using BetterStream::write;
private:
uint8_t start_col, start_row, col, row, video_mode, video_center;
};
 
#endif
 
/C-OSD/arducam-osd/ArduCAM_OSD/ArduNOTES.ino
0,0 → 1,82
/*
 
File : ArduCAM-OSD Notes
Version : v1.0.02
Author : Jani Hirvinen
 
File to define all panels that can be used on ArduCAM-OSD hardware. Also their order numbers
and other information.
 
Order number are also used when configuring their location. Look below
 
Register , Order number , Panel name, Data/Size , Definitions
-------------------------------------------------------
panA_REG.0 01 panCenter "IIII" \r "IIII" - Center xrosshair
panA_REG.1 02 panPitch "DDDDII" - Pitch angle with symbols
panA_REG.2 03 panRoll "DDDDII" - Roll angle with symbols
panA_REG.3 04 panBattery1 "DD.DII" - Voltage sensing #1 symbol with voltage reading
panA_REG.4 05 panBattery2 "DD.DII" - Voltage sensing #2 symbol with voltage reading (!Not Implemented)
panA_REG.5 06 panGPSats "I CC" - Amount of locked satelliset with symbols
panA_REG.6 07 panGPL "II" - GPS Lock symbol
panA_REG.7 08 panGPS "I DDD.DDDDDD" \r "I DDD.DDDDDD" - GPS location data (lat/lon)
panB_REG.0 09 panRose "IIIIIIIIIIIII" \r "ICCCCCCCCCCCCI" - Compass rose
panB_REG.1 10 panHeading "IIDDDDI" - Compass heading with symbols
panB_REG.2 11 panMavBeat "II" - MAVLink heartbeat
panB_REG.3 12 panHomeDir "II" N/Y - Home location arrows
panB_REG.4 13 panHomeDis "IDDDDDI" N/Y - Distance to home
panB_REG.5 14 panWPDir "II" N/Y - Waypoint location arrows (!Not Implemented)
panB_REG.6 15 panWPDis "IDDDDDI" - Distance to next waypoint (!Not Implemented)
panB_REG.7 16 panRSSI - RSSI data from RC (!Not Implemented)
panC_REG.0 17 panCurrent1 - Current sensing #1 (!Not Implemented)
panC_REG.1 18 panCurrent2 - Current sensing #2 (!Not Implemented)
panC_REG.2 19 panAlt "IDDDDDI" - Altitude information
panC_REG.3 20 panVel "IDDDDDI" - Velocity information
panC_REG.4 21 panThr "IDDDDDI" - MAVLink Throttle data
panC_REG.5 22 panFMod "II" - Flight mode display
panC_REG.6 05 panHorizon - Artificial Horizon
panC_REG.7 24 --
 
 
I = Icon
D = Digit
C = Char
 
N/Y = Not Yet working
N/C = Not Confirmed
 
Screen sizes:
PAL 15 Rows x 32 Chars
NTSC 13 Rows x 30 Chars
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
*/
/C-OSD/arducam-osd/ArduCAM_OSD/BOOT_Func.ino
0,0 → 1,76
 
 
/* ******************************************************************/
/* *********************** BOOT UP FUNCTIONS ********************** */
 
 
///////////////////////////////////////////////////////
// Function: loadBar(void)
//
// On bootup time we will show loading bar for defined BOOTTIME seconds
// This is interesting to avoid writing to APM during bootup if OSD's TX is connected
// After that, it continue in normal mode eg starting to listen MAVLink commands
 
#define barX 5
#define barY 12
 
void loadBar() { //change name due we don't have CLI anymore
int waitTimer;
byte barStep = 0;
 
// Write plain panel to let users know what to do
panBoot(barX,barY);
 
delay(500); // To give small extra waittime to users
// Serial.flush();
// Our main loop to wait input from user.
for(waitTimer = 0; waitTimer <= BOOTTIME; waitTimer++) {
 
// How often we update our progress bar is depending on modulo
if(waitTimer % (BOOTTIME / 8) == 0) {
barStep++;
// Update bar it self
osd.setPanel(barX + 12, barY);
osd.openPanel();
switch(barStep) {
case 0:
osd.printf_P(PSTR("\xf1\xf2\xf2\xf2\xf2\xf2\xf2"));
break;
case 1:
osd.printf_P(PSTR("\xef\xf2\xf2\xf2\xf2\xf2\xf2"));
break;
case 2:
osd.printf_P(PSTR("\xee\xf0\xf2\xf2\xf2\xf2\xf2"));
break;
case 3:
osd.printf_P(PSTR("\xee\xee\xf0\xf2\xf2\xf2\xf2"));
break;
case 4:
osd.printf_P(PSTR("\xee\xee\xee\xf0\xf2\xf2\xf2"));
break;
case 5:
osd.printf_P(PSTR("\xee\xee\xee\xee\xf0\xf2\xf2"));
break;
case 6:
osd.printf_P(PSTR("\xee\xee\xee\xee\xee\xf0\xf2"));
break;
case 7:
osd.printf_P(PSTR("\xee\xee\xee\xee\xee\xee\xf0"));
break;
case 8:
osd.printf_P(PSTR("\xee\xee\xee\xee\xee\xee\xee"));
break;
case 9:
osd.printf_P(PSTR("\xee\xee\xee\xee\xee\xee\xee\xee"));
break;
}
osd.closePanel();
}
delay(1); // Minor delay to make sure that we stay here long enough
}
}
 
 
/C-OSD/arducam-osd/ArduCAM_OSD/Font.ino
0,0 → 1,93
 
void uploadFont()
{
uint16_t byte_count = 0;
byte bit_count;
byte ascii_binary[0x08];
// move these local to prevent ram usage
uint8_t character_bitmap[0x40];
int font_count = 0;
osd.clear();
osd.setPanel(6,9);
osd.openPanel();
osd.printf_P(PSTR("Update CharSet"));
osd.closePanel();
Serial.printf_P(PSTR("Ready for Font\n"));
while(font_count < 256) {
int8_t incomingByte = Serial.read();
switch(incomingByte) // parse and decode mcm file
{
case 0x0d: // carridge return, end of line
//Serial.println("cr");
if (bit_count == 8 && (ascii_binary[0] == 0x30 || ascii_binary[0] == 0x31))
{
// turn 8 ascii binary bytes to single byte '01010101' = 0x55
// fill in 64 bytes of character data
// made this local to prevent needing a global
byte ascii_byte;
 
ascii_byte = 0;
if (ascii_binary[0] == 0x31) // ascii '1'
ascii_byte = ascii_byte + 128;
 
if (ascii_binary[1] == 0x31)
ascii_byte = ascii_byte + 64;
 
if (ascii_binary[2] == 0x31)
ascii_byte = ascii_byte + 32;
 
if (ascii_binary[3] == 0x31)
ascii_byte = ascii_byte + 16;
 
if (ascii_binary[4] == 0x31)
ascii_byte = ascii_byte + 8;
 
if (ascii_binary[5] == 0x31)
ascii_byte = ascii_byte + 4;
 
if (ascii_binary[6] == 0x31)
ascii_byte = ascii_byte + 2;
 
if (ascii_binary[7] == 0x31)
ascii_byte = ascii_byte + 1;
character_bitmap[byte_count] = ascii_byte;
byte_count++;
bit_count = 0;
}
else
bit_count = 0;
break;
case 0x0a: // line feed, ignore
//Serial.println("ln");
break;
case 0x30: // ascii '0'
case 0x31: // ascii '1'
ascii_binary[bit_count] = incomingByte;
bit_count++;
break;
default:
break;
}
// we have one completed character
// write the character to NVM
if(byte_count == 64)
{
osd.write_NVM(font_count, character_bitmap);
byte_count = 0;
font_count++;
Serial.printf_P(PSTR("Char Done\n"));
}
}
// character_bitmap[]
}
 
 
/C-OSD/arducam-osd/ArduCAM_OSD/MAVLink.ino
0,0 → 1,154
#define MAVLINK_COMM_NUM_BUFFERS 1
#define MAVLINK_USE_CONVENIENCE_FUNCTIONS
 
// this code was moved from libraries/GCS_MAVLink to allow compile
// time selection of MAVLink 1.0
BetterStream *mavlink_comm_0_port;
BetterStream *mavlink_comm_1_port;
 
mavlink_system_t mavlink_system = {12,1,0,0};
 
#include "Mavlink_compat.h"
 
#ifdef MAVLINK10
#include "../GCS_MAVLink/include/mavlink/v1.0/mavlink_types.h"
#include "../GCS_MAVLink/include/mavlink/v1.0/ardupilotmega/mavlink.h"
#else
#include "../GCS_MAVLink/include/mavlink/v0.9/mavlink_types.h"
#include "../GCS_MAVLink/include/mavlink/v0.9/ardupilotmega/mavlink.h"
#endif
 
// true when we have received at least 1 MAVLink packet
static bool mavlink_active;
static uint8_t crlf_count = 0;
 
static int packet_drops = 0;
static int parse_error = 0;
 
void request_mavlink_rates()
{
const int maxStreams = 6;
const uint8_t MAVStreams[maxStreams] = {MAV_DATA_STREAM_RAW_SENSORS,
MAV_DATA_STREAM_EXTENDED_STATUS,
MAV_DATA_STREAM_RC_CHANNELS,
MAV_DATA_STREAM_POSITION,
MAV_DATA_STREAM_EXTRA1,
MAV_DATA_STREAM_EXTRA2};
const uint16_t MAVRates[maxStreams] = {0x02, 0x02, 0x05, 0x02, 0x05, 0x02};
for (int i=0; i < maxStreams; i++) {
mavlink_msg_request_data_stream_send(MAVLINK_COMM_0,
apm_mav_system, apm_mav_component,
MAVStreams[i], MAVRates[i], 1);
}
}
 
void read_mavlink(){
mavlink_message_t msg;
mavlink_status_t status;
//grabing data
while(Serial.available() > 0) {
uint8_t c = Serial.read();
/* allow CLI to be started by hitting enter 3 times, if no
heartbeat packets have been received */
if (mavlink_active == 0 && millis() < 20000) {
if (c == '\n' || c == '\r') {
crlf_count++;
} else {
crlf_count = 0;
}
if (crlf_count == 3) {
uploadFont();
}
}
//trying to grab msg
if(mavlink_parse_char(MAVLINK_COMM_0, c, &msg, &status)) {
mavlink_active = 1;
//handle msg
switch(msg.msgid) {
case MAVLINK_MSG_ID_HEARTBEAT:
{
mavbeat = 1;
apm_mav_system = msg.sysid;
apm_mav_component = msg.compid;
apm_mav_type = mavlink_msg_heartbeat_get_type(&msg);
#ifdef MAVLINK10
osd_mode = mavlink_msg_heartbeat_get_custom_mode(&msg);
osd_nav_mode = 0;
#endif
lastMAVBeat = millis();
if(waitingMAVBeats == 1){
enable_mav_request = 1;
}
}
break;
case MAVLINK_MSG_ID_SYS_STATUS:
{
#ifndef MAVLINK10
osd_vbat_A = (mavlink_msg_sys_status_get_vbat(&msg) / 1000.0f);
osd_mode = mavlink_msg_sys_status_get_mode(&msg);
osd_nav_mode = mavlink_msg_sys_status_get_nav_mode(&msg);
#else
osd_vbat_A = (mavlink_msg_sys_status_get_voltage_battery(&msg) / 1000.0f);
#endif
osd_battery_remaining_A = mavlink_msg_sys_status_get_battery_remaining(&msg);
//osd_mode = apm_mav_component;//Debug
//osd_nav_mode = apm_mav_system;//Debug
}
break;
#ifndef MAVLINK10
case MAVLINK_MSG_ID_GPS_RAW:
{
osd_lat = mavlink_msg_gps_raw_get_lat(&msg);
osd_lon = mavlink_msg_gps_raw_get_lon(&msg);
osd_fix_type = mavlink_msg_gps_raw_get_fix_type(&msg);
}
break;
case MAVLINK_MSG_ID_GPS_STATUS:
{
osd_satellites_visible = mavlink_msg_gps_status_get_satellites_visible(&msg);
}
break;
#else
case MAVLINK_MSG_ID_GPS_RAW_INT:
{
osd_lat = mavlink_msg_gps_raw_int_get_lat(&msg) / 10000000.0f;
osd_lon = mavlink_msg_gps_raw_int_get_lon(&msg) / 10000000.0f;
osd_fix_type = mavlink_msg_gps_raw_int_get_fix_type(&msg);
osd_satellites_visible = mavlink_msg_gps_raw_int_get_satellites_visible(&msg);
}
break;
#endif
 
case MAVLINK_MSG_ID_VFR_HUD:
{
osd_groundspeed = mavlink_msg_vfr_hud_get_groundspeed(&msg);
osd_heading = mavlink_msg_vfr_hud_get_heading(&msg);// * 3.60f;//0-100% of 360
osd_throttle = mavlink_msg_vfr_hud_get_throttle(&msg);
if(osd_throttle > 100 && osd_throttle < 150) osd_throttle = 100;//Temporary fix for ArduPlane 2.28
if(osd_throttle < 0 || osd_throttle > 150) osd_throttle = 0;//Temporary fix for ArduPlane 2.28
osd_alt = mavlink_msg_vfr_hud_get_alt(&msg);
}
break;
case MAVLINK_MSG_ID_ATTITUDE:
{
osd_pitch = ToDeg(mavlink_msg_attitude_get_pitch(&msg));
osd_roll = ToDeg(mavlink_msg_attitude_get_roll(&msg));
osd_yaw = ToDeg(mavlink_msg_attitude_get_yaw(&msg));
}
break;
default:
//Do nothing
break;
}
}
delayMicroseconds(138);
//next one
}
// Update global packet drops counter
packet_drops += status.packet_rx_drop_count;
parse_error += status.parse_error;
 
}
/C-OSD/arducam-osd/ArduCAM_OSD/OSD_Config.h
0,0 → 1,129
 
#define on 1
#define off 0
 
// Versio number, incrementing this will erase/upload factory settings.
// Only devs should increment this
#define VER 74
 
// EEPROM Stepping, be careful not to overstep.
// We reserved floats for just to be sure if some values needs to be
// changed in future.
// byte = 1
// int = 4
// float = 8
 
// Panel 8bit REGISTER with BIT positions
// panA_REG Byte has:
#define Cen_BIT 0
#define Pit_BIT 1
#define Rol_BIT 2
#define BatA_BIT 3
#define BatB_BIT 4 //(!Not implemented)
#define GPSats_BIT 5
#define GPL_BIT 6
#define GPS_BIT 7
 
// panB_REG Byte has:
#define Rose_BIT 0
#define Head_BIT 1
#define MavB_BIT 2
#define HDir_BIT 3
#define HDis_BIT 4
#define WDir_BIT 5 //(!Not implemented)
#define WDis_BIT 6 //(!Not implemented)
#define RSSI_BIT 7 //(!Not implemented)
 
// panC_REG Byte has:
#define CurA_BIT 0 //(!Not implemented)
#define CurB_BIT 1 //(!Not implemented)
#define Alt_BIT 2
#define Vel_BIT 3
#define Thr_BIT 4
#define FMod_BIT 5
#define Hor_BIT 6
//#define XXC_BIT 7 //Free
 
 
/* *********************************************** */
// EEPROM Storage addresses
 
// First of 8 panels
#define panCenter_en_ADDR 0
#define panCenter_x_ADDR 2
#define panCenter_y_ADDR 4
#define panPitch_en_ADDR 6
#define panPitch_x_ADDR 8
#define panPitch_y_ADDR 10
#define panRoll_en_ADDR 12
#define panRoll_x_ADDR 14
#define panRoll_y_ADDR 16
#define panBatt_A_en_ADDR 18
#define panBatt_A_x_ADDR 20
#define panBatt_A_y_ADDR 22
#define panBatt_B_en_ADDR 24
#define panBatt_B_x_ADDR 26
#define panBatt_B_y_ADDR 28
#define panGPSats_en_ADDR 30
#define panGPSats_x_ADDR 32
#define panGPSats_y_ADDR 34
#define panGPL_en_ADDR 36
#define panGPL_x_ADDR 38
#define panGPL_y_ADDR 40
#define panGPS_en_ADDR 42
#define panGPS_x_ADDR 44
#define panGPS_y_ADDR 46
 
// Second set of 8 panels
#define panRose_en_ADDR 48
#define panRose_x_ADDR 50
#define panRose_y_ADDR 52
#define panHeading_en_ADDR 54
#define panHeading_x_ADDR 56
#define panHeading_y_ADDR 58
#define panMavBeat_en_ADDR 60
#define panMavBeat_x_ADDR 62
#define panMavBeat_y_ADDR 64
#define panHomeDir_en_ADDR 66
#define panHomeDir_x_ADDR 68
#define panHomeDir_y_ADDR 70
#define panHomeDis_en_ADDR 72
#define panHomeDis_x_ADDR 74
#define panHomeDis_y_ADDR 76
#define panWPDir_en_ADDR 80 //(!Not implemented)
#define panWPDir_x_ADDR 82 //
#define panWPDir_y_ADDR 84 //
#define panWPDis_en_ADDR 86 //(!Not implemented)
#define panWPDis_x_ADDR 88 //
#define panWPDis_y_ADDR 90 //
#define panRSSI_en_ADDR 92 //(!Not implemented)
#define panRSSI_x_ADDR 94 //
#define panRSSI_y_ADDR 96 //
 
// Third set of 8 panels
#define panCurA_en_ADDR 98 //(!Not implemented)
#define panCurA_x_ADDR 100 //
#define panCurA_y_ADDR 102 //
#define panCurB_en_ADDR 104 //(!Not implemented)
#define panCurB_x_ADDR 106 //
#define panCurB_y_ADDR 108 //
#define panAlt_en_ADDR 110
#define panAlt_x_ADDR 112
#define panAlt_y_ADDR 114
#define panVel_en_ADDR 116
#define panVel_x_ADDR 118
#define panVel_y_ADDR 120
#define panThr_en_ADDR 122
#define panThr_x_ADDR 124
#define panThr_y_ADDR 126
#define panFMod_en_ADDR 128
#define panFMod_x_ADDR 130
#define panFMod_y_ADDR 132
#define panHorizon_en_ADDR 134
#define panHorizon_x_ADDR 136
#define panHorizon_y_ADDR 138
 
#define CHK1 1000
#define CHK2 1006
 
#define EEPROM_MAX_ADDR 1024 // this is 328 chip
/C-OSD/arducam-osd/ArduCAM_OSD/OSD_Config_Func.ino
0,0 → 1,248
/* ******************************************************************/
/* *********************** GENERAL FUNCTIONS ********************** */
 
//Extract functions (get bits from the positioning bytes
#define ISa(whichBit) getBit(panA_REG, whichBit)
#define ISb(whichBit) getBit(panB_REG, whichBit)
#define ISc(whichBit) getBit(panC_REG, whichBit)
 
boolean getBit(byte Reg, byte whichBit) {
boolean State;
State = Reg & (1 << whichBit);
return State;
}
 
byte setBit(byte &Reg, byte whichBit, boolean stat) {
if (stat) {
Reg = Reg | (1 << whichBit);
}
else {
Reg = Reg & ~(1 << whichBit);
}
return Reg;
}
 
// EEPROM reader/writers
// Utilities for writing and reading from the EEPROM
byte readEEPROM(int address) {
 
return EEPROM.read(address);
}
 
void writeEEPROM(byte value, int address) {
EEPROM.write(address, value);
}
 
 
void InitializeOSD() {
loadBar();
delay(500);
 
writeEEPROM(42, CHK1);
writeEEPROM(VER-42,CHK2);
writeSettings();
osd.setPanel(4,9);
osd.openPanel();
osd.printf_P(PSTR("OSD Initialized, reboot"));
osd.closePanel();
// run for ever so user resets
for(;;) {}
}
 
// Write our latest FACTORY settings to EEPROM
void writeSettings() {
// Writing all default parameters to EEPROM, ON = panel enabled
// All panels have 3 values:
// - Enable/Disable
// - X coordinate on screen
// - Y coordinate on screen
writeEEPROM(off, panCenter_en_ADDR);
writeEEPROM(13, panCenter_x_ADDR);
writeEEPROM(7, panCenter_y_ADDR);
writeEEPROM(on, panPitch_en_ADDR);
writeEEPROM(22, panPitch_x_ADDR);
writeEEPROM(9, panPitch_y_ADDR);
writeEEPROM(on, panRoll_en_ADDR);
writeEEPROM(11, panRoll_x_ADDR);
writeEEPROM(1, panRoll_y_ADDR);
writeEEPROM(on, panBatt_A_en_ADDR);
writeEEPROM(21, panBatt_A_x_ADDR);
writeEEPROM(1, panBatt_A_y_ADDR);
//writeEEPROM(on, panBatt_B_en_ADDR);
//writeEEPROM(21, panBatt_B_x_ADDR);
//writeEEPROM(3, panBatt_B_y_ADDR);
writeEEPROM(on, panGPSats_en_ADDR);
writeEEPROM(2, panGPSats_x_ADDR);
writeEEPROM(13, panGPSats_y_ADDR);
writeEEPROM(on, panGPL_en_ADDR);
writeEEPROM(5, panGPL_x_ADDR);
writeEEPROM(13, panGPL_y_ADDR);
writeEEPROM(on, panGPS_en_ADDR);
writeEEPROM(2, panGPS_x_ADDR);
writeEEPROM(14, panGPS_y_ADDR);
writeEEPROM(on, panRose_en_ADDR);
writeEEPROM(16, panRose_x_ADDR);
writeEEPROM(14, panRose_y_ADDR);
writeEEPROM(on, panHeading_en_ADDR);
writeEEPROM(24, panHeading_x_ADDR);
writeEEPROM(13, panHeading_y_ADDR);
writeEEPROM(on, panMavBeat_en_ADDR);
writeEEPROM(2, panMavBeat_x_ADDR);
writeEEPROM(9, panMavBeat_y_ADDR);
writeEEPROM(on, panHomeDir_en_ADDR);
writeEEPROM(14, panHomeDir_x_ADDR);
writeEEPROM(3, panHomeDir_y_ADDR);
writeEEPROM(on, panHomeDis_en_ADDR);
writeEEPROM(2, panHomeDis_x_ADDR);
writeEEPROM(1, panHomeDis_y_ADDR);
writeEEPROM(off,panWPDir_en_ADDR);
writeEEPROM(0, panWPDir_x_ADDR);
writeEEPROM(0, panWPDir_y_ADDR);
writeEEPROM(off,panWPDis_en_ADDR);
writeEEPROM(0, panWPDis_x_ADDR);
writeEEPROM(0, panWPDis_y_ADDR);
//writeEEPROM(on, panRSSI_en_ADDR);
//writeEEPROM(21, panRSSI_x_ADDR);
//writeEEPROM(5, panRSSI_y_ADDR);
//writeEEPROM(on, panCur_A_en_ADDR);
//writeEEPROM(21, panCur_A_x_ADDR);
//writeEEPROM(2, panCur_A_y_ADDR);
//writeEEPROM(on, panCur_B_en_ADDR);
//writeEEPROM(21, panCur_B_x_ADDR);
//writeEEPROM(4, panCur_B_y_ADDR);
writeEEPROM(on, panAlt_en_ADDR);
writeEEPROM(2, panAlt_x_ADDR);
writeEEPROM(2, panAlt_y_ADDR);
writeEEPROM(on, panVel_en_ADDR);
writeEEPROM(2, panVel_x_ADDR);
writeEEPROM(3, panVel_y_ADDR);
writeEEPROM(on, panThr_en_ADDR);
writeEEPROM(2, panThr_x_ADDR);
writeEEPROM(4, panThr_y_ADDR);
writeEEPROM(on, panFMod_en_ADDR);
writeEEPROM(17, panFMod_x_ADDR);
writeEEPROM(13, panFMod_y_ADDR);
writeEEPROM(on, panHorizon_en_ADDR);
writeEEPROM(8, panHorizon_x_ADDR);
writeEEPROM(7, panHorizon_y_ADDR);
}
 
void readSettings() {
//****** First set of 8 Panels ******
setBit(panA_REG, Cen_BIT, readEEPROM(panCenter_en_ADDR));
panCenter_XY[0] = readEEPROM(panCenter_x_ADDR);
panCenter_XY[1] = checkPAL(readEEPROM(panCenter_y_ADDR));
setBit(panA_REG, Pit_BIT, readEEPROM(panPitch_en_ADDR));
panPitch_XY[0] = readEEPROM(panPitch_x_ADDR);
panPitch_XY[1] = checkPAL(readEEPROM(panPitch_y_ADDR));
setBit(panA_REG, Rol_BIT, readEEPROM(panRoll_en_ADDR));
panRoll_XY[0] = readEEPROM(panRoll_x_ADDR);
panRoll_XY[1] = checkPAL(readEEPROM(panRoll_y_ADDR));
setBit(panA_REG, BatA_BIT, readEEPROM(panBatt_A_en_ADDR));
panBatt_A_XY[0] = readEEPROM(panBatt_A_x_ADDR);
panBatt_A_XY[1] = checkPAL(readEEPROM(panBatt_A_y_ADDR));
 
//setBit(panA_REG, BatB_BIT, readEEPROM(panBatt_B_en_ADDR));
//panBatt_B_XY[0] = readEEPROM(panBatt_B_x_ADDR);
//panBatt_B_XY[1] = checkPAL(readEEPROM(panBatt_B_y_ADDR));
setBit(panA_REG, GPSats_BIT, readEEPROM(panGPSats_en_ADDR));
panGPSats_XY[0] = readEEPROM(panGPSats_x_ADDR);
panGPSats_XY[1] = checkPAL(readEEPROM(panGPSats_y_ADDR));
 
setBit(panA_REG, GPL_BIT, readEEPROM(panGPL_en_ADDR));
panGPL_XY[0] = readEEPROM(panGPL_x_ADDR);
panGPL_XY[1] = checkPAL(readEEPROM(panGPL_y_ADDR));
setBit(panA_REG, GPS_BIT, readEEPROM(panGPS_en_ADDR));
panGPS_XY[0] = readEEPROM(panGPS_x_ADDR);
panGPS_XY[1] = checkPAL(readEEPROM(panGPS_y_ADDR));
 
//****** Second set of 8 Panels ******
setBit(panB_REG, Rose_BIT, readEEPROM(panRose_en_ADDR));
panRose_XY[0] = readEEPROM(panRose_x_ADDR);
panRose_XY[1] = checkPAL(readEEPROM(panRose_y_ADDR));
 
setBit(panB_REG, Head_BIT, readEEPROM(panHeading_en_ADDR));
panHeading_XY[0] = readEEPROM(panHeading_x_ADDR);
panHeading_XY[1] = checkPAL(readEEPROM(panHeading_y_ADDR));
 
setBit(panB_REG, MavB_BIT, readEEPROM(panMavBeat_en_ADDR));
panMavBeat_XY[0] = readEEPROM(panMavBeat_x_ADDR);
panMavBeat_XY[1] = checkPAL(readEEPROM(panMavBeat_y_ADDR));
 
setBit(panB_REG, HDis_BIT, readEEPROM(panHomeDis_en_ADDR));
panHomeDis_XY[0] = readEEPROM(panHomeDis_x_ADDR);
panHomeDis_XY[1] = checkPAL(readEEPROM(panHomeDis_y_ADDR));
 
setBit(panB_REG, HDir_BIT, readEEPROM(panHomeDir_en_ADDR));
panHomeDir_XY[0] = readEEPROM(panHomeDir_x_ADDR);
panHomeDir_XY[1] = checkPAL(readEEPROM(panHomeDir_y_ADDR));
 
//setBit(panB_REG, RSSI_BIT, readEEPROM(panRSSI_en_ADDR));
//panRSSI_XY[0] = readEEPROM(panRSSI_x_ADDR);
//panRSSI_XY[1] = checkPAL(readEEPROM(panRSSI_y_ADDR));
 
//****** Third set of 8 Panels ******
 
//setBit(panC_REG, CurA_BIT, readEEPROM(panCur_A_en_ADDR));
//panCur_A_XY[0] = readEEPROM(panCur_A_x_ADDR);
//panCur_A_XY[1] = checkPAL(readEEPROM(panCur_A_y_ADDR));
 
//setBit(panC_REG, CurB_BIT, readEEPROM(panCur_B_en_ADDR));
//panCur_B_XY[0] = readEEPROM(panCur_B_x_ADDR);
//panCur_B_XY[1] = checkPAL(readEEPROM(panCur_B_y_ADDR));
 
setBit(panC_REG, Alt_BIT, readEEPROM(panAlt_en_ADDR));
panAlt_XY[0] = readEEPROM(panAlt_x_ADDR);
panAlt_XY[1] = checkPAL(readEEPROM(panAlt_y_ADDR));
 
setBit(panC_REG, Vel_BIT, readEEPROM(panVel_en_ADDR));
panVel_XY[0] = readEEPROM(panVel_x_ADDR);
panVel_XY[1] = checkPAL(readEEPROM(panVel_y_ADDR));
 
setBit(panC_REG, Thr_BIT, readEEPROM(panThr_en_ADDR));
panThr_XY[0] = readEEPROM(panThr_x_ADDR);
panThr_XY[1] = checkPAL(readEEPROM(panThr_y_ADDR));
 
setBit(panC_REG, FMod_BIT, readEEPROM(panFMod_en_ADDR));
panFMod_XY[0] = readEEPROM(panFMod_x_ADDR);
panFMod_XY[1] = checkPAL(readEEPROM(panFMod_y_ADDR));
 
setBit(panC_REG, Hor_BIT, readEEPROM(panHorizon_en_ADDR));
panHorizon_XY[0] = readEEPROM(panHorizon_x_ADDR);
panHorizon_XY[1] = checkPAL(readEEPROM(panHorizon_y_ADDR));
 
}
 
int checkPAL(int line){
if(line >= osd.getCenter() && osd.getMode() == 0){
line -= 3;//Cutting lines offset after center if NTSC
}
return line;
}
 
void updateSettings(byte panel, byte panel_x, byte panel_y, byte panel_s ) {
if(panel >= 1 && panel <= 32) {
writeEEPROM(panel_s, (6 * panel) - 6 + 0);
if(panel_s != 0) {
writeEEPROM(panel_x, (6 * panel) - 6 + 2);
writeEEPROM(panel_y, (6 * panel) - 6 + 4);
}
osd.clear();
readSettings();
}
}
 
/C-OSD/arducam-osd/ArduCAM_OSD/OSD_Func.h
0,0 → 1,81
//------------------ Heading and Compass ----------------------------------------
 
static char buf_show[12];
const char buf_Rule[36] = {0xc2,0xc0,0xc0,0xc1,0xc0,0xc0,0xc1,0xc0,0xc0,
0xc4,0xc0,0xc0,0xc1,0xc0,0xc0,0xc1,0xc0,0xc0,
0xc3,0xc0,0xc0,0xc1,0xc0,0xc0,0xc1,0xc0,0xc0,
0xc5,0xc0,0xc0,0xc1,0xc0,0xc0,0xc1,0xc0,0xc0};
void setHeadingPatern()
{
int start;
start = round((osd_heading * 36)/360);
start -= 5;
if(start < 0) start += 36;
for(int x=0; x <= 10; x++){
buf_show[x] = buf_Rule[start];
if(++start > 35) start = 0;
}
buf_show[11] = '\0';
}
 
//------------------ Battery Remaining Picture ----------------------------------
 
char setBatteryPic(uint16_t bat_level)
{
if(bat_level <= 100){
return 0xb4;
}
else if(bat_level <= 300){
return 0xb5;
}
else if(bat_level <= 400){
return 0xb6;
}
else if(bat_level <= 500){
return 0xb7;
}
else if(bat_level <= 800){
return 0xb8;
}
else return 0xb9;
}
 
//------------------ Home Distance and Direction Calculation ----------------------------------
 
void setHomeVars(OSD &osd)
{
float dstlon, dstlat;
long bearing;
if(osd_got_home == 0 && osd_fix_type > 1){
osd_home_lat = osd_lat;
osd_home_lon = osd_lon;
osd_home_alt = osd_alt;
osd_got_home = 1;
}
else if(osd_got_home == 1){
// shrinking factor for longitude going to poles direction
float rads = fabs(osd_home_lat) * 0.0174532925;
double scaleLongDown = cos(rads);
double scaleLongUp = 1.0f/cos(rads);
 
//DST to Home
dstlat = fabs(osd_home_lat - osd_lat) * 111319.5;
dstlon = fabs(osd_home_lon - osd_lon) * 111319.5 * scaleLongDown;
osd_home_distance = sqrt(sq(dstlat) + sq(dstlon));
 
//DIR to Home
dstlon = (osd_home_lon - osd_lon); //OffSet_X
dstlat = (osd_home_lat - osd_lat) * scaleLongUp; //OffSet Y
bearing = 90 + (atan2(dstlat, -dstlon) * 57.295775); //absolut home direction
if(bearing < 0) bearing += 360;//normalization
bearing = bearing - 180;//absolut return direction
if(bearing < 0) bearing += 360;//normalization
bearing = bearing - osd_heading;//relative home direction
if(bearing < 0) bearing += 360; //normalization
osd_home_direction = round((float)(bearing/360.0f) * 16.0f) + 1;//array of arrows =)
if(osd_home_direction > 16) osd_home_direction = 0;
 
}
}
/C-OSD/arducam-osd/ArduCAM_OSD/OSD_Panels.ino
0,0 → 1,570
/******* STARTUP PANEL *******/
 
void startPanels(){
osd.clear();
// Display our logo
panLogo(10,5);
}
 
/******* PANELS - POSITION *******/
 
void writePanels(){
if(millis() < (lastMAVBeat + 2000)){
//osd.clear();
//Testing bits from 8 bit register A
if(ISa(Cen_BIT)) panCenter(panCenter_XY[0], panCenter_XY[1]); //4x2
if(ISa(Pit_BIT)) panPitch(panPitch_XY[0], panPitch_XY[1]); //5x1
if(ISa(Rol_BIT)) panRoll(panRoll_XY[0], panRoll_XY[1]); //5x1
if(ISa(BatA_BIT)) panBatt_A(panBatt_A_XY[0], panBatt_A_XY[1]); //7x1
// if(ISa(BatB_BIT)) panBatt_B(panBatt_B_XY[0], panBatt_B_XY[1]); //7x1
if(ISa(GPSats_BIT)) panGPSats(panGPSats_XY[0], panGPSats_XY[1]); //5x1
if(ISa(GPL_BIT)) panGPL(panGPL_XY[0], panGPL_XY[1]); //2x1
if(ISa(GPS_BIT)) panGPS(panGPS_XY[0], panGPS_XY[1]); //12x3
//Testing bits from 8 bit register B
if(ISb(Rose_BIT)) panRose(panRose_XY[0], panRose_XY[1]); //13x3
if(ISb(Head_BIT)) panHeading(panHeading_XY[0], panHeading_XY[1]); //13x3
if(ISb(MavB_BIT)) panMavBeat(panMavBeat_XY[0], panMavBeat_XY[1]); //13x3
 
if(osd_got_home == 1){
if(ISb(HDis_BIT)) panHomeDis(panHomeDis_XY[0], panHomeDis_XY[1]); //13x3
if(ISb(HDir_BIT)) panHomeDir(panHomeDir_XY[0], panHomeDir_XY[1]); //13x3
}
// if(ISb(WDir_BIT)) panWayPDir(panWayPDir_XY[0], panWayPDir_XY[1]); //??x??
// if(ISb(WDis_BIT)) panWayPDis(panWayPDis_XY[0], panWayPDis_XY[1]); //??x??
// if(ISb(WRSSI_BIT)) panRSSI(panRSSI_XY[0], panRSSI_XY[1]); //??x??
 
//Testing bits from 8 bit register C
//if(osd_got_home == 1){
if(ISc(Alt_BIT)) panAlt(panAlt_XY[0], panAlt_XY[1]); //
if(ISc(Vel_BIT)) panVel(panVel_XY[0], panVel_XY[1]); //
//}
if(ISc(Thr_BIT)) panThr(panThr_XY[0], panThr_XY[1]); //
if(ISc(FMod_BIT)) panFlightMode(panFMod_XY[0], panFMod_XY[1]); //
if(ISc(Hor_BIT)) panHorizon(panHorizon_XY[0], panHorizon_XY[1]); //14x5
}
else{
osd.clear();
waitingMAVBeats = 1;
// Display our logo and wait...
panWaitMAVBeats(5,10); //Waiting for MAVBeats...
}
// OSD debug for development (Shown on top-middle panels)
#ifdef membug
osd.setPanel(13,4);
osd.openPanel();
osd.printf("%i",freeMem());
osd.closePanel();
#endif
}
 
/******* PANELS - DEFINITION *******/
 
 
/* **************************************************************** */
// Panel : panAlt
// Needs : X, Y locations
// Output : Alt symbol and altitude value in meters from MAVLink
// Size : 1 x 7Hea (rows x chars)
// Staus : done
 
void panAlt(int first_col, int first_line){
osd.setPanel(first_col, first_line);
osd.openPanel();
//osd.printf("%c%5.0f%c",0x85, (double)(osd_alt - osd_home_alt), 0x8D);
osd.printf("%c%5.0f%c",0x85, (double)(osd_alt), 0x8D);
osd.closePanel();
}
 
/* **************************************************************** */
// Panel : panVel
// Needs : X, Y locations
// Output : Velocity value from MAVlink with symbols
// Size : 1 x 7 (rows x chars)
// Staus : done
 
void panVel(int first_col, int first_line){
osd.setPanel(first_col, first_line);
osd.openPanel();
osd.printf("%c%3.0f%c",0x86,(double)osd_groundspeed,0x88);
osd.closePanel();
}
 
/* **************************************************************** */
// Panel : panThr
// Needs : X, Y locations
// Output : Throttle value from MAVlink with symbols
// Size : 1 x 7 (rows x chars)
// Staus : done
 
void panThr(int first_col, int first_line){
osd.setPanel(first_col, first_line);
osd.openPanel();
osd.printf("%c%3.0i%c",0x87,osd_throttle,0x25);
osd.closePanel();
}
 
/* **************************************************************** */
// Panel : panHomeDis
// Needs : X, Y locations
// Output : Home Symbol with distance to home in meters
// Size : 1 x 7 (rows x chars)
// Staus : done
 
void panHomeDis(int first_col, int first_line){
osd.setPanel(first_col, first_line);
osd.openPanel();
osd.printf("%c%5.0f%c", 0x1F, (double)osd_home_distance, 0x8D);
osd.closePanel();
}
 
/* **************************************************************** */
// Panel : panCenter
// Needs : X, Y locations
// Output : 2 row croshair symbol created by 2 x 4 chars
// Size : 2 x 4 (rows x chars)
// Staus : done
 
void panCenter(int first_col, int first_line){
osd.setPanel(first_col, first_line);
osd.openPanel();
osd.printf_P(PSTR("\x05\x03\x04\x05|\x15\x13\x14\x15"));
osd.closePanel();
}
 
/* **************************************************************** */
// Panel : panHorizon
// Needs : X, Y locations
// Output : 12 x 4 Horizon line surrounded by 2 cols (left/right rules)
// Size : 14 x 4 (rows x chars)
// Staus : done
 
void panHorizon(int first_col, int first_line){
osd.setPanel(first_col, first_line);
osd.openPanel();
osd.printf_P(PSTR("\xc8\x20\x20\x20\x20\x20\x20\x20\x20\x20\x20\x20\x20\xc9|"));
osd.printf_P(PSTR("\xc8\x20\x20\x20\x20\x20\x20\x20\x20\x20\x20\x20\x20\xc9|"));
osd.printf_P(PSTR("\xd8\x20\x20\x20\x20\x20\x20\x20\x20\x20\x20\x20\x20\xd9|"));
osd.printf_P(PSTR("\xc8\x20\x20\x20\x20\x20\x20\x20\x20\x20\x20\x20\x20\xc9|"));
osd.printf_P(PSTR("\xc8\x20\x20\x20\x20\x20\x20\x20\x20\x20\x20\x20\x20\xc9"));
osd.closePanel();
showHorizon((first_col + 1), first_line);
}
 
/* **************************************************************** */
// Panel : panPitch
// Needs : X, Y locations
// Output : -+ value of current Pitch from vehicle with degree symbols and pitch symbol
// Size : 1 x 6 (rows x chars)
// Staus : done
 
void panPitch(int first_col, int first_line){
osd.setPanel(first_col, first_line);
osd.openPanel();
osd.printf("%4i%c%c",osd_pitch,0xb0,0xb1);
osd.closePanel();
}
 
/* **************************************************************** */
// Panel : panRoll
// Needs : X, Y locations
// Output : -+ value of current Roll from vehicle with degree symbols and roll symbol
// Size : 1 x 6 (rows x chars)
// Staus : done
 
void panRoll(int first_col, int first_line){
osd.setPanel(first_col, first_line);
osd.openPanel();
osd.printf("%4i%c%c",osd_roll,0xb0,0xb2);
osd.closePanel();
}
 
/* **************************************************************** */
// Panel : panBattery A (Voltage 1)
// Needs : X, Y locations
// Output : Voltage value as in XX.X and symbol of over all battery status
// Size : 1 x 8 (rows x chars)
// Staus : done
 
void panBatt_A(int first_col, int first_line){
osd.setPanel(first_col, first_line);
osd.openPanel();
/*************** This commented code is for the next ArduPlane Version
#ifdef MAVLINK10
if(osd_battery_remaining_A > 100){
osd.printf(" %c%5.2f%c", 0xE2, (double)osd_vbat_A, 0x8E);
}
#else
if(osd_battery_remaining_A > 1000){
osd.printf(" %c%5.2f%c", 0xE2, (double)osd_vbat_A, 0x8E);
}
#endif //MAVLINK10
else osd.printf("%c%5.2f%c%c", 0xE2, (double)osd_vbat_A, 0x8E, osd_battery_pic_A);
*/
osd.printf(" %c%5.2f%c", 0xE2, (double)osd_vbat_A, 0x8E);
osd.closePanel();
}
 
//------------------ Panel: Startup ArduCam OSD LOGO -------------------------------
 
void panLogo(int first_col, int first_line){
osd.setPanel(first_col, first_line);
osd.openPanel();
osd.printf_P(PSTR("\x20\x20\x20\x20\xba\xbb\xbc\xbd\xbe|\x20\x20\x20\x20\xca\xcb\xcc\xcd\xce|ArduCam OSD"));
osd.closePanel();
}
 
//------------------ Panel: Waiting for MAVLink HeartBeats -------------------------------
 
void panWaitMAVBeats(int first_col, int first_line){
panLogo(10,5);
osd.setPanel(first_col, first_line);
osd.openPanel();
osd.printf_P(PSTR("Waiting for|MAVLink heartbeats..."));
osd.closePanel();
}
 
/* **************************************************************** */
// Panel : panGPL
// Needs : X, Y locations
// Output : 1 static symbol with changing FIX symbol
// Size : 1 x 2 (rows x chars)
// Staus : done
 
void panGPL(int first_col, int first_line){
osd.setPanel(first_col, first_line);
osd.openPanel();
switch(osd_fix_type) {
case 0:
osd.printf_P(PSTR("\x10\x20"));
break;
case 1:
osd.printf_P(PSTR("\x10\x20"));
break;
case 2:
osd.printf_P(PSTR("\x11\x20"));//If not APM, x01 would show 2D fix
break;
case 3:
osd.printf_P(PSTR("\x11\x20"));//If not APM, x02 would show 3D fix
break;
}
/* if(osd_fix_type <= 1) {
osd.printf_P(PSTR("\x10"));
} else {
osd.printf_P(PSTR("\x11"));
} */
osd.closePanel();
}
 
/* **************************************************************** */
// Panel : panGPSats
// Needs : X, Y locations
// Output : 1 symbol and number of locked satellites
// Size : 1 x 5 (rows x chars)
// Staus : done
 
void panGPSats(int first_col, int first_line){
osd.setPanel(first_col, first_line);
osd.openPanel();
osd.printf("%c%2i", 0x0f,osd_satellites_visible);
osd.closePanel();
}
 
/* **************************************************************** */
// Panel : panGPS
// Needs : X, Y locations
// Output : two row numeric value of current GPS location with LAT/LON symbols as on first char
// Size : 2 x 12 (rows x chars)
// Staus : done
 
void panGPS(int first_col, int first_line){
osd.setPanel(first_col, first_line);
osd.openPanel();
osd.printf("%c%11.6f|%c%11.6f", 0x83, (double)osd_lat, 0x84, (double)osd_lon);
osd.closePanel();
}
 
/* **************************************************************** */
// Panel : panHeading
// Needs : X, Y locations
// Output : Symbols with numeric compass heading value
// Size : 1 x 5 (rows x chars)
// Staus : not ready
 
void panHeading(int first_col, int first_line){
osd.setPanel(first_col, first_line);
osd.openPanel();
osd.printf("%4.0f%c", (double)osd_heading, 0xb0);
osd.closePanel();
}
 
/* **************************************************************** */
// Panel : panRose
// Needs : X, Y locations
// Output : a dynamic compass rose that changes along the heading information
// Size : 2 x 13 (rows x chars)
// Staus : done
 
void panRose(int first_col, int first_line){
osd.setPanel(first_col, first_line);
osd.openPanel();
//osd_heading = osd_yaw;
//if(osd_yaw < 0) osd_heading = 360 + osd_yaw;
osd.printf("%s|%c%s%c", "\x20\xc0\xc0\xc0\xc0\xc0\xc7\xc0\xc0\xc0\xc0\xc0\x20", 0xd0, buf_show, 0xd1);
osd.closePanel();
}
 
 
/* **************************************************************** */
// Panel : panBoot
// Needs : X, Y locations
// Output : Booting up text and empty bar after that
// Size : 1 x 21 (rows x chars)
// Staus : done
 
void panBoot(int first_col, int first_line){
osd.setPanel(first_col, first_line);
osd.openPanel();
osd.printf_P(PSTR("Booting up:\xed\xf2\xf2\xf2\xf2\xf2\xf2\xf2\xf3"));
osd.closePanel();
 
}
 
/* **************************************************************** */
// Panel : panMavBeat
// Needs : X, Y locations
// Output : 2 symbols, one static and one that blinks on every 50th received
// mavlink packet.
// Size : 1 x 2 (rows x chars)
// Staus : done
 
void panMavBeat(int first_col, int first_line){
osd.setPanel(first_col, first_line);
osd.openPanel();
if(mavbeat == 1){
osd.printf_P(PSTR("\xEA\xEC"));
mavbeat = 0;
}
else{
osd.printf_P(PSTR("\xEA\xEB"));
}
osd.closePanel();
}
 
 
/* **************************************************************** */
// Panel : panWPDir
// Needs : X, Y locations
// Output : 2 symbols that are combined as one arrow, shows direction to next waypoint
// Size : 1 x 2 (rows x chars)
// Staus : not ready
 
void panWPDir(int first_col, int first_line){
osd.setPanel(first_col, first_line);
osd.openPanel();
showArrow();
osd.closePanel();
}
 
/* **************************************************************** */
// Panel : panHomeDir
// Needs : X, Y locations
// Output : 2 symbols that are combined as one arrow, shows direction to home
// Size : 1 x 2 (rows x chars)
// Status : not tested
 
void panHomeDir(int first_col, int first_line){
osd.setPanel(first_col, first_line);
osd.openPanel();
showArrow();
osd.closePanel();
}
 
/* **************************************************************** */
// Panel : panFlightMode
// Needs : X, Y locations
// Output : 2 symbols, one static name symbol and another that changes by flight modes
// Size : 1 x 2 (rows x chars)
// Status : done
 
void panFlightMode(int first_col, int first_line){
osd.setPanel(first_col, first_line);
osd.openPanel();
#ifndef MAVLINK10
if(apm_mav_type == 2){//ArduCopter MultiRotor or ArduCopter Heli
if(osd_mode == 100) osd.printf_P(PSTR("\xE0""stab"));//Stabilize
if(osd_mode == 101) osd.printf_P(PSTR("\xE0""acro"));//Acrobatic
if(osd_mode == 102) osd.printf_P(PSTR("\xE0""alth"));//Alt Hold
if(osd_mode == MAV_MODE_AUTO && osd_nav_mode == MAV_NAV_WAYPOINT) osd.printf_P(PSTR("\xE0""auto"));//Auto
if(osd_mode == MAV_MODE_GUIDED && osd_nav_mode == MAV_NAV_WAYPOINT) osd.printf_P(PSTR("\xE0""guid"));//Guided
if(osd_mode == MAV_MODE_AUTO && osd_nav_mode == MAV_NAV_HOLD) osd.printf_P(PSTR("\xE0""loit"));//Loiter
if(osd_mode == MAV_MODE_AUTO && osd_nav_mode == MAV_NAV_RETURNING) osd.printf_P(PSTR("\xE0""retl"));//Return to Launch
if(osd_mode == 107) osd.printf_P(PSTR("\xE0""circ")); // Circle
if(osd_mode == 108) osd.printf_P(PSTR("\xE0""posi")); // Position
if(osd_mode == 109) osd.printf_P(PSTR("\xE0""land")); // Land
if(osd_mode == 110) osd.printf_P(PSTR("\xE0""oflo")); // OF_Loiter
}
else if(apm_mav_type == 1){//ArduPlane
if(osd_mode == MAV_MODE_TEST1 && osd_nav_mode == MAV_NAV_VECTOR) osd.printf_P(PSTR("\xE0""stab"));//Stabilize
if(osd_mode == MAV_MODE_MANUAL && osd_nav_mode == MAV_NAV_VECTOR) osd.printf_P(PSTR("\xE0""manu"));//Manual
if(osd_mode == MAV_MODE_AUTO && osd_nav_mode == MAV_NAV_LOITER) osd.printf_P(PSTR("\xE0""loit"));//Loiter
if(osd_mode == MAV_MODE_AUTO && osd_nav_mode == MAV_NAV_RETURNING) osd.printf_P(PSTR("\xE0""retl"));//Return to Launch
if(osd_mode == MAV_MODE_TEST2 && osd_nav_mode == 1) osd.printf_P(PSTR("\xE0""fbwa"));//FLY_BY_WIRE_A
if(osd_mode == MAV_MODE_TEST2 && osd_nav_mode == 2) osd.printf_P(PSTR("\xE0""fbwb"));//FLY_BY_WIRE_B
if(osd_mode == MAV_MODE_GUIDED) osd.printf_P(PSTR("\xE0""guid"));//GUIDED
if(osd_mode == MAV_MODE_AUTO && osd_nav_mode == MAV_NAV_WAYPOINT) osd.printf_P(PSTR("\xE0""auto"));//AUTO
if(osd_mode == MAV_MODE_TEST3) osd.printf_P(PSTR("\xE0""circ"));//CIRCLE
}
#else
if(apm_mav_type == 2){//ArduCopter MultiRotor or ArduCopter Heli
if(osd_mode == 0) osd.printf_P(PSTR("\xE0""stab"));//Stabilize
if(osd_mode == 1) osd.printf_P(PSTR("\xE0""acro"));//Acrobatic
if(osd_mode == 2) osd.printf_P(PSTR("\xE0""alth"));//Alt Hold
if(osd_mode == 3) osd.printf_P(PSTR("\xE0""auto"));//Auto
if(osd_mode == 4) osd.printf_P(PSTR("\xE0""guid"));//Guided
if(osd_mode == 5) osd.printf_P(PSTR("\xE0""loit"));//Loiter
if(osd_mode == 6) osd.printf_P(PSTR("\xE0""retl"));//Return to Launch
if(osd_mode == 7) osd.printf_P(PSTR("\xE0""circ")); // Circle
if(osd_mode == 8) osd.printf_P(PSTR("\xE0""posi")); // Position
if(osd_mode == 9) osd.printf_P(PSTR("\xE0""land")); // Land
if(osd_mode == 10) osd.printf_P(PSTR("\xE0""oflo")); // OF_Loiter
}
else if(apm_mav_type == 1){//ArduPlane
if(osd_mode == 2 ) osd.printf_P(PSTR("\xE0""stab"));//Stabilize
if(osd_mode == 0) osd.printf_P(PSTR("\xE0""manu"));//Manual
if(osd_mode == 12) osd.printf_P(PSTR("\xE0""loit"));//Loiter
if(osd_mode == 11 ) osd.printf_P(PSTR("\xE0""retl"));//Return to Launch
if(osd_mode == 5 ) osd.printf_P(PSTR("\xE0""fbwa"));//FLY_BY_WIRE_A
if(osd_mode == 6 ) osd.printf_P(PSTR("\xE0""fbwb"));//FLY_BY_WIRE_B
if(osd_mode == 15) osd.printf_P(PSTR("\xE0""guid"));//GUIDED
if(osd_mode == 10 ) osd.printf_P(PSTR("\xE0""auto"));//AUTO
if(osd_mode == 1) osd.printf_P(PSTR("\xE0""circ"));//CIRCLE
}
#endif
osd.closePanel();
}
 
 
// ---------------- EXTRA FUNCTIONS ----------------------
// Show those fancy 2 char arrows
void showArrow() {
switch(osd_home_direction) {
case 0:
osd.printf_P(PSTR("\x90\x91"));
break;
case 1:
osd.printf_P(PSTR("\x90\x91"));
break;
case 2:
osd.printf_P(PSTR("\x92\x93"));
break;
case 3:
osd.printf_P(PSTR("\x94\x95"));
break;
case 4:
osd.printf_P(PSTR("\x96\x97"));
break;
case 5:
osd.printf_P(PSTR("\x98\x99"));
break;
case 6:
osd.printf_P(PSTR("\x9A\x9B"));
break;
case 7:
osd.printf_P(PSTR("\x9C\x9D"));
break;
case 8:
osd.printf_P(PSTR("\x9E\x9F"));
break;
case 9:
osd.printf_P(PSTR("\xA0\xA1"));
break;
case 10:
osd.printf_P(PSTR("\xA2\xA3"));
break;
case 11:
osd.printf_P(PSTR("\xA4\xA5"));
break;
case 12:
osd.printf_P(PSTR("\xA6\xA7"));
break;
case 13:
osd.printf_P(PSTR("\xA8\xA9"));
break;
case 14:
osd.printf_P(PSTR("\xAA\xAB"));
break;
case 15:
osd.printf_P(PSTR("\xAC\xAD"));
break;
case 16:
osd.printf_P(PSTR("\xAE\xAF"));
break;
}
}
 
// Calculate and shows Artificial Horizon
void showHorizon(int start_col, int start_row) {
 
int x, nose, row, minval, hit, subval = 0;
int cols = 12;
int rows = 5;
int col_hit[cols];
float pitch, roll;
(abs(osd_pitch) == 90)?pitch = 89.99 * (90/osd_pitch) * -0.017453293:pitch = osd_pitch * -0.017453293;
(abs(osd_roll) == 90)?roll = 89.99 * (90/osd_roll) * 0.017453293:roll = osd_roll * 0.017453293;
 
nose = round(tan(pitch) * (rows*9));
for(int col=1;col <= cols;col++){
x = (col * 12) - (cols * 6) - 6;//center X point at middle of each col
col_hit[col-1] = (tan(roll) * x) + nose + (rows*9) - 1;//calculating hit point on Y plus offset to eliminate negative values
//col_hit[(col-1)] = nose + (rows * 9);
}
 
for(int col=0;col < cols; col++){
hit = col_hit[col];
if(hit > 0 && hit < (rows * 18)){
row = rows - ((hit-1)/18);
minval = rows*18 - row*18 + 1;
subval = hit - minval;
subval = round((subval*9)/18);
if(subval == 0) subval = 1;
printHit(start_col + col, start_row + row - 1, subval);
}
}
}
 
void printHit(byte col, byte row, byte subval){
osd.openSingle(col, row);
switch (subval){
case 1:
osd.printf_P(PSTR("\x06"));
break;
case 2:
osd.printf_P(PSTR("\x07"));
break;
case 3:
osd.printf_P(PSTR("\x08"));
break;
case 4:
osd.printf_P(PSTR("\x09"));
break;
case 5:
osd.printf_P(PSTR("\x0a"));
break;
case 6:
osd.printf_P(PSTR("\x0b"));
break;
case 7:
osd.printf_P(PSTR("\x0c"));
break;
case 8:
osd.printf_P(PSTR("\x0d"));
break;
case 9:
osd.printf_P(PSTR("\x0e"));
break;
}
}
/C-OSD/arducam-osd/ArduCAM_OSD/OSD_Vars.h
0,0 → 1,88
/*Panels variables*/
//Will come from APM telem port
 
static float osd_vbat_A = 0; // Battery A voltage in milivolt
//static float osd_curr_A = 0; // Battery A current
static uint16_t osd_battery_remaining_A = 0; // 0 to 100 <=> 0 to 1000
static uint8_t osd_battery_pic_A = 0xb4; // picture to show battery remaining
//static float osd_vbat_B = 0; // voltage in milivolt
//static float osd_curr_B = 0; // Battery B current
//static uint16_t osd_battery_remaining_B = 0; // 0 to 100 <=> 0 to 1000
//static uint8_t osd_battery_pic_B = 0xb4; // picture to show battery remaining
 
static uint16_t osd_mode = 0; // Navigation mode from RC AC2 = CH5, APM = CH8
static uint8_t osd_nav_mode = 0; // Navigation mode from RC AC2 = CH5, APM = CH8
 
static float osd_lat = 0; // latidude
static float osd_lon = 0; // longitude
static uint8_t osd_satellites_visible = 0; // number of satelites
static uint8_t osd_fix_type = 0; // GPS lock 0-1=no fix, 2=2D, 3=3D
 
static uint8_t osd_got_home = 0; // tels if got home position or not
static float osd_home_lat = 0; // home latidude
static float osd_home_lon = 0; // home longitude
static float osd_home_alt = 0;
static long osd_home_distance = 0; // distance from home
static uint8_t osd_home_direction; // Arrow direction pointing to home (1-16 to CW loop)
 
static int8_t osd_pitch = 0; // pitch form DCM
static int8_t osd_roll = 0; // roll form DCM
static int8_t osd_yaw = 0; // relative heading form DCM
static float osd_heading = 0; // ground course heading from GPS
static float osd_alt = 0; // altitude
static float osd_groundspeed = 0; // ground speed
static uint16_t osd_throttle = 0; // throtle
 
//MAVLink session control
static boolean mavbeat = 0;
static float lastMAVBeat = 0;
static boolean waitingMAVBeats = 1;
static uint8_t apm_mav_type;
static uint8_t apm_mav_system;
static uint8_t apm_mav_component;
static boolean enable_mav_request = 0;
 
// Panel BIT registers
byte panA_REG = 0b00000000;
byte panB_REG = 0b00000000;
byte panC_REG = 0b00000000;
 
byte modeScreen = 0; //NTSC:0, PAL:1
 
byte SerCMD1 = 0;
byte SerCMD2 = 0;
 
 
int tempvar; // Temporary variable used on many places around the OSD
 
// First 8 panels and their X,Y coordinate holders
byte panCenter_XY[2]; // = { 13,7,0 };
byte panPitch_XY[2]; // = { 11,1 };
byte panRoll_XY[2]; // = { 23,7 };
byte panBatt_A_XY[2]; // = { 23,1 };
//byte panBatt_B_XY[2]; // = { 23,3 };
byte panGPSats_XY[2]; // = { 2,12 };
byte panGPL_XY[2]; // = { 2,11 };
byte panGPS_XY[2]; // = { 2,13 };
 
// Second 8 set of panels and their X,Y coordinate holders
byte panRose_XY[2]; // = { 16,13 };
byte panHeading_XY[2]; // = { 16,12 };
byte panMavBeat_XY[2]; // = { 2,10 };
byte panHomeDir_XY[2]; // = { 0,0 };
byte panHomeDis_XY[2]; // = { 0,0 };
byte panWPDir_XY[2]; // = { 0,0 };
byte panWPDis_XY[2]; // = { 0,0 };
//byte panRSSI_XY[2] = { 0,0 };
 
// Third set of panels and their X,Y coordinate holders
//byte panCur_A_XY[2]; // = { 23,1 };
//byte panCur_B_XY[2]; // = { 23,3 };
byte panAlt_XY[2]; // = { 0,0 };
byte panVel_XY[2]; // = { 0,0 };
byte panThr_XY[2]; // = { 0,0 };
byte panFMod_XY[2]; // = { 0,0 };
byte panHorizon_XY[2]; // = {8,centercalc}
//byte panXXX_XY[2] = { 0,0 };
 
 
/C-OSD/arducam-osd/ArduCAM_OSD/Spi.cpp
0,0 → 1,56
// Get the common arduino functions
#if defined(ARDUINO) && ARDUINO >= 100
#include "Arduino.h"
#else
#include "wiring.h"
#endif
#include "Spi.h"
 
//---------- constructor ----------------------------------------------------
 
SPI::SPI()
{
// initialize the SPI pins
pinMode(SCK_PIN, OUTPUT);
pinMode(MOSI_PIN, OUTPUT);
pinMode(MISO_PIN, INPUT);
pinMode(SS_PIN, OUTPUT); // <------- !!! (Remember! This pin will select USB host chip Max3421)
 
// enable SPI Master, MSB, SPI mode 0, FOSC/4
mode(0);
}
 
//------------------ mode ---------------------------------------------------
 
void SPI::mode(byte config)
{
byte tmp;
 
// enable SPI master with configuration byte specified
SPCR = 0;
SPCR = (config & 0x7F) | (1<<SPE) | (1<<MSTR);
tmp = SPSR;
tmp = SPDR;
}
 
//------------------ transfer -----------------------------------------------
 
byte SPI::transfer(byte value)
{
SPDR = value;
while (!(SPSR & (1<<SPIF))) ;
return SPDR;
}
 
byte SPI::transfer(byte value, byte period)
{
SPDR = value;
if (period > 0) delayMicroseconds(period);
while (!(SPSR & (1<<SPIF))) ;
return SPDR;
}
 
 
//---------- preinstantiate SPI object --------------------------------------
 
SPI Spi = SPI();
/C-OSD/arducam-osd/ArduCAM_OSD/Spi.h
0,0 → 1,27
#ifndef Spi_h
#define Spi_h
 
// Get the common arduino functions
#if defined(ARDUINO) && ARDUINO >= 100
#include "Arduino.h"
#else
#include "wiring.h"
#endif
 
#define SCK_PIN 13
#define MISO_PIN 12
#define MOSI_PIN 11
#define SS_PIN 10 // <------- !!! (Remember! This pin will select USB host chip Max3421)
 
class SPI
{
public:
SPI(void);
void mode(byte);
byte transfer(byte);
byte transfer(byte, byte);
};
 
extern SPI Spi;
 
#endif