zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
31302 Commits
23 Branches
0 Tags
282 MiB
 
 
 
 
 
 
Tree: ac99d37bc1
Branches Tags
${ item.name }
Create tag ${ searchTerm }
Create branch ${ searchTerm }
from 'ac99d37bc1'
${ noResults }
ardupilot/libraries/AP_HAL_F4Light/hardware/osd/osd.cpp

1165 lines
38 KiB
Raw Blame History

/*
wrapper for OSD code (https://github.com/night-ghost/minimosd-extra) to run in the HAL as independent process
(c) night_ghost@ykoctpa.ru 2017
*/
#include <AP_HAL/AP_HAL.h>
#ifdef BOARD_OSD_CS_PIN
#include <utility>
#include "osd_core/compat.h"
using namespace F4Light;
#include <AP_Common/AP_Common.h>
#include <stdio.h>
#include <hal.h>
#include "ring_buffer.h"
#include <inttypes.h>
#include <AP_HAL/AP_HAL.h>
#include <AP_HAL/utility/print_vprintf.h>
#include <AP_HAL_F4Light/AP_HAL_F4Light.h>
#include <AP_HAL_F4Light/SPIDevice.h>
#define SLAVE_BUILD
#include "osd_core/Defs.h"
#include "osd.h"
#include "osd_eeprom.h"
#include "osd_core/eeprom.h"
#include "osd_core/version.h"
#include <sd/SD.h>
namespace OSDns {
#include "osd_core/GCS_MAVLink.h"
#include "osd_core/OSD_Max7456.h"
OSD osd; //OSD object
#include "osd_core/prototypes.h"
#include "osd_core/Vars.h"
#include "osd_core/Config_Func.h"
#include "osd_core/Config.h"
#include "osd_core/Func.h"
#include "osd_core/protocols.h"
#include "osd_core/misc.h"
#include "osd_core/Params.h"
#include "osd_core/Panels.h"
// TODO: чтение конфига и еепром с карты памяти, чтобы закинуть .mcm и .osd и все
static const char * const words[] = {
"Air Speed", // 1
"Altitude", // 2
"Auto Mode", // 3
"Auto Screen Switch", // 4
"batt_a_k", // 5
"BattB", // 6
"batt_b_k", // 7
"Battery", // 8
"Battery A", // 9
"Battery B", // 10
"Battery Percent", // 11
"Battery Warning Level", // 12
"Call Sign", // 13
"Chanel Rotation Switching", // 14
"Channel Raw", // 15
"Channel Scale", // 16
"Channel state", // 17
"Channel Value", // 18
"Configuration", // 19
"Current", // 20
"curr_k", // 21
"Efficiency", // 22
"fBattA", // 23
"fBattB", // 24
"fCurr", // 25
"fILS", // 26
"flgHUD", // 27
"flgOnce", // 28
"flgTrack", // 29
"Flight Data", // 30
"Flight Mode", // 31
"fRussianHUD", // 32
"GPS Coord", // 33
"GPS HDOP", // 34
"Heading", // 35
"Heading Rose", // 36
"Home Altitude", // 37
"Home Direction", // 38
"Home Distance", // 39
"Horizon", // 40
"HOS", // 41
"Message", // 42
"Model Type", // 43
"NSCREENS", // 44
"OSD Brightness", // 45
"Overspeed", // 46
"Panel", // 47
"Pitch", // 48
"pitch_k", // 49
"pitch_kn", // 50
"PWMDST", // 51
"PWMSRC", // 52
"Radar Scale", // 53
"Real heading", // 54
"Roll", // 55
"roll_k", // 56
"roll_kn", // 57
"RSSI", // 58
"RSSI Enable Raw", // 59
"RSSI High", // 60
"rssi_k", // 61
"RSSI Low", // 62
"RSSI Warning Level", // 63
"SAdd1", // 64
"SAdd2", // 65
"SAdd3", // 66
"SAdd4", // 67
"Sensor 1", // 68
"Sensor 2", // 69
"Sensor 3", // 70
"Sensor 4", // 71
"SFactor1", // 72
"SFactor2", // 73
"SFactor3", // 74
"SFactor4", // 75
"SFormat1", // 76
"SFormat2", // 77
"SFormat3", // 78
"SFormat4", // 79
"Stall", // 80
"Temperature", // 81
"Throttle", // 82
"Time", // 83
"Toggle Channel", // 84
"Trip Distance", // 85
"Tune", // 86
"txtTime0", // 87
"txtTime1", // 88
"txtTime2", // 89
"txtTime3", // 90
"Units", // 91
"Velocity", // 92
"Vertical Speed", // 93
"Video Mode", // 94
"Visible Sats", // 95
"VOS", // 96
"Warnings", // 97
"Wind Speed", // 98
"WP Direction", // 99
"WP Distance", // 100
"#", // 101
"Power", // 102
"Date", // 103
"Time of day", // 104
"Motors", // 105
"Vibrations", // 106
"Variometer", // 107
"GPS Coord Lat", // 108
"GPS Coord Lon", // 109
};
typedef struct OSD_PAN {
uint8_t dst;
uint8_t size;
char fmt;
uint8_t pan_n;
} OSD_pan;
#define m1 ((uint8_t)-1)
static const OSD_pan pan_tbl[]={
{ 0, 0, 0, 0, },
{ 0, 0, 0, ID_of(airSpeed), }, // "Air Speed", // 1
{ 0, 0, 0, ID_of(alt), }, // "Altitude", // 2
{ 9, m1, 0, 0, }, // "Auto Mode", // 3 - bit in flags
{ 7, m1, 0, 0, }, // "Auto Screen Switch", // 4
{ offsetof(Settings,evBattA_koef), sizeof(sets.evBattA_koef), 'f', 0, }, // "batt_a_k",
{ offsetof(Settings,battBv), sizeof(sets.battBv), 'b', 0, }, // "BattB",
{ offsetof(Settings,evBattB_koef), sizeof(sets.evBattB_koef), 'f', 0, }, // "batt_b_k",
{ offsetof(Settings,battv), sizeof(sets.battv), 'b', 0, }, // "Battery",
{ 0, 0, 0, ID_of(batt_A), }, // "Battery A", // 9
{ 0, 0, 0, ID_of(batt_B), }, // "Battery B", // 10
{ 0, 0, 0, ID_of(batteryPercent), },// "Battery Percent", // 11
{ offsetof(Settings,batt_warn_level), sizeof(sets.batt_warn_level),'b', 0, }, // "Battery Warning Level", // 12
{ offsetof(Settings,OSD_CALL_SIGN), sizeof(sets.OSD_CALL_SIGN), 'c', 0, }, // "Call Sign", // 13
{ offsetof(Settings,switch_mode), sizeof(sets.switch_mode), 'b', 0, }, // "Chanel Rotation Switching", // 14
{ 0, 0, 0, ID_of(ch), }, // "Channel Raw", // 15
{ 0, 0, 0, ID_of(Scale), }, // "Channel Scale", // 16
{ 0, 0, 0, ID_of(State), }, // "Channel state", // 17
{ 0, 0, 0, ID_of(CValue), }, // "Channel Value", // 18
{ 0, 0, 0, 0, }, // "Configuration", // 19
{ 0, 0, 0, ID_of(curr_A), }, // "Current", // 20
{ offsetof(Settings,eCurrent_koef), sizeof(sets.eCurrent_koef), 'f', 0, }, // "curr_k", // 21
{ 0, 0, 0, ID_of(eff), }, // "Efficiency", // 22
{ 4, m1, 0, 0, }, // "fBattA", // 23
{ 5, m1, 0, 0, }, // "fBattB", // 24
{ 6, m1, 0, 0, }, // "fCurr", // 25
{ 0, 0, 0, 0, }, // 26
{ 0, 0, 0, 0, }, // 27
{ 0, m1, 0, 0, }, // "flgOnce", // 28
{ 0, 0, 0, 0, }, //29
{ 0, 0, 0, ID_of(Fdata), }, // "Flight Data", // 30
{ 0, 0, 0, ID_of(FMod), }, // "Flight Mode", // 31
{ 0, 0, 0, 0, }, // "fRussianHUD", // 32
{ 0, 0, 0, ID_of(GPS), }, // "GPS Coord", // 33
{ 0, 0, 0, ID_of(Hdop), }, // "GPS HDOP", // 34
{ 0, 0, 0, ID_of(heading), }, // "Heading", // 35
{ 0, 0, 0, ID_of(rose), }, // "Heading Rose", // 36
{ 0, 0, 0, ID_of(homeAlt), }, // "Home Altitude", // 37
{ 0, 0, 0, ID_of(homeDir), }, // "Home Direction", // 38
{ 0, 0, 0, ID_of(homeDist), }, // "Home Distance", // 39
{ 0, 0, 0, ID_of(horizon), }, // "Horizon", // 40
{ offsetof(Settings,horiz_offs), sizeof(sets.horiz_offs), 'b', 0, }, // "HOS", // 41
{ 0, 0, 0, ID_of(message), }, // "Message", // 42
{ offsetof(Settings,model_type), sizeof(sets.model_type), 'b', 0, }, // "Model Type", // 43
{ offsetof(Settings,n_screens), sizeof(sets.n_screens), 'b', 0, }, // "NSCREENS", // 44
{ offsetof(Settings,OSD_BRIGHTNESS), sizeof(sets.OSD_BRIGHTNESS),'b', 0, }, // "OSD Brightness", // 45
{ offsetof(Settings,overspeed), sizeof(sets.overspeed), 'b', 0, }, // "Overspeed", // 46
{ 0, 0, 0, 0, }, // "Panel", // 47
{ 0, 0, 0, ID_of(pitch), }, // "Pitch", // 48
{ offsetof(Settings,horiz_kPitch), sizeof(sets.horiz_kPitch), 'f', 0, }, // "pitch_k", // 49
{ offsetof(Settings,horiz_kPitch_a), sizeof(sets.horiz_kPitch_a),'f', 0, }, // "pitch_kn", // 50
{ offsetof(Settings,pwm_dst), sizeof(sets.pwm_dst), 'b', 0, }, // "PWMDST", // 51
{ offsetof(Settings,pwm_src), sizeof(sets.pwm_src), 'b', 0, }, // "PWMSRC", // 52
{ 0, 0, 0, ID_of(RadarScale), }, // "Radar Scale", // 53
{ 0, 0, 0, ID_of(COG), }, // "Real heading", // 54
{ 0, 0, 0, ID_of(roll), }, // "Roll", // 55
{ offsetof(Settings,horiz_kRoll), sizeof(sets.horiz_kRoll), 'f', 0, }, // "roll_k", // 56
{ offsetof(Settings,horiz_kRoll_a), sizeof(sets.horiz_kRoll_a), 'f', 0, }, // "roll_kn", // 57
{ 0, 0, 0, ID_of(RSSI), }, // "RSSI", // 58
{ offsetof(Settings,RSSI_raw), sizeof(sets.RSSI_raw), 'b', 0, }, // "RSSI Enable Raw", // 59
{ offsetof(Settings,RSSI_16_high), sizeof(sets.RSSI_16_high), 'w', 0, }, // "RSSI High", // 60
{ offsetof(Settings,eRSSI_koef), sizeof(sets.eRSSI_koef), 'f', 0, }, // "rssi_k", // 61
{ offsetof(Settings,RSSI_16_low), sizeof(sets.RSSI_16_low), 'w', 0, }, // "RSSI Low", // 62
{ offsetof(Settings,rssi_warn_level),sizeof(sets.rssi_warn_level),'b', 0, }, // "RSSI Warning Level", // 63
{ 0, 0, 0, 0, }, // "SAdd1", // 64 // sensors not supported
{ 0, 0, 0, 0, }, // "SAdd2", // 65
{ 0, 0, 0, 0, }, // "SAdd3", // 66
{ 0, 0, 0, 0, }, // "SAdd4", // 67
{ 0, 0, 0, ID_of(sensor1), }, // "Sensor 1", // 68
{ 0, 0, 0, ID_of(sensor2), }, // "Sensor 2", // 69
{ 0, 0, 0, ID_of(sensor3), }, // "Sensor 3", // 70
{ 0, 0, 0, ID_of(sensor4), }, // "Sensor 4", // 71
{ 0, 0, 0, 0, }, // "SFactor1", // 72
{ 0, 0, 0, 0, }, // "SFactor2", // 73
{ 0, 0, 0, 0, }, // "SFactor3", // 74
{ 0, 0, 0, 0, }, // "SFactor4", // 75
{ 0, 0, 0, 0, }, // "SFormat1", // 76
{ 0, 0, 0, 0, }, // "SFormat2", // 77
{ 0, 0, 0, 0, }, // "SFormat3", // 78
{ 0, 0, 0, 0, }, // "SFormat4", // 79
{ offsetof(Settings,stall), sizeof(sets.stall), 'b', 0, }, // "Stall", // 80
{ 0, 0, 0, ID_of(temp), }, // "Temperature", // 81
{ 0, 0, 0, ID_of(throttle), }, // "Throttle", // 82
{ 0, 0, 0, ID_of(time), }, // "Time", // 83
{ offsetof(Settings,ch_toggle), sizeof(sets.ch_toggle), 'b', 0, }, // "Toggle Channel", // 84
{ 0, 0, 0, ID_of(distance), }, // "Trip Distance", // 85
{ 0, 0, 0, ID_of(tune), }, // "Tune", // 86
{ 0, 0, 0, 0, }, // "txtTime0", // 87
{ 0, 0, 0, 0, }, // "txtTime1", // 88
{ 0, 0, 0, 0, }, // "txtTime2", // 89
{ 0, 0, 0, 0, }, // "txtTime3", // 90
{ 1, m1, 0, 0, }, // "Units", // 91
{ 0, 0, 0, ID_of(vel), }, // "Velocity", // 92
{ 0, 0, 0, ID_of(climb), }, // "Vertical Speed", // 93
{ 3, m1, 0, 0, }, // "Video Mode", // 94
{ 0, 0, 0, ID_of(GPS_sats), }, // "Visible Sats", // 95
{ offsetof(Settings,vert_offs), sizeof(sets.vert_offs), 'b', 0, }, // "VOS", // 96
{ 0, 0, 0, ID_of(warn), }, // "Warnings", // 97
{ 0, 0, 0, ID_of(windSpeed), }, // "Wind Speed", // 98
{ 0, 0, 0, ID_of(WP_dir), }, // "WP Direction", // 99
{ 0, 0, 0, ID_of(WP_dist), }, // "WP Distance", // 100
{ 0, 0, 0, 0, }, // #
{ 0, 0, 0, ID_of(Power), }, // "Power", // 102
{ 0, 0, 0, ID_of(fDate), }, // "Date", // 103
{ 0, 0, 0, ID_of(dayTime), }, // "Time of day", // 104
{ 0, 0, 0, ID_of(pMotor), }, // "Motors", // 105
{ 0, 0, 0, ID_of(fVibe), }, // "Vibrations", // 106
{ 0, 0, 0, ID_of(fVario), }, // "Variometer", // 107
{ 0, 0, 0, ID_of(coordLat), }, // "GPS Coord Lat", // 108
{ 0, 0, 0, ID_of(coordLon), }, // "GPS Coord Lon", // 109
};
static ring_buffer osd_rxrb IN_CCM;
static uint8_t osd_rx_buf[OSD_RX_BUF_SIZE] IN_CCM;
static ring_buffer osd_txrb IN_CCM;
static uint8_t osd_tx_buf[OSD_TX_BUF_SIZE] IN_CCM;
AP_HAL::OwnPtr<F4Light::SPIDevice> osd_spi;
AP_HAL::Semaphore *osd_spi_sem;
//static volatile byte vas_vsync=false;
mavlink_system_t mavlink_system = {12,1}; // sysid, compid
#ifdef OSD_DMA_TRANSFER
#define DMA_BUFFER_SIZE 510
static uint8_t dma_buffer[DMA_BUFFER_SIZE+1]; // in RAM for DMA
static uint16_t dma_transfer_length IN_CCM;
static uint8_t shadowbuf[sizeof(OSD::osdbuf)] IN_CCM;
#endif
static bool diff_done;
extern void heartBeat();
extern void writePanels();
void On100ms() {}
void On20ms() {}
void osd_loop();
void vsync_ISR();
void max_do_transfer(const char *buffer, uint16_t len);
static void max7456_cs_off(){
osd_spi->wait_busy(); // wait for transfer complete
const stm32_pin_info &pp = PIN_MAP[BOARD_OSD_CS_PIN];
gpio_write_bit(pp.gpio_device, pp.gpio_bit, HIGH);
delay_ns100(3);
}
static void max7456_cs_on(){
const stm32_pin_info &pp = PIN_MAP[BOARD_OSD_CS_PIN];
gpio_write_bit(pp.gpio_device, pp.gpio_bit, LOW);
delay_ns100(1);
}
static uint32_t sem_count=0;
void max7456_on(){
max7456_cs_on();
osd_spi->set_speed(AP_HAL::Device::SPEED_HIGH);
}
static void max7456_sem_on(){
if(osd_spi_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
max7456_on();
}
}
void max7456_off(){
max7456_cs_off();
}
static void max7456_sem_off(){
max7456_off();
osd_spi_sem->give(); // give sem on last count
}
void MAX_write(byte addr, byte data){
max7456_cs_on();
osd_spi->transfer(addr); // this transfer don't controls CS
osd_spi->transfer(data);
max7456_cs_off();
}
byte MAX_read(byte addr){
max7456_cs_on();
osd_spi->transfer(addr); // this transfer don't controls CS
uint8_t ret = osd_spi->transfer(0xff);
max7456_cs_off();
return ret;
}
byte MAX_rw(byte b){
max7456_cs_on();
uint8_t ret=osd_spi->transfer(b);
max7456_cs_off();
return ret;
}
static uint16_t rdb_ptr IN_CCM;
#ifdef OSD_DMA_TRANSFER
static void prepare_dma_buffer(){
uint16_t rp;
uint16_t wp=0;
uint8_t last_h=0xff;
// MAX_write(MAX7456_DMM_reg, 0);
// MAX_write(MAX7456_VM1_reg, B01000111);
memset(dma_buffer,0xff,sizeof(dma_buffer));
dma_buffer[wp++] = MAX7456_DMM_reg; dma_buffer[wp++] = 0;
dma_buffer[wp++] = MAX7456_VM1_reg; dma_buffer[wp++] = B01000111;
// сначала все изменения
for(rp=0; rp<MAX7456_screen_size ; rp++){
uint8_t c = OSD::osdbuf[rp];
if(c != shadowbuf[rp] ){
if(wp>=DMA_BUFFER_SIZE-6) break;
uint8_t h = rp>>8;
if(last_h != h){
last_h = h;
dma_buffer[wp++] = MAX7456_DMAH_reg; dma_buffer[wp++] = h;
if(wp>=DMA_BUFFER_SIZE-6) break;
}
dma_buffer[wp++] = MAX7456_DMAL_reg; dma_buffer[wp++] = rp&0xFF;
dma_buffer[wp++] = MAX7456_DMDI_reg; dma_buffer[wp++] = c;
shadowbuf[rp] = c;
}
}
// а в оставшееся место все остальное по кольцу. таким образом пересылка у нас всегда 500 байт, и на частоте 4.5МГц занимает ~1ms.
// длинные пересылки имеют низкий приоритет, и никому не мешают
while(wp<DMA_BUFFER_SIZE-6){
uint8_t c = OSD::osdbuf[rdb_ptr];
uint8_t h = rdb_ptr>>8;
if(last_h != h){
last_h = h;
dma_buffer[wp++] = MAX7456_DMAH_reg; dma_buffer[wp++] = h;
if(wp>=DMA_BUFFER_SIZE-6) break;
}
dma_buffer[wp++] = MAX7456_DMAL_reg; dma_buffer[wp++] = rdb_ptr&0xFF;
dma_buffer[wp++] = MAX7456_DMDI_reg; dma_buffer[wp++] = c;
shadowbuf[rdb_ptr] = c;
rdb_ptr++;
if(rdb_ptr >= MAX7456_screen_size) rdb_ptr=0; // loop
}
// dma_buffer[wp++] = MAX7456_VM0_reg; dma_buffer[wp++] = MAX7456_ENABLE_display | MAX7456_SYNC_autosync | OSD::video_mode;
dma_transfer_length = wp;
diff_done = true;
}
#endif
uint32_t get_word(char *buf, char * &ptr){
uint32_t sel_len=0;
uint8_t sel_id=0;
for(uint32_t i=0; i<ARRAY_SIZE(words); i++){
uint32_t len=strlen(words[i]);
if(strncmp(buf, words[i],len)==0){
if(len > sel_len) { // longest match
sel_len = len;
sel_id = i+1;
}
}
}
ptr=buf+sel_len;
return sel_id;
}
char * get_lex(char * &ptro){
char *ptr;
char *buf = ptro;
while(*buf && (*buf=='\t' || *buf == ' ')) buf++;
ptr=buf;
while(*ptr && *ptr!='\t') ptr++;
if(*ptr==0) {
ptro=NULL;
} else {
*ptr=0;
ptro=ptr+1;
}
return buf;
}
static bool get_flag(char *p) {
if(!p) return false;
if(*p=='T' || *p=='t' || *p=='1') return true;
return false;
}
// x, y, vis, sign, Altf, Alt2, Alt3, Alt4, strings
// 30 15 False 0 1 1 1 1 A||||
static point create_point(char *px, char *py, char *pVis, char *pSign, char *pAlt, char *pAlt2, char *pAlt3, char *pAlt4, char *ps ){
point p;
p.x = strtoul(px, nullptr, 10);
p.y = strtoul(py, nullptr, 10);
p = do_on(p, get_flag(pVis));
p = do_sign(p, get_flag(pSign));
if(get_flag(pAlt)) p=do_alt(p);
if(get_flag(pAlt2)) p=do_alt2(p);
if(get_flag(pAlt3)) p=do_alt3(p);
if(get_flag(pAlt4)) p=do_alt4(p);
// if(ps) collect_strings(ps); not supported
return p;
}
#define write_point(n,p) eeprom_write_len((byte *)&p, OffsetBITpanel * (int)panel_num + n * sizeof(Point), sizeof(Point) );
static void load_config(){
File fd = SD.open("eeprom.osd", FILE_READ);
if (fd) {
printf("\nLoading OSD config\n");
char buf[80];
// memset(buf, 0, sizeof(buf));
uint32_t panel_num=-1;
bool is_config=false;
while(fd.gets(buf, sizeof(buf)-1) > 0) {
// we readed one line
char *ptr;
char *p[10];
uint8_t word=get_word(buf,ptr);
switch(word) {
case 0: // not found
case 101: // #
continue;
case 47: { // panel
char *p2 = get_lex(ptr);
panel_num=strtoul(p2, nullptr, 10);
uint16_t flags=strtoul(ptr, nullptr, 10);
write_point(0,flags);
}break;
case 19: // config
is_config=true;
break;
default:
char **pp = p;
memset(p,0,sizeof(p));
do {
*pp++ = get_lex(ptr);
}while(ptr);
if(is_config){
if(pan_tbl[word].size == (uint8_t)-1){ // bit flags
uint32_t flags = sets.flags.dw;
if(get_flag(p[0])) flags |= (1<<pan_tbl[word].dst);
else flags &= ~(1<<pan_tbl[word].dst);
sets.flags.dw=flags;
} else {
union {
float f;
uint8_t b;
char buf[8];
uint16_t w;
} val;
switch(pan_tbl[word].fmt){
case 'f': // float
val.f = atof(p[0]);
break;
case 'b': // byte
val.b=(uint8_t)strtoul(p[0], nullptr, 10);
break;
case 'c': // char
strncpy(val.buf, p[0], 8);
break;
case 'w':
val.w=(uint16_t)strtoul(p[0], nullptr, 10);
break;
default:
continue; // ignore this line
}
memmove( ((uint8_t*)(&sets)) + pan_tbl[word].dst, &val, pan_tbl[word].size);
eeprom_write_len(((uint8_t*)(&sets)) + pan_tbl[word].dst, EEPROM_offs(sets) + pan_tbl[word].dst, pan_tbl[word].size);
}
}else{ // panel
uint8_t id = pan_tbl[word].pan_n;
if(id) {
// 30 15 False 0 1 1 1 1 A||||
point po=create_point(p[0], p[1], p[2], p[3], p[4], p[5], p[6], p[7], p[8] );
write_point(id, po); // save to eeprom
}
}
break;
}
}
fd.close();
// we don't save flags when parsing so lets do it now
eeprom_write_len( &sets.flags.pad[0], EEPROM_offs(sets) + ((byte *)&sets.flags.pad - (byte *)&sets), sizeof(sets.flags.pad));
readSettings(); // re-read new values back, for settings that not in config.osd get all-1 state
sets.CHK1_VERSION = VER; // set version - EEPROM OK
sets.CHK2_VERSION = (VER ^ 0x55);
eeprom_write_len( &sets.CHK1_VERSION, EEPROM_offs(sets) + ((byte *)&sets.CHK1_VERSION - (byte *)&sets), 2 );
}
}
static void load_font(){
const char font[]="font.mcm";
File fd = SD.open(font, FILE_READ);
if (fd) {
char buf[80];
if(fd.gets(buf, sizeof(buf)-1)){
printf("\nLoading OSD font\n");
OSD::setPanel(5, 5);
osd_print_S(PSTR("font uploading "));
OSD::update();// Show it
char patt[]="MAX7456";
if(strncmp(buf,patt,strlen(patt))==0){
uint8_t character_bitmap[0x40];
uint16_t font_count = 0;
byte byte_count = 0;
byte bit_count=0;
uint8_t chk=0;
uint8_t c=0;
uint8_t last_c;
uint8_t b=0;
uint8_t cnt=0;
while(font_count < 256) {
last_c = c;
if(fd.read(&c,1)<=0) break; // get a char
switch(c){ // parse and decode mcm file
case 0x0A: // line feed - skip
if(last_c == 0x0d) continue;
// lf without cr cause line end
case 0x0d: // carridge return, end of line
if (bit_count == 8) {
chk ^= b;
character_bitmap[byte_count] = b;
b = 0;
byte_count++;
}
bit_count = 0;
break;
case 0x30: // ascii '0'
case 0x31: // ascii '1'
b <<= 1;
if(c == 0x31)
b += 1;
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++;
printf(".");
chk=0;
}
}
printf("done\n");
}
}
fd.close();
//* not in debug mode
SD.remove(font); // once!
//*/
}
}
static bool osd_need_redraw = false;
static void * task_handle;
// slowly write all buffer
static void write_buff_to_MAX(bool all){
max7456_on();
MAX_write(MAX7456_DMM_reg, 0);
// clear internal memory
uint8_t old_h=0xff;
for(uint16_t len = MAX7456_screen_size, cnt=0;len--; cnt++){
uint8_t c= OSD::osdbuf[cnt];
if(all || c!=0x20) {
max7456_cs_on();
uint8_t h = cnt>>8;
if(old_h!=h){
MAX_write(MAX7456_DMAH_reg, h);
old_h = h;
}
MAX_write(MAX7456_DMAL_reg, cnt&0xFF);
MAX_write(MAX7456_DMDI_reg, c);
max7456_cs_off();
}
#ifdef OSD_DMA_TRANSFER
shadowbuf[cnt] = c;
#endif
}
max7456_off();
}
void osd_begin(AP_HAL::OwnPtr<F4Light::SPIDevice> spi){
osd_spi = std::move(spi);
osd_spi_sem = osd_spi->get_semaphore(); // bus semaphore
{
const stm32_pin_info &pp = PIN_MAP[BOARD_OSD_CS_PIN];
gpio_set_mode(pp.gpio_device, pp.gpio_bit, GPIO_OUTPUT_PP);
gpio_set_speed(pp.gpio_device, pp.gpio_bit, GPIO_speed_100MHz);
gpio_write_bit(pp.gpio_device, pp.gpio_bit, HIGH);
}
#ifdef BOARD_OSD_RESET_PIN
{
const stm32_pin_info &pp = PIN_MAP[BOARD_OSD_RESET_PIN];
gpio_set_mode(pp.gpio_device, pp.gpio_bit, GPIO_OUTPUT_PP);
gpio_set_speed(pp.gpio_device, pp.gpio_bit, GPIO_speed_25MHz);
gpio_write_bit(pp.gpio_device, pp.gpio_bit, LOW);
delayMicroseconds(50);
gpio_write_bit(pp.gpio_device, pp.gpio_bit, HIGH);
delayMicroseconds(120);
}
#endif
rb_init(&osd_rxrb, OSD_RX_BUF_SIZE, osd_rx_buf);
rb_init(&osd_txrb, OSD_TX_BUF_SIZE, osd_tx_buf);
OSD_EEPROM::init();
// clear memory
memset(OSD::osdbuf,0x20, sizeof(OSD::osdbuf));
#ifdef OSD_DMA_TRANSFER
memset(shadowbuf, 0x20, sizeof(shadowbuf));
#endif
/*
lets try to load settings from SD card
*/
load_config();
readSettings();
doScreenSwitch(); // set vars for startup screen
if( sets.CHK1_VERSION != VER || sets.CHK2_VERSION != (VER ^ 0x55)) { // wrong version
lflags.bad_config=1;
// some useful defaults
sets.OSD_BRIGHTNESS = 2;
sets.horiz_offs = 0x20;
sets.vert_offs = 0x10;
}
while(millis()<1000) { // delay initialization until video stabilizes
hal_yield(1000);
}
max7456_sem_on();
write_buff_to_MAX(true);
for(uint8_t i=0; i<100; i++) {
osd.init(); // Start display
max7456_on();
uint8_t vm0 = MAX_read(MAX7456_VM0_reg | MAX7456_reg_read); // check register
max7456_off();
uint8_t patt = MAX7456_ENABLE_display | MAX7456_SYNC_autosync | OSD::video_mode;
if(vm0==patt) break;
}
max7456_off();
load_font();
max7456_sem_off();
#define REL_1 int(RELEASE_NUM/100)
#define REL_2 int((RELEASE_NUM - REL_1*100 )/10)
#define REL_3 int((RELEASE_NUM - REL_1*100 - REL_2*10 ))
if(sets.FW_VERSION[0]!=(REL_1 + '0') || sets.FW_VERSION[1]!=(REL_2 + '0') || sets.FW_VERSION[2]!=(REL_3 + '0') ){
sets.FW_VERSION[0]=REL_1 + '0';
sets.FW_VERSION[1]=REL_2 + '0';
sets.FW_VERSION[2]=REL_3 + '0';
eeprom_write_len( sets.FW_VERSION, EEPROM_offs(sets) + ((uint8_t *)sets.FW_VERSION - (uint8_t *)&sets), sizeof(sets.FW_VERSION) );
}
logo();
#ifdef BOARD_OSD_VSYNC_PIN
Revo_hal_handler h = { .vp = vsync_ISR };
GPIO::_attach_interrupt(BOARD_OSD_VSYNC_PIN, h.h, RISING, VSI_INT_PRIORITY);
#endif
task_handle = Scheduler::start_task(OSDns::osd_loop, SMALL_TASK_STACK); //
Scheduler::set_task_priority(task_handle, OSD_LOW_PRIORITY); // less than main task
Scheduler::set_task_period(task_handle, 10000); // 100Hz
}
// all task is in one thread so no sync required
void osd_loop() {
if(osd_need_redraw){ // если была отложенная передача
osd_need_redraw=false;
OSD::update();
Scheduler::set_task_priority(task_handle, OSD_LOW_PRIORITY); // restore priority to low
}
uint32_t pt=millis();
seconds = pt / 1000;
osd_dequeue(); // we MUST parse input even in case of bad config because it is the only way to communicate
if(pt < BOOTTIME || lflags.bad_config){ // startup delay for fonts or EEPROM error
logo();
return;
}
#if defined(MAV_REQUEST) && (defined(USE_MAVLINK) || defined(USE_MAVLINKPX4))
if(apm_mav_system && !lflags.mav_request_done){ // we got HEARTBEAT packet and still don't send requests
for(uint8_t n = 3; n >0; n--){
request_mavlink_rates(); //Three times to certify it will be readed
delay_150();
}
lflags.mav_request_done=1;
}
#endif
if(lflags.got_data){ // if new data comes
pan_toggle(); // check for screen toggle
if(!lflags.need_redraw) {
lflags.need_redraw=1;
vsync_wait=1; // will wait for interrupt
}
lflags.got_data=0; // data parsed
}
if( lflags.need_redraw) {
lflags.need_redraw=0; // screen drawn
setHomeVars(); // calculate and set Distance from home and Direction to home
setFdataVars(); // statistics and min/max
writePanels(); // writing enabled panels (check OSD_Panels Tab)
#ifdef OSD_DMA_TRANSFER
prepare_dma_buffer(); // prepare diff with addresses
#endif
update_screen = 1; // data comes, wee need to redraw screen
}
if(pt > timer_20ms){
timer_20ms+=20;
On20ms();
if(update_screen && vsync_wait && (millis() - vsync_time)>50){ // interrupts stopped - more than 50 ms passed from the last one
vsync_wait=0; // хватит ждать
Scheduler::set_task_priority(task_handle, OSD_HIGH_PRIORITY); // equal to main
OSD::update(); // update compulsorily (and then update every 20ms)
update_screen = 0;
Scheduler::set_task_priority(task_handle, OSD_LOW_PRIORITY);
}
}
if(pt > timer_100ms){
timer_100ms+= 100;
On100ms();
lflags.flag_01s = !lflags.flag_01s;
if(lflags.flag_01s) {
if(skip_inc) {
skip_inc++;
if(skip_inc >=3){
count02s++;
skip_inc=0; // we go again
}
} else
count02s++;
}
// count01s++;
}
if(pt > timer_500ms){
timer_500ms+= 500;
lflags.got_data=1; // every half second forcibly
update_screen = 1;
lflags.flag_05s = 1;
count05s++;
lflags.blinker = !lflags.blinker;
if(lflags.blinker) {
// seconds++;
lflags.one_sec_timer_switch = 1; // for warnings
if(lflags.got_date) day_seconds++; // if we has GPS time - let it ticks
if( vas_vsync && vsync_count < 5) { // at a frame rate they should be 25 or 50
// but there are boards where this pin is not connected. China...
max7456_err_count++;
if(max7456_err_count>3) { // 3 seconds bad sync
#ifdef DEBUG
printf(PSTR("restart MAX! vsync_count=%d\n"),vsync_count);
#endif
osd.reset(); // restart MAX7456
}
} else max7456_err_count=0;
vsync_count=0;
heartBeat();
}
}
}
void vsync_ISR(){
vas_vsync=true;
vsync_wait=0; // note its presence
vsync_count++; // count VSYNC interrupts
vsync_time=millis(); // and note a time
if(update_screen) { // there is data for screen
osd_need_redraw=true;
Scheduler::set_task_priority(task_handle, OSD_HIGH_PRIORITY); // higher than all drivers so it will be scheduled just after semaphore release
Scheduler::task_resume(task_handle); // task should be finished at this time so resume it
update_screen = 0;
}
}
// is there any chars in ring buffer?
int16_t osd_available(){
return rb_full_count(&osd_rxrb);
}
void osd_queue(uint8_t c) { // push bytes from OSD to FC around in the ring buffer
uint8_t cnt=10;
while(rb_is_full(&osd_rxrb)) {
hal_yield(0);
if(--cnt == 0) return; // destination don't listen
}
rb_push_insert(&osd_rxrb, c);
}
int16_t osd_getc(){ // get char from ring buffer
return rb_remove(&osd_rxrb);
}
uint32_t osd_txspace() {
return osd_txrb.size - rb_full_count(&osd_txrb);
}
void osd_putc(uint8_t c){
uint8_t cnt=10;
while(rb_is_full(&osd_txrb)) {
Scheduler::set_task_priority(task_handle, OSD_HIGH_PRIORITY); // to run in time of yield()
hal_yield(0);
if(--cnt == 0) break; // destination don't listen
}
rb_push_insert(&osd_txrb, c);
Scheduler::set_task_priority(task_handle, OSD_LOW_PRIORITY); // restore priority to low
}
void osd_dequeue() {
Scheduler::set_task_priority(task_handle, 100); // equal to main to not overflow buffers on packet decode
while(!rb_is_empty(&osd_txrb)) {
extern bool mavlink_one_byte(char c);
char c = rb_remove(&osd_txrb);
if(mavlink_one_byte(c)) lflags.got_data=true;
}
Scheduler::set_task_priority(task_handle, OSD_LOW_PRIORITY); // restore priority to low
}
static uint8_t max_err_cnt=0;
void update_max_buffer(const uint8_t *buffer, uint16_t len){
max7456_sem_on();
uint16_t cnt=0;
#if 1
uint8_t patt = MAX7456_ENABLE_display | MAX7456_SYNC_autosync | OSD::video_mode;
max7456_cs_on();
uint8_t vm0 = MAX_read(MAX7456_VM0_reg | MAX7456_reg_read);
if(vm0 != patt) {
max_err_cnt++;
if(max_err_cnt<3) {
OSD::hw_init(); // first try without reset
} else {
// 3 errors together - nothing helps :(
#ifdef BOARD_OSD_RESET_PIN
{
const stm32_pin_info &pp = PIN_MAP[BOARD_OSD_RESET_PIN];
gpio_write_bit(pp.gpio_device, pp.gpio_bit, LOW);
delayMicroseconds(50);
gpio_write_bit(pp.gpio_device, pp.gpio_bit, HIGH);
delayMicroseconds(120);
}
#endif
OSD::init();
max_err_cnt=0;
}
} else {
max_err_cnt=0;
}
MAX_write(MAX7456_DMAH_reg, 0);
MAX_write(MAX7456_DMAL_reg, 0);
MAX_write(MAX7456_DMM_reg, 1); // set address auto-increment
max7456_cs_off();
if(osd_spi->send_strobe(buffer, len)!=len) {
/*/// for debug - mark last written char
MAX_rw(0x86); // finish transfer
//*///
MAX_rw(0xff); // finish transfer
}
#elif 0
MAX_write(MAX7456_DMAH_reg, 0);
MAX_write(MAX7456_DMAL_reg, 0);
MAX_write(MAX7456_DMM_reg, 0);
// try to send just diffenence - don't clears last chars
uint8_t last_h=0;
while(len--){
if(*buffer != shadowbuf[cnt]){
uint8_t h = cnt>>8 ;
if(last_h != h){
MAX_write(MAX7456_DMAH_reg, h);
last_h = h;
}
MAX_write(MAX7456_DMAL_reg, cnt&0xFF);
MAX_write(MAX7456_DMDI_reg, *buffer);
shadowbuf[cnt] = *buffer;
}
buffer++;
cnt++;
}
#elif 0
// a try to do writes in software strobe mode
MAX_write(MAX7456_DMAH_reg, 0);
MAX_write(MAX7456_DMAL_reg, 0);
MAX_write(MAX7456_DMM_reg, 1); // set address auto-increment
max7456_cs_off();
while(len--){
max7456_cs_on();
// osd_spi->transfer(*buffer++); MAX7456
MAX_rw(*buffer++);
buffer++;
cnt++;
osd_spi->wait_busy();
max7456_cs_off();
}
max7456_cs_on();
MAX_write(MAX7456_DMM_reg, 0); // clear address auto-increment
#else
// just write all to MAX
MAX_write(MAX7456_DMAH_reg, 0);
MAX_write(MAX7456_DMAL_reg, 0);
MAX_write(MAX7456_DMM_reg, 0);
uint8_t last_h=0;
while(len--){
uint8_t h = cnt>>8 ;
if(last_h != h){
MAX_write(MAX7456_DMAH_reg, h);
last_h = h;
}
MAX_write(MAX7456_DMAL_reg, cnt&0xFF);
MAX_write(MAX7456_DMDI_reg, *buffer);
buffer++;
cnt++;
}
#endif
max7456_sem_off();
}
} // namespace
#endif