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

Merge branch 'landtest' into zr-v4.0.4-dev

master
yaozb 5 years ago
parent
0c80a9843b 0b250ceb63
commit
352121e16a
12 changed files with 165 additions and 41 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. 24
      ArduCopter/Parameters.cpp
  2. 12
      ArduCopter/Parameters.h
  3. 9
      ArduCopter/UserCode.cpp
  4. 12
      ArduCopter/config.h
  5. 5
      ArduCopter/mode.cpp
  6. 11
      ArduCopter/mode.h
  7. 28
      ArduCopter/mode_auto.cpp
  8. 33
      ArduCopter/mode_brake.cpp
  9. 44
      ArduCopter/mode_rtl.cpp
  10. 4
      libraries/GCS_MAVLink/GCS.h
  11. 12
      libraries/GCS_MAVLink/GCS_Common.cpp
  12. 2
      modules/mavlink

24
ArduCopter/Parameters.cpp
Unescape View File

@ -124,14 +124,32 @@ const AP_Param::Info Copter::var_info[] = { @@ -124,14 +124,32 @@ 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),
#if MODE_RTL_ENABLED == ENABLED
// @Param: RTL_ALT

12
ArduCopter/Parameters.h
Unescape View File

@ -386,7 +386,9 @@ public: @@ -386,7 +386,9 @@ public:
k_param_land_lock_alt,
k_param_land_lock_rpy,
k_param_takeoff_alt_req,
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
};
@ -493,11 +495,11 @@ public: @@ -493,11 +495,11 @@ 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 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;

12
ArduCopter/config.h
Unescape View File

@ -536,6 +536,18 @@ @@ -536,6 +536,18 @@
# 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
#ifndef ZR_TK_DELAY
#define ZR_TK_DELAY 5
#endif
#ifndef ZR_RTL_DELAY
#define ZR_RTL_DELAY 15
#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);
}
}

11
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,10 @@ protected: @@ -134,7 +131,10 @@ protected:
RC_Channel *&channel_throttle;
RC_Channel *&channel_yaw;
float &G_Dt;
int16_t countdown ;
uint32_t last_takeoff_request_time;
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
@ -331,7 +331,6 @@ public: @@ -331,7 +331,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 +479,7 @@ private: @@ -480,6 +479,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 {
@ -1052,6 +1052,7 @@ private: @@ -1052,6 +1052,7 @@ private:
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);
}
}
@ -765,19 +768,32 @@ void ModeAuto::takeoff_run() @@ -765,19 +768,32 @@ 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;
set_mode(Mode::Number::BRAKE, ModeReason::REQUEST_CMD);
gcs().update_zr_fly_status(copter.updownStatus);
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.zr_tk_delay>0){
is_takeoff_delay_enable =true;
}else{
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();
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);
}
}
}

33
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.zr_tk_delay;
}
if(g.zr_tk_delay>0){
is_takeoff_delay_enable =true;
}else{
is_takeoff_delay_enable =false;
}
return true;
}
@ -67,6 +75,29 @@ void ModeBrake::run() @@ -67,6 +75,29 @@ void ModeBrake::run()
copter.set_mode(Mode::Number::ALT_HOLD, ModeReason::BRAKE_TIMEOUT);
}
}
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();
// 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);
}
countdown --; //TODO Actually 1 second longer
}
}
}
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.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.zr_rtl_delay>0){
is_rtl_delay_enable = true;
}else{
is_rtl_delay_enable = false;
}
}
if (copter.updownStatus == UpDown_ContinueDescent)
else if(copter.updownStatus ==UpDown_RequestDescent&&is_rtl_delay_enable)
{
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,"着陆倒计时(秒): %d",countdown);
}
else if(countdown<=0){
gcs().send_text(MAV_SEVERITY_INFO,"开始着陆");
copter.updownStatus = UpDown_ContinueDescent;
_state = RTL_Land;
}
countdown --; //TODO shiji countdown+1 s
}
}
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

12
libraries/GCS_MAVLink/GCS_Common.cpp
Unescape View File

@ -1924,11 +1924,11 @@ void GCS::update_serial_battgo_history(mavlink_battgo_history_t *battgo_history_ @@ -1924,11 +1924,11 @@ 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);
}
}
@ -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