You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
243 lines
11 KiB
243 lines
11 KiB
#include "Copter.h" |
|
|
|
|
|
void Copter::zr_app_1hz() |
|
{ |
|
static bool before_fly = true; |
|
if(motors->armed()){ |
|
if(before_fly){ |
|
before_fly = false; |
|
} |
|
relay.on(3); // 电子开关，保持通路 |
|
// camera.set_in_arm_mode(true); |
|
}else{ |
|
relay.off(3); |
|
if(before_fly){ |
|
uint8_t bat_cnt = zr_app.get_battery_capacity(1,battery.voltage()); |
|
battery.reset_remaining(0,(float)bat_cnt ); |
|
// gcs().send_text(MAV_SEVERITY_INFO, "bat:%.2f,pst%d,%.2f",battery.voltage(),bat_cnt,(float)bat_cnt); |
|
} |
|
} |
|
} |
|
|
|
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)flightmode->mode_number(),!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){ |
|
zr_serial_api.update(); |
|
|
|
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)flightmode->mode_number() ,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); |
|
gcs().send_text(MAV_SEVERITY_INFO,"%s",buf); |
|
memset(buf,0,50); |
|
} |
|
switch (msg_type) |
|
{ |
|
case AC_ZR_Serial_API::ZR_Msg_Type::MSG_CONTROL_TKOFF: |
|
{ |
|
if(!motors->armed()){ |
|
if(arming.arm(AP_Arming::Method::SCRIPTING,false)){ |
|
gcs().send_text(MAV_SEVERITY_INFO,"arm by scripting"); |
|
}else{ |
|
gcs().send_text(MAV_SEVERITY_INFO,"arm faild"); |
|
} |
|
} |
|
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); |
|
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); |
|
gcs().send_text(MAV_SEVERITY_INFO,"%s",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); |
|
|
|
mode_guided.set_destination(target_loc, true, (float)ahrs_yaw_cd, true, 200.0, false); |
|
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); |
|
gcs().send_text(MAV_SEVERITY_INFO,"%s",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); |
|
gcs().send_text(MAV_SEVERITY_INFO,"%s",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); |
|
gcs().send_text(MAV_SEVERITY_INFO,"%s",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",target_vel.x,target_vel.y,target_vel.z); |
|
gcs().send_text(MAV_SEVERITY_INFO,"%s",buf); |
|
memset(buf,0,50); |
|
} |
|
} |
|
break; |
|
case AC_ZR_Serial_API::ZR_Msg_Type::MSG_CONTROL_MODE: |
|
{ |
|
|
|
if(flightmode->mode_number() != 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; |
|
} |
|
} |
|
} |
|
|
|
} |
|
|
|
|
|
|