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

GPS change, Flow rotation

master
z 5 years ago
parent
dfe43b2a10
commit
d716d9a258
9 changed files with 36 additions and 21 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. 10
      ArduCopter/UserCode.cpp
  2. 2
      ArduCopter/mode_brake.cpp
  3. 2
      ArduCopter/mode_rtl.cpp
  4. 2
      ArduCopter/version.h
  5. 9
      libraries/AP_GPS/AP_GPS.cpp
  6. 1
      libraries/AP_GPS/AP_GPS.h
  7. 12
      libraries/AP_GPS/AP_GPS_UBLOX.cpp
  8. 9
      libraries/AP_OpticalFlow/AP_OpticalFlow_HereFlow.cpp
  9. 10
      zr_version

10
ArduCopter/UserCode.cpp
Unescape View File

@ -99,16 +99,8 @@ void Copter::userhook_SuperSlowLoop() @@ -99,16 +99,8 @@ void Copter::userhook_SuperSlowLoop()
break;
}
cacl_volt_pst = 100 - cnt;
battery.reset_remaining(0, cacl_volt_pst);
battery.reset_remaining(1, cacl_volt_pst);
}
// // check AHRS and GPS are within 10m of each other
// const Location gps_loc = gps.location();
// Location ahrs_loc;
// if (AP::ahrs().get_position(ahrs_loc)) {
// const float distance = gps_loc.get_distance(ahrs_loc);
// gcs().send_text(MAV_SEVERITY_INFO, "GPS and AHRS differ by %4.1fm", (double)distance);
// }
}
#endif

2
ArduCopter/mode_brake.cpp
Unescape View File

@ -91,7 +91,7 @@ void ModeBrake::run() @@ -91,7 +91,7 @@ void ModeBrake::run()
}
else if(countdown<=0){
gcs().send_text(MAV_SEVERITY_INFO," 执行继续任务");
copter.updownStatus == UpDown_ContinueClimb;
copter.updownStatus = UpDown_ContinueClimb;
set_mode(Mode::Number::AUTO, ModeReason::REQUEST_CMD);
}
}

2
ArduCopter/mode_rtl.cpp
Unescape View File

@ -96,7 +96,7 @@ void ModeRTL::run(bool disarm_on_land) @@ -96,7 +96,7 @@ void ModeRTL::run(bool disarm_on_land)
}
else if(countdown<=0){
gcs().send_text(MAV_SEVERITY_INFO,"开始着陆");
copter.updownStatus == UpDown_ContinueDescent;
copter.updownStatus = UpDown_ContinueDescent;
_state = RTL_Land;
}
}

2
ArduCopter/version.h
Unescape View File

@ -6,7 +6,7 @@ @@ -6,7 +6,7 @@
#include "ap_version.h"
#define THISFIRMWARE "ZRUAV v4.0.4" //"ArduCopter V4.0.0"
#define THISFIRMWARE "ZRUAV v4.0.4-rc1.0" //"ArduCopter V4.0.0"
// the following line is parsed by the autotest scripts
#define FIRMWARE_VERSION 4,0,4,FIRMWARE_VERSION_TYPE_OFFICIAL

9
libraries/AP_GPS/AP_GPS.cpp
Unescape View File

@ -282,6 +282,9 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = { @@ -282,6 +282,9 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = {
// @Range: 5.0 30.0
// @User: Advanced
AP_GROUPINFO("BLEND_TC", 21, AP_GPS, _blend_tc, 10.0f),
AP_GROUPINFO("NUM_SW", 22, AP_GPS, _nunber_to_switch, 5),
#endif
AP_GROUPEND
@ -782,7 +785,7 @@ void AP_GPS::update(void) @@ -782,7 +785,7 @@ void AP_GPS::update(void)
for (uint8_t i=1; i<GPS_MAX_RECEIVERS; i++) {
// choose GPS with highest state or higher number of satellites
if ((state[i].status > state[primary_instance].status) ||
((state[i].status == state[primary_instance].status) && (state[i].num_sats > state[primary_instance].num_sats))) {
((state[i].status == state[primary_instance].status) && (state[i].num_sats > state[primary_instance].num_sats + _nunber_to_switch))) {
primary_instance = i;
_last_instance_swap_ms = now;
}
@ -800,11 +803,11 @@ void AP_GPS::update(void) @@ -800,11 +803,11 @@ void AP_GPS::update(void)
continue;
}
bool another_gps_has_1_or_more_sats = (state[i].num_sats >= state[primary_instance].num_sats + 1);
bool another_gps_has_1_or_more_sats = (state[i].num_sats >= state[primary_instance].num_sats + _nunber_to_switch);
if (state[i].status == state[primary_instance].status && another_gps_has_1_or_more_sats) {
bool another_gps_has_2_or_more_sats = (state[i].num_sats >= state[primary_instance].num_sats + 2);
bool another_gps_has_2_or_more_sats = (state[i].num_sats >= state[primary_instance].num_sats + _nunber_to_switch + 2);
if ((another_gps_has_1_or_more_sats && (now - _last_instance_swap_ms) >= 20000) ||
(another_gps_has_2_or_more_sats && (now - _last_instance_swap_ms) >= 5000)) {

1
libraries/AP_GPS/AP_GPS.h
Unescape View File

@ -458,6 +458,7 @@ protected: @@ -458,6 +458,7 @@ protected:
AP_Int16 _delay_ms[GPS_MAX_RECEIVERS];
AP_Int8 _blend_mask;
AP_Float _blend_tc;
AP_Int8 _nunber_to_switch;
uint32_t _log_gps_bit = -1;

12
libraries/AP_GPS/AP_GPS_UBLOX.cpp
Unescape View File

@ -1024,7 +1024,8 @@ AP_GPS_UBLOX::_parse_gps(void) @@ -1024,7 +1024,8 @@ AP_GPS_UBLOX::_parse_gps(void)
if (_buffer.status.fix_status & NAV_STATUS_FIX_VALID) {
if( (_buffer.status.fix_type == AP_GPS_UBLOX::FIX_3D) &&
(_buffer.status.fix_status & AP_GPS_UBLOX::NAV_STATUS_DGPS_USED)) {
next_fix = AP_GPS::GPS_OK_FIX_3D_DGPS;
// next_fix = AP_GPS::GPS_OK_FIX_3D_DGPS;
next_fix = AP_GPS::GPS_OK_FIX_3D;
}else if( _buffer.status.fix_type == AP_GPS_UBLOX::FIX_3D) {
next_fix = AP_GPS::GPS_OK_FIX_3D;
}else if (_buffer.status.fix_type == AP_GPS_UBLOX::FIX_2D) {
@ -1065,7 +1066,8 @@ AP_GPS_UBLOX::_parse_gps(void) @@ -1065,7 +1066,8 @@ AP_GPS_UBLOX::_parse_gps(void)
if (_buffer.solution.fix_status & NAV_STATUS_FIX_VALID) {
if( (_buffer.solution.fix_type == AP_GPS_UBLOX::FIX_3D) &&
(_buffer.solution.fix_status & AP_GPS_UBLOX::NAV_STATUS_DGPS_USED)) {
next_fix = AP_GPS::GPS_OK_FIX_3D_DGPS;
// next_fix = AP_GPS::GPS_OK_FIX_3D_DGPS;
next_fix = AP_GPS::GPS_OK_FIX_3D;
}else if( _buffer.solution.fix_type == AP_GPS_UBLOX::FIX_3D) {
next_fix = AP_GPS::GPS_OK_FIX_3D;
}else if (_buffer.solution.fix_type == AP_GPS_UBLOX::FIX_2D) {
@ -1166,8 +1168,10 @@ AP_GPS_UBLOX::_parse_gps(void) @@ -1166,8 +1168,10 @@ AP_GPS_UBLOX::_parse_gps(void)
break;
case 3:
state.status = AP_GPS::GPS_OK_FIX_3D;
if (_buffer.pvt.flags & 0b00000010) // diffsoln
state.status = AP_GPS::GPS_OK_FIX_3D_DGPS;
if (_buffer.pvt.flags & 0b00000010){ // diffsoln
// state.status = AP_GPS::GPS_OK_FIX_3D_DGPS;
state.status = AP_GPS::GPS_OK_FIX_3D;
}
if (_buffer.pvt.flags & 0b01000000) // carrsoln - float
state.status = AP_GPS::GPS_OK_FIX_3D_RTK_FLOAT;
if (_buffer.pvt.flags & 0b10000000) // carrsoln - fixed

9
libraries/AP_OpticalFlow/AP_OpticalFlow_HereFlow.cpp
Unescape View File

@ -66,8 +66,13 @@ void AP_OpticalFlow_HereFlow::handle_measurement(AP_UAVCAN* ap_uavcan, uint8_t n @@ -66,8 +66,13 @@ void AP_OpticalFlow_HereFlow::handle_measurement(AP_UAVCAN* ap_uavcan, uint8_t n
if (_ap_uavcan == ap_uavcan && _node_id == node_id) {
WITH_SEMAPHORE(_driver->_sem);
_driver->new_data = true;
_driver->flowRate = Vector2f(cb.msg->flow_integral[0], cb.msg->flow_integral[1]);
_driver->bodyRate = Vector2f(cb.msg->rate_gyro_integral[0], cb.msg->rate_gyro_integral[1]);
if(0){
_driver->flowRate = Vector2f(cb.msg->flow_integral[0], cb.msg->flow_integral[1]);
_driver->bodyRate = Vector2f(cb.msg->rate_gyro_integral[0], cb.msg->rate_gyro_integral[1]);
}else{
_driver->flowRate = Vector2f(-cb.msg->flow_integral[1], cb.msg->flow_integral[0]);
_driver->bodyRate = Vector2f(-cb.msg->rate_gyro_integral[1], cb.msg->rate_gyro_integral[0]);
}
_driver->integral_time = cb.msg->integration_interval;
_driver->surface_quality = cb.msg->quality;
}

10
zr_version
Unescape View File

@ -0,0 +1,10 @@ @@ -0,0 +1,10 @@
zr-v4.0: 经过飞行测试，稳定版本
zr-v4.0-dev: 新添加的功能，并入主分支前先验证
zr-v4.0.1:
zr-dev-4.0.2: NMEA 时间修复，日志目录名
v4.0.3: 快速解锁
v4.0.4： 分阶段降落，光流门限，限制GPS切换 _nunber_to_switch,
Write Preview
Loading…
Cancel
Save