ardupilot/ArduCopter/zr_app.cpp

#include "Copter.h"

// start takeoff to given altitude (for use by scripting)
bool Copter::start_takeoff(float alt)
{
    // exit if vehicle is not in Guided mode or Auto-Guided mode
    if (!flightmode->in_guided_mode()) {
        return false;
    }

    if (mode_guided.do_user_takeoff_start(alt * 100.0f)) {
        copter.set_auto_armed(true);
        return true;
    }
    return false;
}

// set target location (for use by scripting)
bool Copter::set_target_location(const Location& target_loc,int32_t yaw_cd)
{
    // exit if vehicle is not in Guided mode or Auto-Guided mode
    if (!flightmode->in_guided_mode()) {
        return false;
    }

    // return mode_guided.set_destination(target_loc);
    return mode_guided.set_destination(target_loc, true, (float)yaw_cd, true, 200.0, false);
}

// set target position (for use by scripting)
bool Copter::set_target_pos_NED(const Vector3f& target_pos, bool use_yaw, float yaw_deg, bool use_yaw_rate, float yaw_rate_degs, bool yaw_relative, bool terrain_alt)
{
    // exit if vehicle is not in Guided mode or Auto-Guided mode
    if (!flightmode->in_guided_mode()) {
        return false;
    }

    const Vector3f pos_neu_cm(target_pos.x * 100.0f, target_pos.y * 100.0f, -target_pos.z * 100.0f);

    return mode_guided.set_destination(pos_neu_cm, use_yaw, yaw_deg * 100.0, use_yaw_rate, yaw_rate_degs * 100.0, yaw_relative);
}


bool Copter::set_target_velocity_NED(const Vector3f& vel_ned)
{
    // exit if vehicle is not in Guided mode or Auto-Guided mode
    if (!flightmode->in_guided_mode()) {
        return false;
    }

    // convert vector to neu in cm
    const Vector3f vel_neu_cms(vel_ned.x * 100.0f, vel_ned.y * 100.0f, -vel_ned.z * 100.0f);
    mode_guided.set_velocity(vel_neu_cms);
    return true;
}


void Copter::zr_app_10hz()
{
    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)control_mode,!ap.land_complete,home_distance(),now_volt,battery.capacity_remaining_pct());
    }

}
void Copter::zr_app_50hz(){

    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){
        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(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((uint8_t)control_mode ,msg_type,data,yaw_deg);
        if(new_data){
            static char  buf[50];
            int32_t ahrs_yaw_cd = wrap_360_cd(int32_t(yaw_deg * 100));
            // gcs().send_text(MAV_SEVERITY_INFO,"get msg_type:%d, data:%ld,%ld,%ld, y:%.2f",(int)msg_type,data.x,data.y,data.z,yaw_deg);
            // zr_serial_api.print_data("get msg_type:%d, data:%ld,%ld,%ld, y:%.2f",(int)msg_type,data.x,data.y,data.z,yaw_deg);
            if(zr_serial_api.get_param_debug()){
            snprintf(buf, sizeof(buf), "type:%d, data:%ld,%ld,%ld, y:%.2f",(int)msg_type,data.x,data.y,data.z,yaw_deg);
            zr_serial_api.print_msg(buf);
            memset(buf,0,50);
            }
            switch (msg_type)
            {
                case AC_ZR_Serial_API::ZR_Msg_Type::MSG_CONTROL_TKOFF:
                {
                    if(!motors->armed()){
                        arming.arm(AP_Arming::Method::SCRIPTING);
                    }
                    if(control_mode != Mode::Number::GUIDED){
                        set_mode(Mode::Number::GUIDED, ModeReason::SCRIPTING);
                    }
                    float tk_alt = (data.z)/100.0;
                    start_takeoff(tk_alt);
                    if(zr_serial_api.get_param_debug()){

                        // zr_serial_api.print_data("do takeoff,alt %.2f",tk_alt);
                        snprintf(buf, sizeof(buf), "do takeoff,alt %.2f",tk_alt);
                        zr_serial_api.print_msg(buf);
                        memset(buf,0,50);
                    }
                }
                    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,ahrs_yaw_cd);
                        if(zr_serial_api.get_param_debug()){
                            // zr_serial_api.print_data("set location,lat %ld,%ld,%ld",target_loc.lat,target_loc.lng,target_loc.alt);
                            snprintf(buf, sizeof(buf), "set location,lat %ld,%ld,%ld",target_loc.lat,target_loc.lng,target_loc.alt);
                            zr_serial_api.print_msg(buf);
                            memset(buf,0,50);
                        }
                    }
                    break;
                case AC_ZR_Serial_API::ZR_Msg_Type::MSG_CONTROL_VEL:
                    if(motors->armed()){
                        Vector3f target_vel;
                        target_vel.x = data.x / 1.0;
                        target_vel.y = data.y / 1.0;
                        target_vel.z = data.z / 1.0;
                        // target_loc.alt = (data.z + 500)/100.0;
                        // set_target_velocity_NED(target_vel);
                        mode_guided.set_velocity(target_vel,true,ahrs_yaw_cd,false,0.0f,false,true);
                        if(zr_serial_api.get_param_debug()){

                            // zr_serial_api.print_data("set vel %.2f,%.2f,%.2f",target_vel.x,target_vel.y,target_vel.z);
                            snprintf(buf, sizeof(buf), "set vel %.2f,%.2f,%.2f",target_vel.x,target_vel.y,target_vel.z);
                            zr_serial_api.print_msg(buf);
                            memset(buf,0,50);
                        }
                    }
                    break;
                case AC_ZR_Serial_API::ZR_Msg_Type::MSG_CONTROL_VEL_P:
                    if(motors->armed()){
                        Vector3f target_vel;
                        target_vel.x = data.x / 1.0;
                        target_vel.y = data.y / 1.0;
                        target_vel.z = data.z / 1.0;
                        // float speed_forward = target_vel.x * ahrs.cos_yaw() + target_vel.y * ahrs.sin_yaw();
                        // float speed_right   = -target_vel.x * ahrs.sin_yaw() + target_vel.y * ahrs.cos_yaw();

                        // rotate from body-frame to NE frame
                        const float ne_x = target_vel.x * ahrs.cos_yaw() - target_vel.y * ahrs.sin_yaw();
                        const float ne_y = target_vel.x * ahrs.sin_yaw() + target_vel.y * ahrs.cos_yaw();
                        // target_loc.alt = (data.z + 500)/100.0;
                        // set_target_velocity_NED(target_vel);

                        // exit if vehicle is not in Guided mode or Auto-Guided mode
                        if (!flightmode->in_guided_mode()) {
                            return;
                        }

                        // convert vector to neu in cm
                        const Vector3f vel_neu_cms(ne_x, ne_y, -target_vel.z);
                        mode_guided.set_velocity(vel_neu_cms,true,ahrs_yaw_cd,false,0.0f,false,true);
                        if(zr_serial_api.get_param_debug()){
                            // zr_serial_api.print_data("set speed %.2f,%.2f,%.2f,yaw:%ld",ne_x,ne_y,target_vel.z,ahrs_yaw_cd);
                            snprintf(buf, sizeof(buf), "set speed %.2f,%.2f,%.2f,yaw:%ld",ne_x,ne_y,target_vel.z,ahrs_yaw_cd);
                            zr_serial_api.print_msg(buf);
                            memset(buf,0,50);
                        }
                    }
                    break;
                case AC_ZR_Serial_API::ZR_Msg_Type::MSG_CONTROL_VEL_LOCK_YAW:
                    if(motors->armed()){
                        Vector3f target_vel;
                        target_vel.x = 0;
                        target_vel.y = 0;
                        target_vel.z = 0;
                        // target_loc.alt = (data.z + 500)/100.0;
                        // set_target_velocity_NED(target_vel);
                        mode_guided.set_velocity(target_vel,false,ahrs_yaw_cd,false,0.0f,false,true);
                        if(zr_serial_api.get_param_debug()){

                            // zr_serial_api.print_data("set vel %.2f,%.2f,%.2f",target_vel.x,target_vel.y,target_vel.z);
                            snprintf(buf, sizeof(buf), "set vel %.2f,%.2f,%.2f,lock yaw",target_vel.x,target_vel.y,target_vel.z);
                            zr_serial_api.print_msg(buf);
                            memset(buf,0,50);
                        }
                    }
                    break;
                case AC_ZR_Serial_API::ZR_Msg_Type::MSG_CONTROL_MODE:
                {

                    if(control_mode != Mode::Number::GUIDED){
                        set_mode(Mode::Number::GUIDED, ModeReason::SCRIPTING);
                    }
                    uint8_t user_mode = (data.x);
                    if(user_mode == 3 || user_mode == 4 || user_mode == 6 || user_mode == 17){
                        bool change_ok = set_mode(user_mode, ModeReason::SCRIPTING);
    
                        if(zr_serial_api.get_param_debug() && change_ok){
                            // zr_serial_api.print_data("do takeoff,alt %.2f",tk_alt);
                            snprintf(buf, sizeof(buf), "Change mode %d",user_mode);
                            zr_serial_api.print_msg(buf);
                            memset(buf,0,50);
                        }
                    }else{
                        if(zr_serial_api.get_param_debug()){
                            // zr_serial_api.print_data("do takeoff,alt %.2f",tk_alt);
                            snprintf(buf, sizeof(buf), "Unsupported mode %d",user_mode);
                            zr_serial_api.print_msg(buf);
                            memset(buf,0,50);
                        }
                    }
                    
                }
                    break;
                    
            
                default:
                    break;
            }
        }
    }
    
}