diff --git a/APMrover2/GCS_Mavlink.h b/APMrover2/GCS_Mavlink.h index 72ca98af77..df5b27eb4a 100644 --- a/APMrover2/GCS_Mavlink.h +++ b/APMrover2/GCS_Mavlink.h @@ -20,6 +20,7 @@ protected: MAV_RESULT handle_command_long_packet(const mavlink_command_long_t &packet) override; void send_position_target_global_int() override; + void send_zr_flying_status() override; virtual bool in_hil_mode() const override; diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index 973cab218e..2d9103a6f4 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -111,6 +111,19 @@ void GCS_MAVLINK_Copter::send_position_target_global_int() 0.0f); // yaw_rate } +void GCS_MAVLINK_Copter::send_zr_flying_status(){ + + if (!copter.ap.initialised) { + return; + } + + mavlink_msg_zr_flying_status_send( + chan, + (uint8_t)copter.updownStatus, + 0//TODO change this param + ); +} + void GCS_MAVLINK_Copter::send_position_target_local_ned() { #if MODE_GUIDED_ENABLED == ENABLED diff --git a/ArduCopter/GCS_Mavlink.h b/ArduCopter/GCS_Mavlink.h index 9010523835..78564db45e 100644 --- a/ArduCopter/GCS_Mavlink.h +++ b/ArduCopter/GCS_Mavlink.h @@ -27,6 +27,8 @@ protected: void send_position_target_global_int() override; void send_position_target_local_ned() override; + void send_zr_flying_status() override; + MAV_RESULT handle_command_do_set_roi(const Location &roi_loc) override; MAV_RESULT handle_preflight_reboot(const mavlink_command_long_t &packet) override; MAV_RESULT handle_command_mount(const mavlink_command_long_t &packet) override; diff --git a/ArduCopter/UserCode.cpp b/ArduCopter/UserCode.cpp index 583ec8f5ec..316a4be1fc 100644 --- a/ArduCopter/UserCode.cpp +++ b/ArduCopter/UserCode.cpp @@ -79,6 +79,8 @@ static float batt_mah_teb[] = 19.88,19.73,19.53,19.34,19.19,19.00,18.80,18.51,18.00,17.40, 16.80 }; + +//uint8_t flying_status_count =0; void Copter::userhook_SuperSlowLoop() { // put your 1Hz code here @@ -96,10 +98,6 @@ void Copter::userhook_SuperSlowLoop() relay.off(3); // rc().set_enable_ch(); updownStatus =UpDown_TakeOffStart; - mavlink_zr_flying_status_t zr_flying_status_t; - zr_flying_status_t.updown_status = updownStatus; - //TODO UPDATE COUNTDOWN - gcs().update_zr_fly_status(&zr_flying_status_t); } if(before_fly){ uint8_t cnt,cacl_volt_pst; @@ -113,6 +111,13 @@ void Copter::userhook_SuperSlowLoop() cacl_volt_pst = 100 - cnt; battery.reset_remaining(1, cacl_volt_pst); } + + //2s发送一个状态码 + // flying_status_count++; + // if(flying_status_count>=2){ + // flying_status_count =0; + gcs().send_message(MSG_ZR_FLYING_STATUS); + // } } #endif diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index 32df8b98e4..807c9569c1 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -824,10 +824,7 @@ void Copter::update_updownStatus2(uint8_t status) if (status == 3) { set_mode(Mode::Number::AUTO, ModeReason::REQUEST_CMD); - mavlink_zr_flying_status_t zr_flying_status_t; - zr_flying_status_t.updown_status = copter.updownStatus; - zr_flying_status_t.countdown = 255; - gcs().update_zr_fly_status(&zr_flying_status_t); + gcs().send_message(MSG_ZR_FLYING_STATUS); gcs().send_text(MAV_SEVERITY_WARNING,"NOTICE: 执行继续任务"); } } diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index ae01bffa76..7cf23f32ca 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -238,10 +238,7 @@ void ModeAuto::wp_start(const Location& dest_loc) if (copter.updownStatus < UpDown_InMission) { copter.updownStatus = UpDown_InMission; - mavlink_zr_flying_status_t zr_flying_status_t; - zr_flying_status_t.updown_status = copter.updownStatus; - zr_flying_status_t.countdown = 255; - gcs().update_zr_fly_status(&zr_flying_status_t); + gcs().send_message(MSG_ZR_FLYING_STATUS); } } @@ -773,11 +770,10 @@ void ModeAuto::takeoff_run() && copter.updownStatus < UpDown_RequestClimb) { copter.updownStatus = UpDown_RequestClimb; - countdown = g.zr_tk_delay; - mavlink_zr_flying_status_t zr_flying_status_t; - zr_flying_status_t.updown_status = copter.updownStatus; - zr_flying_status_t.countdown = countdown; - gcs().update_zr_fly_status(&zr_flying_status_t); + countdown = g.zr_tk_delay; + + gcs().send_message(MSG_ZR_FLYING_STATUS); + last_takeoff_request_time = AP_HAL::millis(); if(g.zr_tk_delay>0){ is_takeoff_delay_enable =true; @@ -785,17 +781,15 @@ void ModeAuto::takeoff_run() is_takeoff_delay_enable =false; } set_mode(Mode::Number::BRAKE, ModeReason::REQUEST_CMD); - } + } + else if (g.zr_tk_req_alt > 0 && alt_rev < g.zr_tk_req_alt && alt_rev > 0 - &&(AP_HAL::millis()-last_time_send_updownload>500)) + &&(AP_HAL::millis()-last_time_send_updownload>1000)) { last_time_send_updownload = AP_HAL::millis(); copter.updownStatus = UpDown_TakeOffClimb; - mavlink_zr_flying_status_t zr_flying_status_t; - zr_flying_status_t.updown_status = copter.updownStatus; - zr_flying_status_t.countdown = 255; - gcs().update_zr_fly_status(&zr_flying_status_t); + // gcs().send_message(MSG_ZR_FLYING_STATUS); //TODO } } } diff --git a/ArduCopter/mode_brake.cpp b/ArduCopter/mode_brake.cpp index 8b3a326b46..63dc22d895 100644 --- a/ArduCopter/mode_brake.cpp +++ b/ArduCopter/mode_brake.cpp @@ -82,10 +82,6 @@ void ModeBrake::run() { last_takeoff_request_time = AP_HAL::millis(); - // mavlink_zr_flying_status_t zr_flying_status_t; - // zr_flying_status_t.updown_status = copter.updownStatus; - // zr_flying_status_t.countdown = countdown; - // gcs().update_zr_fly_status(&zr_flying_status_t); if(countdown>0){ gcs().send_text(MAV_SEVERITY_INFO,"NOTICE: 继续任务倒计时(秒):%d",countdown); } diff --git a/ArduCopter/mode_rtl.cpp b/ArduCopter/mode_rtl.cpp index dee89779b5..f5eb039339 100644 --- a/ArduCopter/mode_rtl.cpp +++ b/ArduCopter/mode_rtl.cpp @@ -70,11 +70,8 @@ void ModeRTL::run(bool disarm_on_land) if (copter.updownStatus < UpDown_RequestDescent) { copter.updownStatus = UpDown_RequestDescent; - mavlink_zr_flying_status_t zr_flying_status_t; - zr_flying_status_t.updown_status = copter.updownStatus; - countdown = g.zr_rtl_delay; - zr_flying_status_t.countdown = countdown; - gcs().update_zr_fly_status(&zr_flying_status_t); + countdown = g.zr_rtl_delay; + gcs().send_message(MSG_ZR_FLYING_STATUS); last_decent_time = AP_HAL::millis(); if(g.zr_rtl_delay>0){ is_rtl_delay_enable = true; @@ -86,16 +83,13 @@ void ModeRTL::run(bool disarm_on_land) { if((AP_HAL::millis()- last_decent_time)>=1000){ last_decent_time = AP_HAL::millis(); - // mavlink_zr_flying_status_t zr_flying_status_t; - // zr_flying_status_t.updown_status = copter.updownStatus; - // zr_flying_status_t.countdown = countdown; - // gcs().update_zr_fly_status(&zr_flying_status_t); if(countdown>0){ gcs().send_text(MAV_SEVERITY_INFO ,"NOTICE: 着陆倒计时(秒): %d",countdown); } else if(countdown<=0){ gcs().send_text(MAV_SEVERITY_INFO,"NOTICE: 开始着陆"); copter.updownStatus = UpDown_ContinueDescent; + gcs().send_message(MSG_ZR_FLYING_STATUS); _state = RTL_Land; } countdown --; //TODO shiji countdown+1 s @@ -296,10 +290,7 @@ void ModeRTL::loiterathome_run() void ModeRTL::descent_start() { copter.updownStatus = UpDown_RTLDescent; - mavlink_zr_flying_status_t zr_flying_status_t; - zr_flying_status_t.updown_status = copter.updownStatus; - zr_flying_status_t.countdown = 255; - gcs().update_zr_fly_status(&zr_flying_status_t); + gcs().send_message(MSG_ZR_FLYING_STATUS); _state = RTL_FinalDescent; _state_complete = false; @@ -439,13 +430,6 @@ void ModeRTL::land_run(bool disarm_on_land) land_run_horizontal_control(); land_run_vertical_control(); - - if(AP_HAL::millis()-last_send_time>=500){ - mavlink_zr_flying_status_t zr_flying_status_t; - zr_flying_status_t.updown_status = copter.updownStatus; - zr_flying_status_t.countdown = 255; - gcs().update_zr_fly_status(&zr_flying_status_t); - } } void ModeRTL::build_path() diff --git a/ArduPlane/GCS_Mavlink.h b/ArduPlane/GCS_Mavlink.h index dd7676c594..6f679cb3c9 100644 --- a/ArduPlane/GCS_Mavlink.h +++ b/ArduPlane/GCS_Mavlink.h @@ -25,7 +25,7 @@ protected: MAV_RESULT handle_command_long_packet(const mavlink_command_long_t &packet) override; void send_position_target_global_int() override; - + void send_zr_flying_status() override; virtual bool in_hil_mode() const override; void send_attitude() const override; diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Serial_BattGo.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_Serial_BattGo.cpp index 004f8bf732..349b7cb323 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Serial_BattGo.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Serial_BattGo.cpp @@ -279,7 +279,7 @@ void AP_BattMonitor_Serial_BattGo::read() void AP_BattMonitor_Serial_BattGo::notice_to_send_battgo_param() { - gcs().send_message(MSG_BATTGAO_INFO); + gcs().send_message(MSG_BATTGO_INFO); } void AP_BattMonitor_Serial_BattGo::request_info() diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index 950737a560..52f5005701 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -211,6 +211,7 @@ public: void send_gps_global_origin() const; virtual void send_position_target_global_int() { }; virtual void send_position_target_local_ned() { }; + virtual void send_zr_flying_status(){}; void send_servo_output_raw(); void send_accelcal_vehicle_position(uint32_t position); void send_scaled_imu(uint8_t instance, void (*send_fn)(mavlink_channel_t chan, uint32_t time_ms, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag, int16_t temperature)); @@ -219,7 +220,6 @@ public: void send_rpm() const; bool send_battgo_info(const mavlink_battgo_info_t *mavlink_battgo_info_t); bool send_battgo_history(const mavlink_battgo_history_t *mavlink_battgo_info_t); - bool send_zr_flying_status(const mavlink_zr_flying_status_t *zr_flying_status_t); //void send_serial_battgo(mavlink_battgo_info_t &battgo_info_t ); bool locked() const; @@ -838,7 +838,6 @@ public: void update_serial_battgo_info(mavlink_battgo_info_t *battgo_info_t ); void update_serial_battgo_history(mavlink_battgo_history_t *battgo_history_t ); - void update_zr_fly_status(mavlink_zr_flying_status_t *zr_flying_status_t ); // minimum amount of time (in microseconds) that must remain in // the main scheduler loop before we are allowed to send any // mavlink messages. We want to prioritise the main flight diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 597bdee8c2..642a29b35a 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -256,13 +256,13 @@ bool GCS_MAVLINK::send_battery_status() const } bool GCS_MAVLINK::send_battgo_info(const mavlink_battgo_info_t *battgo_info_t) { - mavlink_msg_battgo_info_send_struct(chan, battgo_info_t); + mavlink_msg_battgo_info_send_struct(chan, battgo_info_t); //TODO return true; } bool GCS_MAVLINK::send_battgo_history(const mavlink_battgo_history_t *battgo_history_t) { - mavlink_msg_battgo_history_send_struct(chan, battgo_history_t); + mavlink_msg_battgo_history_send_struct(chan, battgo_history_t);//TODO return true; } void GCS_MAVLINK::send_distance_sensor(const AP_RangeFinder_Backend *sensor, const uint8_t instance) const @@ -795,6 +795,7 @@ ap_message GCS_MAVLINK::mavlink_id_to_ap_message_id(const uint32_t mavlink_id) c { MAVLINK_MSG_ID_DEEPSTALL, MSG_LANDING}, { MAVLINK_MSG_ID_EXTENDED_SYS_STATE, MSG_EXTENDED_SYS_STATE}, { MAVLINK_MSG_ID_AUTOPILOT_VERSION, MSG_AUTOPILOT_VERSION}, + { MAVLINK_MSG_ID_ZR_FLYING_STATUS, MSG_ZR_FLYING_STATUS}, }; for (uint8_t i=0; isend_zr_flying_status(zr_flying_status_t); - } -} - void GCS::send_mission_item_reached_message(uint16_t mission_index) { for (uint8_t i=0; i