zr-v4/ArduCopter/UserCode.cpp

/*
 * @Author: your name
 * @Date: 2020-10-29 14:13:48
 * @LastEditTime: 2020-10-29 14:19:30
 * @LastEditors: your name
 * @Description: In User Settings Edit
 * @FilePath: /zr-v4/ArduCopter/UserCode.cpp
 */
#include "Copter.h"

#define CAM_DEBUG  1

float angle_max_origin =DEFAULT_ANGLE_MAX;
 int8_t land_delay =0;

#ifdef USERHOOK_INIT
void Copter::userhook_init()
{
    // put your initialisation code here
    // this will be called once at start-up

    relay.on(1);
    set_mode(Mode::Number::LOITER, ModeReason::STARTUP);
    angle_max_origin  = aparm.angle_max;  //back up angle_max,when change the param  need to reboot the uav
    land_delay = 30;
    // gcs().send_text(MAV_SEVERITY_INFO, "NOTICE: version: ZRUAV v4.0.7-rc1");
}
#endif

#ifdef USERHOOK_FASTLOOP
void Copter::userhook_FastLoop()
{
    // put your 100Hz code here
}
#endif

#ifdef USERHOOK_50HZLOOP
void Copter::userhook_50Hz()
{
    // put your 50Hz code here
}
#endif

#ifdef USERHOOK_MEDIUMLOOP
void Copter::userhook_MediumLoop()
{
    // put your 10Hz code here
#if CAM_DEBUG
   static uint64_t prt_cnt;
   
    uint16_t rc7_in = RC_Channels::rc_channel(CH_7)->get_radio_in();
    switch (in_debug_mode)
    {
    case 0:
        if(rc7_in > 1600){
            in_debug_mode = 1;
        }
        break;
    case 1:
        if(rc7_in < 1600){
            in_debug_mode = 2;
        }
        break;
    case 2:
        if(rc7_in > 1600){
            in_debug_mode = 3;
        }
        break;
    case 3:
        if(rc7_in < 1600){
            in_debug_mode = 4;
        }
        break;
    case 4:
        if(rc7_in > 1600){
            in_debug_mode = 0;
        }
        break;
    
    default:
        break;
    }

    if(in_debug_mode == 4 || control_mode==Mode::Number::SPORT){
        prt_cnt++;
        if(!motors->armed()){
            arming.arm(AP_Arming::Method::SCRIPTING);
            gcs().send_text(MAV_SEVERITY_INFO,"NOTICE: debug mode arm!");
        }
        gcs().send_text(MAV_SEVERITY_INFO,"NOTICE: cnt:%lld",prt_cnt);
        camera.take_picture();
    }else{

        prt_cnt = 0;
    }
#endif
}
#endif

#ifdef USERHOOK_SLOWLOOP
void Copter::userhook_SlowLoop()
{
    // put your 3.3Hz code here
    zr_SlowLoop();
}
#endif

#ifdef USERHOOK_SUPERSLOWLOOP

void Copter::userhook_SuperSlowLoop()
{
    // put your 1Hz code here
    zr_SuperSlowLoop();
#if CAM_DEBUG 
    // static uint64_t prt_cnt;

    // if(in_debug_mode == 2 || control_mode==Mode::Number::ACRO){
    //     prt_cnt++;
    //     if(!motors->armed()){
    //         arming.arm(AP_Arming::Method::SCRIPTING);
    //         gcs().send_text(MAV_SEVERITY_INFO,"NOTICE: debug mode arm!");
    //     }
    //     gcs().send_text(MAV_SEVERITY_INFO,"NOTICE: cnt:%lld",prt_cnt);
    //     camera.take_picture();
    // }else{

    //     prt_cnt = 0;
    // }
#endif

}
#endif

#ifdef USERHOOK_AUXSWITCH
void Copter::userhook_auxSwitch1(uint8_t ch_flag)
{
    // put your aux switch #1 handler here (CHx_OPT = 47)

    // switch (ch_flag) {
    //     case 2: {
    //         relay.on(2);
    //         break;
    //     }
    //     case 0: {
    //         relay.off(2);
    //         break;
    //     }
    // }
#if CAM_DEBUG      
    switch (ch_flag) {
        case 2: {
            // relay.on(2);
            gcs().send_text(MAV_SEVERITY_INFO,"NOTICE: debug mode 2!");
            in_debug_mode = 4;
            break;
        }
        case 1: {
            // relay.on(2);
            gcs().send_text(MAV_SEVERITY_INFO,"NOTICE: debug mode 1!");
            in_debug_mode = 1;
            break;
        }
        case 0: {
            // relay.off(2);
            gcs().send_text(MAV_SEVERITY_INFO,"NOTICE: debug mode off!");
            in_debug_mode = 0;
            break;
        }
    }
#endif
}

void Copter::userhook_auxSwitch2(uint8_t ch_flag)
{
    // put your aux switch #2 handler here (CHx_OPT = 48)
}

void Copter::userhook_auxSwitch3(uint8_t ch_flag)
{
    // put your aux switch #3 handler here (CHx_OPT = 49)
}


#endif