ardupilot/ArduCopter/zr_flight.cpp

#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 || \
            channel_throttle->get_control_in()<380 || \
            channel_throttle->get_control_in()>590  )
        {
            // rc().set_disable_ch();
            gcs().send_text(MAV_SEVERITY_INFO,"NOTICE: 注意遥控通道是否在中位");
            
            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);

}

void Copter::zr_SuperSlowLoop(){
    static bool before_fly = true;
    static float batt_mah_teb[] =
    {
        25.20,24.90,24.84,24.76,24.69,24.64,24.60,24.57,24.54,24.52,
        24.50,24.48,24.45,24.43,24.40,24.36,24.32,24.26,24.21,24.14,
        24.07,24.00,23.94,23.88,23.79,23.71,23.65,23.58,23.51,23.46,
        23.41,23.36,23.30,23.24,23.19,23.13,23.07,23.01,22.95,22.91,
        22.86,22.81,22.76,22.70,22.65,22.59,22.54,22.48,22.43,22.37,
        22.31,22.26,22.21,22.16,22.11,22.06,22.01,21.96,21.92,21.88,
        21.84,21.79,21.76,21.72,21.69,21.64,21.59,21.54,21.49,21.45,
        21.40,21.34,21.29,21.24,21.18,21.13,21.07,21.02,20.96,20.90,
        20.85,20.80,20.75,20.69,20.63,20.56,20.47,20.29,20.16,20.04,
        19.88,19.73,19.53,19.34,19.19,19.00,18.80,18.51,18.00,17.40,
        16.80
    };
    static float batt_mah_teb_7s[] =
    {
        29.30,29.13,28.97,28.86,28.77,28.71,28.67,28.64,28.61,28.59,
        28.56,28.53,28.50,28.46,28.43,28.38,28.33,28.28,28.21,28.16,
        28.10,28.03,27.94,27.87,27.80,27.75,27.66,27.59,27.52,27.46,
        27.38,27.32,27.26,27.19,27.13,27.07,27.00,26.93,26.85,26.79,
        26.74,26.67,26.61,26.54,26.47,26.4,26.35,26.28,26.21,26.16,
        26.1,26.05,25.98,25.91,25.85,25.79,25.74,25.69,25.63,25.58,
        25.54,25.5,25.45,25.41,25.36,25.32,25.27,25.23,25.18,25.13,
        25.08,25.03,24.97,24.93,24.86,24.8,24.74,24.67,24.6,24.52,
        24.46,24.39,24.3,24.25,24.22,24.18,24.1,24.02,23.94,23.81,
        23.65,23.52,23.32,23.19,22.95,22.74,22.52,22.28,22.04,21.8,
        21.6,
    };
    
    if(motors->armed()){
         if(before_fly){
            before_fly = false;
            camera.create_new_folder();
        }
        relay.on(3);
        // 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){
        uint8_t cnt,cacl_volt_pst;
        float delt_volt;
        cnt = 0;
        switch (battery.get_batt_type(1))
        {
            case 0:
                /* code */
                break;
            case 1:
                for(cnt = 0; cnt<102; cnt++ ){
                    delt_volt = batt_mah_teb[cnt] - battery.voltage();
                    // delt_volt = batt_mah_teb[cnt] - test_volt;
                    if(delt_volt <= 0)
                        break;
                }
                break;
            case 2:
                for(cnt = 0; cnt<102; cnt++ ){
                    delt_volt = batt_mah_teb_7s[cnt] - battery.voltage();
                    // delt_volt = batt_mah_teb[cnt] - test_volt;
                    if(delt_volt <= 0)
                        break;
                }
                break;
            
            default:
                break;
        }
        cacl_volt_pst = 100 - cnt;
        battery.reset_remaining(1, cacl_volt_pst);
    }

    gcs().send_message(MSG_ZR_FLYING_STATUS);

}
#endif