zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

修改状态码mavlink发送为主流方式,降落减少发送

mission-4.1.18
yaozb 5 years ago
parent
c6b43b7eef
commit
c4a1cf0cb2
13 changed files with 54 additions and 68 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 1
      APMrover2/GCS_Mavlink.h
  2. 13
      ArduCopter/GCS_Mavlink.cpp
  3. 2
      ArduCopter/GCS_Mavlink.h
  4. 13
      ArduCopter/UserCode.cpp
  5. 5
      ArduCopter/mode.cpp
  6. 24
      ArduCopter/mode_auto.cpp
  7. 4
      ArduCopter/mode_brake.cpp
  8. 24
      ArduCopter/mode_rtl.cpp
  9. 2
      ArduPlane/GCS_Mavlink.h
  10. 2
      libraries/AP_BattMonitor/AP_BattMonitor_Serial_BattGo.cpp
  11. 3
      libraries/GCS_MAVLink/GCS.h
  12. 24
      libraries/GCS_MAVLink/GCS_Common.cpp
  13. 5
      libraries/GCS_MAVLink/ap_message.h

1
APMrover2/GCS_Mavlink.h
Unescape View File

@ -20,6 +20,7 @@ protected: @@ -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;

13
ArduCopter/GCS_Mavlink.cpp
Unescape View File

@ -111,6 +111,19 @@ void GCS_MAVLINK_Copter::send_position_target_global_int() @@ -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

2
ArduCopter/GCS_Mavlink.h
Unescape View File

@ -27,6 +27,8 @@ protected: @@ -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;

13
ArduCopter/UserCode.cpp
Unescape View File

@ -79,6 +79,8 @@ static float batt_mah_teb[] = @@ -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() @@ -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() @@ -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

5
ArduCopter/mode.cpp
Unescape View File

@ -824,10 +824,7 @@ void Copter::update_updownStatus2(uint8_t status) @@ -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: 执行继续任务");
}
}

24
ArduCopter/mode_auto.cpp
Unescape View File

@ -238,10 +238,7 @@ void ModeAuto::wp_start(const Location& dest_loc) @@ -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() @@ -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() @@ -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
}
}
}

4
ArduCopter/mode_brake.cpp
Unescape View File

@ -82,10 +82,6 @@ void ModeBrake::run() @@ -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);
}

24
ArduCopter/mode_rtl.cpp
Unescape View File

@ -70,11 +70,8 @@ void ModeRTL::run(bool disarm_on_land) @@ -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) @@ -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() @@ -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) @@ -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()

2
ArduPlane/GCS_Mavlink.h
Unescape View File

@ -25,7 +25,7 @@ protected: @@ -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;

2
libraries/AP_BattMonitor/AP_BattMonitor_Serial_BattGo.cpp
Unescape View File

@ -279,7 +279,7 @@ void AP_BattMonitor_Serial_BattGo::read() @@ -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()

3
libraries/GCS_MAVLink/GCS.h
Unescape View File

@ -211,6 +211,7 @@ public: @@ -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: @@ -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: @@ -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

24
libraries/GCS_MAVLink/GCS_Common.cpp
Unescape View File

@ -256,13 +256,13 @@ bool GCS_MAVLINK::send_battery_status() const @@ -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 @@ -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; i<ARRAY_SIZE(map); i++) {
@ -1924,14 +1925,6 @@ void GCS::update_serial_battgo_history(mavlink_battgo_history_t *battgo_history_ @@ -1924,14 +1925,6 @@ void GCS::update_serial_battgo_history(mavlink_battgo_history_t *battgo_history_
}
}
void GCS::update_zr_fly_status(mavlink_zr_flying_status_t *zr_flying_status_t )
{
for (uint8_t i=0; i<num_gcs(); i++) {
// mavlink_msg_battgo_info_send_struct(chan,&battgo_info_t);
chan(i)->send_zr_flying_status(zr_flying_status_t);
}
}
void GCS::send_mission_item_reached_message(uint16_t mission_index)
{
for (uint8_t i=0; i<num_gcs(); i++) {
@ -4491,6 +4484,11 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id) @@ -4491,6 +4484,11 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id)
send_autopilot_version();
break;
case MSG_ZR_FLYING_STATUS:
CHECK_PAYLOAD_SIZE(ZR_FLYING_STATUS);
send_zr_flying_status();
break;
case MSG_ESC_TELEMETRY: {
#ifdef HAVE_AP_BLHELI_SUPPORT
CHECK_PAYLOAD_SIZE(ESC_TELEMETRY_1_TO_4);
@ -4836,12 +4834,6 @@ void GCS_MAVLINK::handle_zr_fly_status(const mavlink_message_t &msg) @@ -4836,12 +4834,6 @@ void GCS_MAVLINK::handle_zr_fly_status(const mavlink_message_t &msg)
}
}
bool GCS_MAVLINK::send_zr_flying_status(const mavlink_zr_flying_status_t *zr_flying_status_t)
{
mavlink_msg_zr_flying_status_send_struct(chan, zr_flying_status_t);
return true;
}
GCS &gcs()
{
return *GCS::get_singleton();

5
libraries/GCS_MAVLink/ap_message.h
Unescape View File

@ -73,6 +73,9 @@ enum ap_message : uint8_t { @@ -73,6 +73,9 @@ enum ap_message : uint8_t {
MSG_NAMED_FLOAT,
MSG_EXTENDED_SYS_STATE,
MSG_AUTOPILOT_VERSION,
MSG_BATTGAO_INFO,
MSG_ZR_FLYING_STATUS,
MSG_ZR_CAMERA_STATUS,
MSG_BATTGO_INFO,
MSG_BATTGO_HISTORY,
MSG_LAST // MSG_LAST must be the last entry in this enum
};

Write Preview
Loading…
Cancel
Save