#if 1
#include "Copter.h"


void Copter::do_avoid_action(){
   
}
// bool Copter::close_to_EKF_origin(const Location& loc)
// {
//     // check distance to EKF origin
//     const struct Location &ekf_origin = inertial_nav.get_origin();
//     if (get_distance(ekf_origin, loc) < 15 && ekf_origin.alt < 5000) {
//         // gcs().send_text(MAV_SEVERITY_INFO,"NOTICE: dist:%.2f,alt:%d",get_distance(ekf_origin, loc) ,ekf_origin.alt);
//         return true;
//     }

//     // close enough to origin
//     return false;
// }

bool Copter::zr_radio_valid(){
    if(!motors->armed()){
        return true;
    }
    if (!rc().ignore_ctrl_channel() && (control_mode == Mode::Number::AUTO || control_mode == Mode::Number::RTL || control_mode == Mode::Number::LAND)){
        if(abs(channel_roll->get_control_in())>750 || \
            abs(channel_pitch->get_control_in())>750 || \
            abs(channel_yaw->get_control_in())>750)
        {
            // rc().set_disable_ch();
            gcs().send_text(MAV_SEVERITY_INFO,"注意遥控通道是否在中位");
            
            return false;
        }
    }
    return true;
}

void Copter::zr_SlowLoop(){

/********** @Brown,for get camera pos **********/
    static int32_t last_pos_lat;
    static int32_t last_pos_lng;

    camera.get_last_trigger_pos(last_pos_lat,last_pos_lng);
    if(mode_auto.mission._type_resume && last_pos_lat!=0)
        mode_auto.mission.auto_resume_mission((uint8_t)control_mode,motors->armed(),last_pos_lat,last_pos_lng);
    else
        mode_auto.mission.auto_resume_mission((uint8_t)control_mode,motors->armed(),current_loc.lat,current_loc.lng);

}

uint8_t Copter::BinarySearch2f(float a[], float value, int low, int high)
{

    if (low > high)
        return cacl_volt_pst;
    cacl_volt_pst = (low + high) / 2;

    if (fabs(a[cacl_volt_pst] - value)<0.01)
        //if (abs(a[mid] - value) < 30)
        return cacl_volt_pst;
    else if (a[cacl_volt_pst] > value)
        return BinarySearch2f(a, value, low, cacl_volt_pst - 1);
    else
        return BinarySearch2f(a, value, cacl_volt_pst + 1, high);
}

void Copter::zr_SuperSlowLoop(){
    static bool before_fly = true;
    static float batt_mah_teb[] =
    {
        16.80, 17.40, 18.00, 18.51, 18.80, 19.00, 19.19, 19.34, 19.53, 19.73,
        19.88, 20.04, 20.16, 20.29, 20.47, 20.56, 20.63, 20.69, 20.75, 20.80,
        20.85, 20.90, 20.96, 21.02, 21.07, 21.13, 21.18, 21.24, 21.29, 21.34,
        21.40, 21.45, 21.49, 21.54, 21.59, 21.64, 21.69, 21.72, 21.76, 21.79,
        21.84, 21.88, 21.92, 21.96, 22.01, 22.06, 22.11, 22.16, 22.21, 22.26,
        22.31, 22.37, 22.43, 22.48, 22.54, 22.59, 22.65, 22.70, 22.76, 22.81,
        22.86, 22.91, 22.95, 23.01, 23.07, 23.13, 23.19, 23.24, 23.30, 23.36,
        23.41, 23.46, 23.51, 23.58, 23.65, 23.71, 23.79, 23.88, 23.94, 24.00,
        24.07, 24.14, 24.21, 24.26, 24.32, 24.36, 24.40, 24.43, 24.45, 24.48,
        24.50, 24.52, 24.54, 24.57, 24.60, 24.64, 24.69, 24.76, 24.84, 24.90,
        25.20
    };
    static float batt_mah_teb_7s[] =
    {
        21.60, 21.80, 22.04, 22.28, 22.52, 22.74, 22.95, 23.19, 23.32, 23.52,
        23.65, 23.81, 23.94, 24.02, 24.10, 24.18, 24.22, 24.25, 24.30, 24.39,
        24.46, 24.52, 24.60, 24.67, 24.74, 24.80, 24.86, 24.93, 24.97, 25.03,
        25.08, 25.13, 25.18, 25.23, 25.27, 25.32, 25.36, 25.41, 25.45, 25.50,
        25.54, 25.58, 25.63, 25.69, 25.74, 25.79, 25.85, 25.91, 25.98, 26.05,
        26.10, 26.16, 26.21, 26.28, 26.35, 26.40, 26.47, 26.54, 26.61, 26.67,
        26.74, 26.79, 26.85, 26.93, 27.00, 27.07, 27.13, 27.19, 27.26, 27.32,
        27.38, 27.46, 27.52, 27.59, 27.66, 27.75, 27.80, 27.87, 27.94, 28.03,
        28.10, 28.16, 28.21, 28.28, 28.33, 28.38, 28.43, 28.46, 28.50, 28.53,
        28.56, 28.59, 28.61, 28.64, 28.67, 28.71, 28.77, 28.86, 28.97, 29.13,
        29.30
    };
    
    if(motors->armed()){
         if(before_fly){
            before_fly = false;
           // camera.create_new_folder(10);
            gcs().send_text(MAV_SEVERITY_INFO,"%s",get_sysid_board_id());
        }
        relay.on(3);
        gcs().send_message(MSG_ZR_FLYING_STATUS);
        // camera.set_in_arm_mode(true);

    }else{
        relay.off(3);
        // camera.set_in_arm_mode(false);
        // rc().set_enable_ch();
        updownStatus =UpDown_TakeOffStart;        
    }
    if(before_fly){
        cacl_volt_pst = 0;
        uint8_t bat_cnt;
        float now_volt = battery.voltage();
        switch (battery.get_batt_type(0))
        {
            case 0:
                /* code */
                break;
            case 1:
                bat_cnt =  BinarySearch2f(batt_mah_teb,now_volt,0,100);
                battery.reset_remaining(1,bat_cnt);
                // gcs().send_text(MAV_SEVERITY_INFO,"cnt:%d,mid:%d",bat_cnt,cacl_volt_pst);
                break;
            case 2:
                bat_cnt = BinarySearch2f(batt_mah_teb_7s,now_volt,0,100);
                battery.reset_remaining(1,bat_cnt);
                // gcs().send_text(MAV_SEVERITY_INFO,"cnt:%d,mid:%d",bat_cnt,cacl_volt_pst);
                break;
            
            default:
                break;
        }
    }


}
#endif