zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

起飞着陆添加倒计时

master
yaozb 5 years ago
parent
a6d437808a
commit
bd615a6892
11 changed files with 122 additions and 34 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
      ArduCopter/Parameters.cpp
  2. 8
      ArduCopter/Parameters.h
  3. 4
      ArduCopter/config.h
  4. 5
      ArduCopter/mode.cpp
  5. 14
      ArduCopter/mode.h
  6. 28
      ArduCopter/mode_auto.cpp
  7. 32
      ArduCopter/mode_brake.cpp
  8. 44
      ArduCopter/mode_rtl.cpp
  9. 4
      libraries/GCS_MAVLink/GCS.h
  10. 14
      libraries/GCS_MAVLink/GCS_Common.cpp
  11. 2
      modules/mavlink

1
ArduCopter/Parameters.cpp
Unescape View File

@ -131,6 +131,7 @@ const AP_Param::Info Copter::var_info[] = { @@ -131,6 +131,7 @@ const AP_Param::Info Copter::var_info[] = {
// @User: Standard
GSCALAR(takeoff_alt_req, "TAKEOFF_ALT_REQ", TAKEOFF_ALT_REQ),
GSCALAR(updown_countdown, "UPDOWN_COUNTDOWN", UPDOWN_COUNTDOWN),
#if MODE_RTL_ENABLED == ENABLED
// @Param: RTL_ALT
// @DisplayName: RTL Altitude

8
ArduCopter/Parameters.h
Unescape View File

@ -387,6 +387,7 @@ public: @@ -387,6 +387,7 @@ public:
k_param_land_lock_rpy,
k_param_takeoff_alt_req,
k_param_updown_countdown,
// the k_param_* space is 9-bits in size
// 511: reserved
};
@ -493,11 +494,10 @@ public: @@ -493,11 +494,10 @@ public:
AP_Float acro_yaw_p;
AP_Float acro_balance_roll;
AP_Float acro_balance_pitch;
AP_Int8 acro_trainer;
AP_Int8 acro_trainer;
AP_Float acro_rp_expo;
AP_Int16 takeoff_alt_req;
AP_Int16 takeoff_alt_req;
AP_Int16 updown_countdown;
// Note: keep initializers here in the same order as they are declared
// above.
Parameters()

4
ArduCopter/config.h
Unescape View File

@ -536,6 +536,10 @@ @@ -536,6 +536,10 @@
# define TAKEOFF_ALT_REQ 0 // the altitude the vehicle will move to as the final stage of Returning to Launch. Set to zero to land.
#endif
#ifndef UPDOWN_COUNTDOWN
# define UPDOWN_COUNTDOWN 20 // the altitude the vehicle will move to as the final stage of Returning to Launch. Set to zero to land.
#endif
// RTL Mode
#ifndef RTL_ALT_FINAL
# define RTL_ALT_FINAL 0 // the altitude the vehicle will move to as the final stage of Returning to Launch. Set to zero to land.

5
ArduCopter/mode.cpp
Unescape View File

@ -824,7 +824,10 @@ void Copter::update_updownStatus2(uint8_t status) @@ -824,7 +824,10 @@ void Copter::update_updownStatus2(uint8_t status)
if (status == 3)
{
set_mode(Mode::Number::AUTO, ModeReason::REQUEST_CMD);
gcs().update_zr_fly_status(status);
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);
}
}

14
ArduCopter/mode.h
Unescape View File

@ -7,9 +7,7 @@ class ParametersG2; @@ -7,9 +7,7 @@ class ParametersG2;
class GCS_Copter;
class Mode {
public:
// Auto Pilot Modes enumeration
enum class Number : uint8_t {
STABILIZE = 0, // manual airframe angle with manual throttle
@ -37,7 +35,6 @@ public: @@ -37,7 +35,6 @@ public:
SYSTEMID = 25, // System ID mode produces automated system identification signals in the controllers
AUTOROTATE = 26, // Autonomous autorotation
};
// constructor
Mode(void);
@ -134,7 +131,9 @@ protected: @@ -134,7 +131,9 @@ protected:
RC_Channel *&channel_throttle;
RC_Channel *&channel_yaw;
float &G_Dt;
int16_t countdown ;
uint32_t last_takeoff_request_time;
bool is_countdown_enable = true;
// note that we support two entirely different automatic takeoffs:
// "user-takeoff", which is available in modes such as ALT_HOLD
@ -331,7 +330,6 @@ public: @@ -331,7 +330,6 @@ public:
bool init(bool ignore_checks) override;
void run() override;
bool requires_GPS() const override { return true; }
bool has_manual_throttle() const override { return false; }
bool allows_arming(bool from_gcs) const override { return false; };
@ -480,6 +478,7 @@ private: @@ -480,6 +478,7 @@ private:
int32_t condition_value; // used in condition commands (eg delay, change alt, etc.)
uint32_t condition_start;
uint32_t last_time_send_updownload;
LandStateType land_state = LandStateType_FlyToLocation; // records state of land (flying to location, descending)
struct {
@ -1038,7 +1037,7 @@ protected: @@ -1038,7 +1037,7 @@ protected:
void land_run(bool disarm_on_land);
void set_descent_target_alt(uint32_t alt) { rtl_path.descent_target.alt = alt; }
private:
void climb_start();
@ -1048,10 +1047,11 @@ private: @@ -1048,10 +1047,11 @@ private:
void loiterathome_run();
void build_path();
void compute_return_target();
RTLState _state = RTL_InitialClimb; // records state of rtl (initial climb, returning home, etc)
bool _state_complete = false; // set to true if the current state is completed
uint32_t last_decent_time =0;
struct {
// NEU w/ Z element alt-above-ekf-origin unless use_terrain is true in which case Z element is alt-above-terrain
Location origin_point;

28
ArduCopter/mode_auto.cpp
Unescape View File

@ -238,7 +238,10 @@ void ModeAuto::wp_start(const Location& dest_loc) @@ -238,7 +238,10 @@ void ModeAuto::wp_start(const Location& dest_loc)
if (copter.updownStatus < UpDown_InMission)
{
copter.updownStatus = UpDown_InMission;
gcs().update_zr_fly_status(copter.updownStatus);
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);
}
}
@ -768,17 +771,30 @@ void ModeAuto::takeoff_run() @@ -768,17 +771,30 @@ void ModeAuto::takeoff_run()
if (g.takeoff_alt_req > 0 && alt_rev >= g.takeoff_alt_req
&& copter.updownStatus < UpDown_RequestClimb)
{
copter.updownStatus = UpDown_RequestClimb;
set_mode(Mode::Number::BRAKE, ModeReason::REQUEST_CMD);
gcs().update_zr_fly_status(copter.updownStatus);
copter.updownStatus = UpDown_RequestClimb;
countdown = g.updown_countdown;
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);
last_takeoff_request_time = AP_HAL::millis();
if(g.updown_countdown>0){
is_countdown_enable =true;
}else{
is_countdown_enable =false;
}
set_mode(Mode::Number::BRAKE, ModeReason::REQUEST_CMD);
}
else if (g.takeoff_alt_req > 0 && alt_rev < g.takeoff_alt_req && alt_rev > 0
&&(AP_HAL::millis()-last_time_send_updownload>500))
{
last_time_send_updownload = AP_HAL::millis();
copter.updownStatus = UpDown_TakeOffClimb;
gcs().update_zr_fly_status(copter.updownStatus);
}
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);
}
}
}
auto_takeoff_run();

32
ArduCopter/mode_brake.cpp
Unescape View File

@ -23,7 +23,15 @@ bool ModeBrake::init(bool ignore_checks) @@ -23,7 +23,15 @@ bool ModeBrake::init(bool ignore_checks)
}
_timeout_ms = 0;
if(copter.updownStatus == UpDown_RequestClimb){
countdown = g.updown_countdown;
}
if(g.updown_countdown>0){
is_countdown_enable =true;
}else{
is_countdown_enable =false;
}
return true;
}
@ -67,6 +75,28 @@ void ModeBrake::run() @@ -67,6 +75,28 @@ void ModeBrake::run()
copter.set_mode(Mode::Number::ALT_HOLD, ModeReason::BRAKE_TIMEOUT);
}
}
if(copter.updownStatus == UpDown_RequestClimb&&is_countdown_enable)
{
if((AP_HAL::millis()-last_takeoff_request_time)>1000)
{
last_takeoff_request_time = AP_HAL::millis();
countdown --;
// 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,"继续任务倒计时(秒)：%d",countdown);
}
else if(countdown<=0){
gcs().send_text(MAV_SEVERITY_INFO," 执行继续任务");
copter.updownStatus == UpDown_ContinueClimb;
set_mode(Mode::Number::AUTO, ModeReason::REQUEST_CMD);
}
}
}
}
void ModeBrake::timeout_to_loiter_ms(uint32_t timeout_ms)

44
ArduCopter/mode_rtl.cpp
Unescape View File

@ -70,10 +70,38 @@ void ModeRTL::run(bool disarm_on_land) @@ -70,10 +70,38 @@ void ModeRTL::run(bool disarm_on_land)
if (copter.updownStatus < UpDown_RequestDescent)
{
copter.updownStatus = UpDown_RequestDescent;
gcs().update_zr_fly_status(copter.updownStatus);
mavlink_zr_flying_status_t zr_flying_status_t;
zr_flying_status_t.updown_status = copter.updownStatus;
countdown = g.updown_countdown;
zr_flying_status_t.countdown = countdown;
gcs().update_zr_fly_status(&zr_flying_status_t);
last_decent_time = AP_HAL::millis();
if(g.updown_countdown>0){
is_countdown_enable = true;
}else{
is_countdown_enable = false;
}
}
if (copter.updownStatus == UpDown_ContinueDescent)
else if(copter.updownStatus ==UpDown_RequestDescent&&is_countdown_enable)
{
if((AP_HAL::millis()- last_decent_time)>=1000){
last_decent_time = AP_HAL::millis();
countdown --;
// 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,"着陆倒计时(秒): %d",countdown);
}
else if(countdown<=0){
gcs().send_text(MAV_SEVERITY_INFO,"开始着陆");
copter.updownStatus == UpDown_ContinueDescent;
_state = RTL_Land;
}
}
}
else if (copter.updownStatus == UpDown_ContinueDescent)
{
_state = RTL_Land;
}
@ -267,7 +295,10 @@ void ModeRTL::loiterathome_run() @@ -267,7 +295,10 @@ void ModeRTL::loiterathome_run()
void ModeRTL::descent_start()
{
copter.updownStatus = UpDown_RTLDescent;
gcs().update_zr_fly_status(copter.updownStatus);
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);
_state = RTL_FinalDescent;
_state_complete = false;
@ -409,7 +440,10 @@ void ModeRTL::land_run(bool disarm_on_land) @@ -409,7 +440,10 @@ void ModeRTL::land_run(bool disarm_on_land)
land_run_vertical_control();
if(AP_HAL::millis()-last_send_time>=500){
gcs().update_zr_fly_status(copter.updownStatus);
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);
}
}

4
libraries/GCS_MAVLink/GCS.h
Unescape View File

@ -219,7 +219,7 @@ public: @@ -219,7 +219,7 @@ 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 uint8_t flying_status);
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,7 @@ public: @@ -838,7 +838,7 @@ 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(uint8_t flying_status );
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

14
libraries/GCS_MAVLink/GCS_Common.cpp
Unescape View File

@ -1924,12 +1924,12 @@ void GCS::update_serial_battgo_history(mavlink_battgo_history_t *battgo_history_ @@ -1924,12 +1924,12 @@ void GCS::update_serial_battgo_history(mavlink_battgo_history_t *battgo_history_
}
}
void GCS::update_zr_fly_status(uint8_t flying_status)
void GCS::update_zr_fly_status(mavlink_zr_flying_status_t *zr_flying_status_t )
{
for (uint8_t i = 0; i < num_gcs(); i++)
{
chan(i)->send_zr_flying_status(flying_status);
}
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)
@ -4836,9 +4836,9 @@ void GCS_MAVLINK::handle_zr_fly_status(const mavlink_message_t &msg) @@ -4836,9 +4836,9 @@ void GCS_MAVLINK::handle_zr_fly_status(const mavlink_message_t &msg)
}
}
bool GCS_MAVLINK::send_zr_flying_status(const uint8_t flying_status)
bool GCS_MAVLINK::send_zr_flying_status(const mavlink_zr_flying_status_t *zr_flying_status_t)
{
mavlink_msg_zr_flying_status_send(chan, flying_status,0);
mavlink_msg_zr_flying_status_send_struct(chan, zr_flying_status_t);
return true;
}

2
modules/mavlink

@ -1 +1 @@ @@ -1 +1 @@
Subproject commit ca507c39ff56b88ce78a072a691e4b0c2a95f527
Subproject commit 18cab6950986cd7b9b7a507ec35f3501d4a7e019
Write Preview
Loading…
Cancel
Save