zr-v4/ArduCopter/UserCode.cpp

#include "Copter.h"

#ifdef USERHOOK_INIT
void Copter::userhook_init()
{
    // put your initialisation code here
    // this will be called once at start-up
    zr_serial_api.init(serial_manager);
}
#endif

#ifdef USERHOOK_FASTLOOP
void Copter::userhook_FastLoop()
{
    // put your 100Hz code here
    // zr_app_50hz();
}
#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
    zr_app_10hz();
    zr_app_50hz();
}
#endif

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

#ifdef USERHOOK_SUPERSLOWLOOP
void Copter::userhook_SuperSlowLoop()
{
    // put your 1Hz code here
}
#endif

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

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

void Copter::zr_app_10hz()
{
        
    if(zr_serial_api.data_trans_init){
        zr_serial_api.update();
    }else{
        static uint32_t last_5s;
        uint32_t tnow = AP_HAL::millis();
        if (tnow - last_5s > 5000) {
            last_5s = tnow;
            zr_serial_api.init(serial_manager);
            gcs().send_text(MAV_SEVERITY_INFO, "data_trans init");
        }
    }

    if(zr_serial_api.data_trans_init){
// void AC_ZR_Serial_API::get_vehicle_status(uint8_t mode,uint8_t in_air,uint32_t home_distance,uint16_t volt_mv,uint8_t bat_remaining)
        uint16_t now_volt = uint16_t(battery.voltage() * 100);
        zr_serial_api.get_vehicle_status((uint8_t)flightmode->mode_number(),(uint8_t)flightmode->is_landing(),home_distance(),now_volt,battery.capacity_remaining_pct());
    }

}
void Copter::zr_app_50hz(){

    if(zr_serial_api.data_trans_init){
        Vector3l pos_neu_cm;
        
        // if (current_loc.get_vector_from_origin_NEU(pos_neu_cm)) {
        Location my_loc;
        if (ahrs.get_position(my_loc)) {
            pos_neu_cm.x = my_loc.lat;
            pos_neu_cm.y = my_loc.lng;
            pos_neu_cm.z = my_loc.alt;
            // zr_serial_api.get_vehicle_NEU_pos(pos_neu_cm);
            // gcs().send_text(MAV_SEVERITY_INFO,"get loc:%.2f,%.2f,%.2f",pos_neu_cm.x,pos_neu_cm.y,pos_neu_cm.z);
        }
        // }

        Vector3f euler,euler_deg;
        if(ahrs.get_secondary_attitude(euler)){
            euler_deg.x = degrees(euler.x);
            euler_deg.y = degrees(euler.y);
            euler_deg.z = wrap_360_cd(degrees(euler.z));
            // zr_serial_api.get_vehicle_euler_angles(euler_deg);
            // gcs().send_text(MAV_SEVERITY_INFO,"get euler:%.2f,%.2f,%.2f",euler_deg.x,euler_deg.y,euler_deg.z);
        }

        Vector3f vel_ned_m;
        Vector3l vel_ned_cm;
        if (ahrs.get_velocity_NED(vel_ned_m)) {
            vel_ned_cm.x = vel_ned_m.x * 100;
            vel_ned_cm.y = vel_ned_m.y * 100;
            vel_ned_cm.z = vel_ned_m.z * 100;
        }
        // zr_serial_api.get_vehicle_flight_data(pos_neu_cm,vel_ned_cm,euler_deg,(uint8_t)flightmode->mode_number(),(uint8_t)flightmode->is_landing());
        zr_serial_api.get_vehicle_flight_data(pos_neu_cm,vel_ned_cm,euler_deg);

        ///////////
        AC_ZR_Serial_API::ZR_Msg_Type msg_type;
        Vector3l data;
        float yaw_deg;
        bool new_data = zr_serial_api.get_control_data(msg_type,data,yaw_deg);
        if(new_data){
            gcs().send_text(MAV_SEVERITY_INFO,"get msg_type:%d, data:%d,%d,%d, y:%.2f",(int)msg_type,data.x,data.y,data.z,yaw_deg);
            switch (msg_type)
            {
                case AC_ZR_Serial_API::ZR_Msg_Type::MSG_CONTROL_TKOFF:
                {
                    if(!motors->armed()){
                        motors->armed(true);
                        hal.util->set_soft_armed(true);
                    }
                    if(flightmode->mode_number() != Mode::Number::GUIDED){
                        set_mode(Mode::Number::GUIDED, ModeReason::SCRIPTING);
                    }
                    float tk_alt = (data.z)/100.0;
                    start_takeoff(tk_alt);
                    gcs().send_text(MAV_SEVERITY_INFO,"do takeoff,alt %.2f",tk_alt);
                }
                    break;
                case AC_ZR_Serial_API::ZR_Msg_Type::MSG_CONTROL_POS:
                    if(motors->armed()){
                        Location target_loc;
                        target_loc.lat = data.x + 100;
                        target_loc.lng = data.y;
                        target_loc.alt = (data.z + 500);
                        // target_loc.alt = (data.z + 500)/100.0;
                        set_target_location(target_loc);
                        gcs().send_text(MAV_SEVERITY_INFO,"set location,lat %d,%d,%d",target_loc.lat,target_loc.lng,target_loc.alt);
                    }
                    break;
                case AC_ZR_Serial_API::ZR_Msg_Type::MSG_CONTROL_VEL:
                    if(motors->armed()){
                        Vector3f target_vel;
                        target_vel.x = data.x / 100.0;
                        target_vel.y = data.y / 100.0;
                        target_vel.z = data.z / 100.0;
                        // target_loc.alt = (data.z + 500)/100.0;
                        set_target_velocity_NED(target_vel);
                        gcs().send_text(MAV_SEVERITY_INFO,"set vel %.2f,%.2f,%.2f",target_vel.x,target_vel.y,target_vel.z);
                    }
                    break;
            
                default:
                    break;
            }
        }
    }
    
}