ardupilot/libraries/AP_HAL_F4Light/HAL_F4Light_Class.cpp

/*
(c) 2017 night_ghost@ykoctpa.ru
 
*/

#pragma GCC optimize ("O2")

#include <AP_HAL/AP_HAL.h>

#if CONFIG_HAL_BOARD == HAL_BOARD_F4LIGHT
#include <assert.h>
#include <AP_HAL_F4Light/AP_HAL_F4Light.h>
#include "AP_HAL_F4Light_Namespace.h"
#include "AP_HAL_F4Light_Private.h"
#include "HAL_F4Light_Class.h"
#include "RCInput.h"
#include "Util.h"

#include <AP_Param_Helper/AP_Param_Helper.h>

#include "UART_PPM.h"

#if defined(USE_SOFTSERIAL)
#include "UART_SoftDriver.h"
#endif



#if defined(BOARD_SDCARD_NAME) || defined(BOARD_DATAFLASH_FATFS)
#include "sd/SD.h"
#endif

#if defined(BOARD_OSD_CS_PIN)
#include "UART_OSD.h"
#endif

#if defined(USB_MASSSTORAGE) 
#include "massstorage/mass_storage.h"
#endif

using namespace F4Light;


static F4Light::I2CDeviceManager i2c_mgr_instance;

// XXX make sure these are assigned correctly
static USBDriver          USB_Driver(1);        // ACM
static UARTDriver uart1Driver(_USART1); // main port
static UARTDriver uart6Driver(_USART6); // pin 7&8(REVO)/5&6(RevoMini) of input port



#ifdef BOARD_HAS_UART3
 static UARTDriver uart3Driver(_USART3); // flexi port
// static SerialDriver IN_CCM softDriver(BOARD_SOFTSERIAL_TX, BOARD_SOFTSERIAL_RX, false);  // pin 7&8 of input port
#endif

#if defined(BOARD_SOFT_UART3) || defined(USE_SOFTSERIAL)
 static SerialDriver IN_CCM softDriver(BOARD_SOFTSERIAL_TX, BOARD_SOFTSERIAL_RX, false);
#endif


#ifdef BOARD_OSD_CS_PIN
 static UART_OSD uartOSDdriver;
#endif

#if defined(BOARD_USART4_RX_PIN) && defined( BOARD_USART4_TX_PIN)
 static UARTDriver uart4Driver(_UART4);  // pin 5&6 of servo port
#endif

static UART_PPM uartPPM2(1); // PPM2 input
static UART_PPM uartPPM1(0); // PPM1 input 



// only for DSM satellite, served in rc_input
//static UARTDriver uart5Driver(_UART5,0);  // D26/PD2  6 EXTI_RFM22B / UART5_RX  input-only UART for DSM satellite

/*
        input port pinout
        1    2    3    4    5    6    7   8
pin              b14  b15  c6   c7   c8   c9
       gnd  vcc  PPM  buzz 6_tx 6_rx Sscl Ssda
USE_SOFTSERIAL ->                    S_tx S_rx
servos ->        srv7 srv8 srv9 srv10 srv11 srv12 
*/


/* OPLINK AIR port pinout
1       2       3       4       5       6       7
                        
gnd    +5      26      103                     
               rx      pow
*/

static F4Light::SPIDeviceManager spiDeviceManager;
static AnalogIn  analogIn IN_CCM;
static Storage   storageDriver;
static GPIO      gpioDriver;
static RCInput   rcinDriver;
static RCOutput  rcoutDriver;
static Scheduler schedulerInstance;
static Util      utilInstance;

HAL_state HAL_F4Light::state;

//const AP_HAL::UARTDriver** HAL_F4Light::uarts[] = { &uartA, &uartB, &uartC, &uartD, &uartE, &uartF };

/*
        AP_HAL::UARTDriver* _uartA, // console
        AP_HAL::UARTDriver* _uartB, // 1st GPS
        AP_HAL::UARTDriver* _uartC, // telem1
        AP_HAL::UARTDriver* _uartD, // telem2
        AP_HAL::UARTDriver* _uartE, // 2nd GPS
        AP_HAL::UARTDriver* _uartF, // extra1


AP_SerialManager.cpp line 159
    // initialise pointers to serial ports
    state[1].uart = hal.uartC;  // serial1, uartC, normally telem1
    state[2].uart = hal.uartD;  // serial2, uartD, normally telem2
    state[3].uart = hal.uartB;  // serial3, uartB, normally 1st GPS
    state[4].uart = hal.uartE;  // serial4, uartE, normally 2nd GPS
    state[5].uart = hal.uartF;  // serial5



*/



HAL_F4Light::HAL_F4Light() :
    AP_HAL::HAL(
        &USB_Driver,            // uartA - USB console                                         - Serial0
        &uart6Driver,           // uartB - pin 7&8(REVO)/5&6(RevoMini) of input port - for GPS - Serial3
        &uart1Driver,           // uartC - main port  - for telemetry                          - Serial1

#if   BOARD_UARTS_LAYOUT == 1 // Revolution

        &uart3Driver,           // uartD - flexi port                                          - Serial2 
        &uart4Driver,           // uartE - PWM pins 5&6                                        - Serial4
        &softDriver,            // uartF - soft UART on pins 7&8                               - Serial5 

#elif BOARD_UARTS_LAYOUT == 2 // Airbot

        &uart4Driver,           // uartD - PWM pins 5&6                                        - Serial2 
        &uartPPM2,              // uartE - input data from PPM2 pin                            - Serial4
        &uartPPM1,              // uartF - input data from PPM1 pin                            - Serial5 
        
#elif BOARD_UARTS_LAYOUT == 3 // AirbotV2

        &uartOSDdriver,         // uartD - OSD emulated UART                                   - Serial2
        &uart4Driver,           // uartE - PWM pins 5&6                                        - Serial4 
        &uartPPM2,              // uartF - input data from PPM2 pin                            - Serial5

#elif BOARD_UARTS_LAYOUT == 4 // MiniOSD

        NULL,
        NULL,
        NULL,
        
#elif BOARD_UARTS_LAYOUT == 5 // MicroOSD

        NULL,
        NULL,
        NULL,
        
#elif BOARD_UARTS_LAYOUT == 6 // MatekF405_OSD

        NULL,
        NULL,
        NULL,
        
#elif BOARD_UARTS_LAYOUT == 7 // Cl_Racing F4

        &uartOSDdriver,         // uartD - OSD emulated UART                                   - Serial2
        &uartPPM1,              // uartE - input data from PPM1 pin                            - Serial4
        NULL,                   // no uartF
        
#else
 #error no BOARD_UARTS_LAYOUT!
#endif

        &i2c_mgr_instance,  // I2C
        &spiDeviceManager,  // spi 
        &analogIn,          // analogin 
        &storageDriver,     // storage 
        &HAL_CONSOLE,       // console via radio or USB on per-board basis 
        &gpioDriver,        // gpio 
        &rcinDriver,        // rcinput 
        &rcoutDriver,       // rcoutput 
        &schedulerInstance, // scheduler 
        &utilInstance,	    // util 
        nullptr,            // no onboard optical flow 
        nullptr             // no CAN 
    )
    
           //  0     1        2        3        4         5
           // USB    Main   Flexi/OSD  Uart4   UART6    Soft/Uart4
    , uarts{ &uartA, &uartC, &uartD,   &uartE, &uartB,   &uartF }

{

    uint32_t sig = board_get_rtc_register(RTC_CONSOLE_REG);
    if( (sig & ~CONSOLE_PORT_MASK) == CONSOLE_PORT_SIGNATURE) {
        AP_HAL::UARTDriver** up = uarts[sig & CONSOLE_PORT_MASK];
        if(up){
            console =  *up;
        }
    }
}

extern const AP_HAL::HAL& hal;


void HAL_F4Light::run(int argc,char* const argv[], Callbacks* callbacks) const
{
    assert(callbacks);

    /* initialize all drivers and private members here.
     * up to the programmer to do this in the correct order.
     * Scheduler should likely come first. */

    scheduler->init();
    
    uint32_t start_t = AP_HAL::millis();

    gpio->init();

    rcout->init(); 
    
    usart_disable_all();

    {
#if defined(USB_MASSSTORAGE)
        uint32_t sig = board_get_rtc_register(RTC_MASS_STORAGE_REG);
        if( sig == MASS_STORAGE_SIGNATURE) {
            board_set_rtc_register(0, RTC_MASS_STORAGE_REG);

#if defined(BOARD_SDCARD_NAME) && defined(BOARD_SDCARD_CS_PIN)
            SD.begin(F4Light::SPIDeviceManager::_get_device(BOARD_SDCARD_NAME));
#elif defined(BOARD_DATAFLASH_FATFS)
            SD.begin(F4Light::SPIDeviceManager::_get_device(HAL_DATAFLASH_NAME));
#endif
            state.sd_busy=true;
            massstorage.setup();        //      init USB as mass-storage
        } 
        else 
#endif
        {
            extern void usb_init();     //      init as VCP
            usb_init(); // moved from boards.cpp

            uartA->begin(115200); // uartA is the USB serial port used for the console, so lets make sure it is initialized at boot 
        }
    }
    

    if(console != uartA) {
        console->begin(57600);  // init telemetry port as console
    }


    rcin->init();

    storage->init(); // Uses EEPROM.*, flash_stm* reworked
    analogin->init();

    printf("\nHAL startup  at %ldms\n", start_t);            

    if(!state.sd_busy) {


#if defined(BOARD_SDCARD_NAME) && defined(BOARD_SDCARD_CS_PIN)
        printf("\nEnabling SD at %ldms\n", AP_HAL::millis());            
        SD.begin(F4Light::SPIDeviceManager::_get_device(BOARD_SDCARD_NAME));
#elif defined(BOARD_DATAFLASH_FATFS)
        printf("\nEnabling DataFlash as SD at %ldms\n", AP_HAL::millis());            
        SD.begin(F4Light::SPIDeviceManager::_get_device(HAL_DATAFLASH_NAME));
#endif

#if defined(BOARD_OSD_NAME)
        uartOSDdriver.begin(57600); // init OSD after SD but before call to lateInit(), but only if not in USB_STORAGE
#endif

    }


    printf("\nHAL init done at %ldms\n", AP_HAL::millis());

    callbacks->setup();

#if 0 //[ here is too late :( so we need a small hack and call lateInit from Scheduler::register_delay_callback 
//         which called when parameters already initialized
    
    lateInit();
#endif //]

    
    scheduler->system_initialized(); // clear bootloader flag

    Scheduler::start_stats_task(); 

    printf("\nLoop started at %ldms\n", AP_HAL::millis());            


// main application loop hosted here!
    for (;;) {
        callbacks->loop();
//        ((F4Light::Scheduler *)scheduler)->loop(); // to execute stats in main loop
//        ((F4Light::RCInput *)rcin)->loop(); // to execute debug in main loop
    }
}


static bool lateInitDone=false;

void HAL_F4Light::lateInit() {
    
    if(lateInitDone) return;
    
    lateInitDone=true;

    {
        uint32_t sig = board_get_rtc_register(RTC_CONSOLE_REG);
        uint8_t port = hal_param_helper->_console_uart;

        if(port < sizeof(uarts)/sizeof(AP_HAL::UARTDriver**) ){

            if( (sig & ~CONSOLE_PORT_MASK) == CONSOLE_PORT_SIGNATURE) {
                if(port != (sig & CONSOLE_PORT_MASK)) { // wrong console - reboot needed
                    board_set_rtc_register(CONSOLE_PORT_SIGNATURE | (port & CONSOLE_PORT_MASK), RTC_CONSOLE_REG);
                    board_set_rtc_register(FORCE_APP_RTC_SIGNATURE, RTC_SIGNATURE_REG); // force bootloader to not wait
                    Scheduler::_reboot(false);
                }
            } else { // no signature - set and check console
                board_set_rtc_register(CONSOLE_PORT_SIGNATURE | (port & CONSOLE_PORT_MASK), RTC_CONSOLE_REG);

                AP_HAL::UARTDriver** up = uarts[port];
                if(up && *up != console) {
                    board_set_rtc_register(FORCE_APP_RTC_SIGNATURE, RTC_SIGNATURE_REG); // force bootloader to not wait
                    Scheduler::_reboot(false);
                }
            }
        }
    }
    {
        uint32_t g   = board_get_rtc_register(RTC_OV_GUARD_REG);
        uint32_t sig = board_get_rtc_register(RTC_OVERCLOCK_REG);
        uint8_t  oc  = hal_param_helper->_overclock;
        
        if(g==OV_GUARD_FAIL_SIGNATURE) {
            if(oc==0) {
                board_set_rtc_register(OVERCLOCK_SIGNATURE | oc, RTC_OVERCLOCK_REG); // set default clock
                board_set_rtc_register(0, RTC_OV_GUARD_REG);                    // and reset failure
            } else printf("\noverclocking failed!\n");            
        } else {

            if((sig & ~OVERCLOCK_SIG_MASK ) == OVERCLOCK_SIGNATURE ) { // if correct signature
                board_set_rtc_register(OVERCLOCK_SIGNATURE | oc, RTC_OVERCLOCK_REG); // set required clock in any case
                if((sig & OVERCLOCK_SIG_MASK) != oc) {                 // if wrong clock
                    board_set_rtc_register(FORCE_APP_RTC_SIGNATURE, RTC_SIGNATURE_REG); // force bootloader to not wait
                    Scheduler::_reboot(false);                 //  then reboot required
                } 
            } else { // no signature, write only if needed
                if(oc) board_set_rtc_register(OVERCLOCK_SIGNATURE | oc, RTC_OVERCLOCK_REG); // set required clock
            }
        }
    }
    
    { // one-time connection to COM-port
        uint8_t conn = hal_param_helper->_connect_com;
        if(conn) {
            hal_param_helper->_connect_com = 0;
            hal_param_helper->_connect_com.save();
            
            if(conn < sizeof(uarts)/sizeof(AP_HAL::UARTDriver**) ){
                AP_HAL::UARTDriver** up = uarts[conn];
                if(up && *up){
                    connect_uart(uartA,*up, NULL);
                }
            } else printf("\nWrong HAL_CONNECT_COM selected!");
    
        }
    }

#if defined(USB_MASSSTORAGE)
    if(!state.sd_busy){    
        uint8_t conn=hal_param_helper->_usb_storage;
        
        if(conn){
            hal_param_helper->_usb_storage=0;
            hal_param_helper->_usb_storage.save();

            board_set_rtc_register(MASS_STORAGE_SIGNATURE, RTC_MASS_STORAGE_REG);
            board_set_rtc_register(FORCE_APP_RTC_SIGNATURE, RTC_SIGNATURE_REG); // force bootloader to not wait
            Scheduler::_reboot(false);
        }
    }
#endif

#ifdef USE_SERIAL_4WAY_BLHELI_INTERFACE
    { // one-time connection to ESC
        uint8_t conn = hal_param_helper->_connect_esc;

        if(conn){
            hal_param_helper->_connect_esc = 0;
            hal_param_helper->_connect_esc.save();

            conn-=1;

            if(conn < sizeof(uarts)/sizeof(AP_HAL::UARTDriver**) ){
                AP_HAL::UARTDriver** up = uarts[conn];
                if(up && *up){
                    RCOutput::do_4way_if(*up);
                }
            } else printf("\nWrong HAL_CONNECT_ESC selected!");
        }
    }
#endif

    RCOutput::lateInit(); // 2nd stage - now with loaded parameters

    // all another parameter-dependent inits

#ifdef BOARD_I2C_FLEXI
    if(hal_param_helper->_flexi_i2c) {
        uart3Driver.end();
        uart3Driver.disable(); // uses Flexi port occupied by I2C 
        printf("\nUART3 disabled by I2C2\n");            
    }
#endif
    I2CDevice::lateInit();

    Storage::late_init(hal_param_helper->_eeprom_deferred); 
    
    uint8_t flags=0;

    if(hal_param_helper->_rc_fs)      flags |= BOARD_RC_FAILSAFE;

    RCInput::late_init(flags);
}

// 57600 gives ~6500 chars per second or 6 chars per ms
void HAL_F4Light::connect_uart(AP_HAL::UARTDriver* uartL,AP_HAL::UARTDriver* uartR, AP_HAL::Proc proc){
    while(1){
        bool got=false;
        if(uartL->available() && uartR->txspace()) { uartR->write(uartL->read()); got=true; }
        if(uartR->available() && uartL->txspace()) { uartL->write(uartR->read()); got=true; }
        if(proc) proc();
        if(!got) Scheduler::yield(100); // give a chance to other threads
        if(state.disconnect) break; // if USB disconnected
    }
}

static const HAL_F4Light hal_revo;

const AP_HAL::HAL& AP_HAL::get_HAL() {
    return hal_revo;
}


extern "C" void usb_mass_mal_USBdisconnect();

void usb_mass_mal_USBdisconnect(){ 
    HAL_F4Light::state.sd_busy=false;
    HAL_F4Light::state.disconnect=true;
}

#endif