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

无人机上锁更难状态码，起飞降落倒计时分开

master
yaozb 5 years ago
parent
59e0797af0
commit
0b250ceb63
8 changed files with 66 additions and 30 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. 25
      ArduCopter/Parameters.cpp
  2. 10
      ArduCopter/Parameters.h
  3. 9
      ArduCopter/UserCode.cpp
  4. 10
      ArduCopter/config.h
  5. 3
      ArduCopter/mode.h
  6. 12
      ArduCopter/mode_auto.cpp
  7. 13
      ArduCopter/mode_brake.cpp
  8. 12
      ArduCopter/mode_rtl.cpp

25
ArduCopter/Parameters.cpp
Unescape View File

@ -124,16 +124,33 @@ const AP_Param::Info Copter::var_info[] = { @@ -124,16 +124,33 @@ const AP_Param::Info Copter::var_info[] = {
// @Bitmask: 0:Roll,1:Pitch,2:Yaw,3:AccelZ
GSCALAR(gcs_pid_mask, "GCS_PID_MASK", 0),
// @Param: TAKEOFF_ALT_REQ
// @Param: ZR_TK_REQ_ALT
// @DisplayName: take off Altitude
// @Description:
// @Units: cm
// @Range: -1 5000
// @Range:0 5000
// @Increment: 1
// @User: Standard
GSCALAR(takeoff_alt_req, "TAKEOFF_ALT_REQ", TAKEOFF_ALT_REQ),
GSCALAR(zr_tk_req_alt, "ZR_TK_REQ_ALT", TAKEOFF_ALT_REQ),
// @Param: ZR_TK_DELAY
// @DisplayName: take off delay at ZR_TK_REQ_ALT
// @Description:
// @Units: second
// @Range: 0 360
// @Increment: 1
// @User: Standard
GSCALAR(zr_tk_delay, "ZR_TK_DELAY", ZR_TK_DELAY),
// @Param: ZR_RTL_DELAY
// @DisplayName: rtl Altitude when at final decent alt
// @Description:
// @Units: second
// @Range: 0 360
// @Increment: 1
// @User: Standard
GSCALAR(zr_rtl_delay, "ZR_RTL_DELAY", ZR_RTL_DELAY),
GSCALAR(updown_countdown, "UPDOWN_COUNTDOWN", UPDOWN_COUNTDOWN),
#if MODE_RTL_ENABLED == ENABLED
// @Param: RTL_ALT
// @DisplayName: RTL Altitude

10
ArduCopter/Parameters.h
Unescape View File

@ -386,8 +386,9 @@ public: @@ -386,8 +386,9 @@ public:
k_param_land_lock_alt,
k_param_land_lock_rpy,
k_param_takeoff_alt_req,
k_param_updown_countdown,
k_param_zr_tk_req_alt,
k_param_zr_tk_delay,
k_param_zr_rtl_delay,
// the k_param_* space is 9-bits in size
// 511: reserved
};
@ -496,8 +497,9 @@ public: @@ -496,8 +497,9 @@ public:
AP_Float acro_balance_pitch;
AP_Int8 acro_trainer;
AP_Float acro_rp_expo;
AP_Int16 takeoff_alt_req;
AP_Int16 updown_countdown;
AP_Int16 zr_tk_req_alt;
AP_Int16 zr_tk_delay;
AP_Int16 zr_rtl_delay;
// Note: keep initializers here in the same order as they are declared
// above.
Parameters()

9
ArduCopter/UserCode.cpp
Unescape View File

@ -82,8 +82,15 @@ void Copter::userhook_SuperSlowLoop() @@ -82,8 +82,15 @@ void Copter::userhook_SuperSlowLoop()
{
// put your 1Hz code here
static bool before_fly = true;
if(motors->armed())
if(motors->armed()){
before_fly = false;
}else{
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;
float delt_volt;

10
ArduCopter/config.h
Unescape View File

@ -537,7 +537,15 @@ @@ -537,7 +537,15 @@
#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.
#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
#ifndef ZR_TK_DELAY
#define ZR_TK_DELAY 5
#endif
#ifndef ZR_RTL_DELAY
#define ZR_RTL_DELAY 15
#endif
// RTL Mode

3
ArduCopter/mode.h
Unescape View File

@ -133,7 +133,8 @@ protected: @@ -133,7 +133,8 @@ protected:
float &G_Dt;
int16_t countdown ;
uint32_t last_takeoff_request_time;
bool is_countdown_enable = true;
bool is_takeoff_delay_enable = true;
bool is_rtl_delay_enable = true;
// note that we support two entirely different automatic takeoffs:
// "user-takeoff", which is available in modes such as ALT_HOLD

12
ArduCopter/mode_auto.cpp
Unescape View File

@ -768,24 +768,24 @@ void ModeAuto::takeoff_run() @@ -768,24 +768,24 @@ void ModeAuto::takeoff_run()
int32_t alt_rev = 0;
if (copter.current_loc.get_alt_cm(Location::AltFrame::ABOVE_TERRAIN, alt_rev))
{
if (g.takeoff_alt_req > 0 && alt_rev >= g.takeoff_alt_req
if (g.zr_tk_req_alt > 0 && alt_rev >= g.zr_tk_req_alt
&& copter.updownStatus < UpDown_RequestClimb)
{
copter.updownStatus = UpDown_RequestClimb;
countdown = g.updown_countdown;
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);
last_takeoff_request_time = AP_HAL::millis();
if(g.updown_countdown>0){
is_countdown_enable =true;
if(g.zr_tk_delay>0){
is_takeoff_delay_enable =true;
}else{
is_countdown_enable =false;
is_takeoff_delay_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
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))
{
last_time_send_updownload = AP_HAL::millis();

13
ArduCopter/mode_brake.cpp
Unescape View File

@ -24,13 +24,13 @@ bool ModeBrake::init(bool ignore_checks) @@ -24,13 +24,13 @@ bool ModeBrake::init(bool ignore_checks)
_timeout_ms = 0;
if(copter.updownStatus == UpDown_RequestClimb){
countdown = g.updown_countdown;
countdown = g.zr_tk_delay;
}
if(g.updown_countdown>0){
is_countdown_enable =true;
if(g.zr_tk_delay>0){
is_takeoff_delay_enable =true;
}else{
is_countdown_enable =false;
is_takeoff_delay_enable =false;
}
return true;
}
@ -76,12 +76,12 @@ void ModeBrake::run() @@ -76,12 +76,12 @@ void ModeBrake::run()
}
}
if(copter.updownStatus == UpDown_RequestClimb&&is_countdown_enable)
if(copter.updownStatus == UpDown_RequestClimb&&is_takeoff_delay_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;
@ -94,6 +94,7 @@ void ModeBrake::run() @@ -94,6 +94,7 @@ void ModeBrake::run()
copter.updownStatus = UpDown_ContinueClimb;
set_mode(Mode::Number::AUTO, ModeReason::REQUEST_CMD);
}
countdown --; //TODO Actually 1 second longer
}
}

12
ArduCopter/mode_rtl.cpp
Unescape View File

@ -72,21 +72,20 @@ void ModeRTL::run(bool disarm_on_land) @@ -72,21 +72,20 @@ void ModeRTL::run(bool disarm_on_land)
copter.updownStatus = UpDown_RequestDescent;
mavlink_zr_flying_status_t zr_flying_status_t;
zr_flying_status_t.updown_status = copter.updownStatus;
countdown = g.updown_countdown;
countdown = g.zr_rtl_delay;
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;
if(g.zr_rtl_delay>0){
is_rtl_delay_enable = true;
}else{
is_countdown_enable = false;
is_rtl_delay_enable = false;
}
}
else if(copter.updownStatus ==UpDown_RequestDescent&&is_countdown_enable)
else if(copter.updownStatus ==UpDown_RequestDescent&&is_rtl_delay_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;
@ -99,6 +98,7 @@ void ModeRTL::run(bool disarm_on_land) @@ -99,6 +98,7 @@ void ModeRTL::run(bool disarm_on_land)
copter.updownStatus = UpDown_ContinueDescent;
_state = RTL_Land;
}
countdown --; //TODO shiji countdown+1 s
}
}
else if (copter.updownStatus == UpDown_ContinueDescent)

Write Preview
Loading…
Cancel
Save