Browse Source

是否起飞判断调整,偏航角度调整

zr-v5.1
Brown.Z 3 years ago
parent
commit
02a1b504ae
  1. 4
      ArduCopter/zr_app.cpp

4
ArduCopter/zr_app.cpp

@ -25,7 +25,7 @@ void Copter::zr_app_10hz() @@ -25,7 +25,7 @@ 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(),(uint8_t)flightmode->is_landing(),home_distance(),now_volt,battery.capacity_remaining_pct());
zr_serial_api.get_vehicle_status((uint8_t)flightmode->mode_number(),!ap.land_complete,home_distance(),now_volt,battery.capacity_remaining_pct());
}
}
@ -64,7 +64,7 @@ void Copter::zr_app_50hz(){ @@ -64,7 +64,7 @@ void Copter::zr_app_50hz(){
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));
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);
}

Loading…
Cancel
Save