Browse Source

battgo,arming,flow

master
z 5 years ago
parent
commit
9cc70f84c6
  1. 2
      ArduCopter/AP_Arming.cpp
  2. 8
      ArduCopter/Parameters.cpp
  3. 2
      ArduCopter/UserCode.cpp
  4. 3
      libraries/AP_BattMonitor/AP_BattMonitor.cpp
  5. 9
      libraries/AP_OpticalFlow/OpticalFlow.cpp

2
ArduCopter/AP_Arming.cpp

@ -457,7 +457,7 @@ bool AP_Arming_Copter::mandatory_deadline_checks(bool display_failure) @@ -457,7 +457,7 @@ bool AP_Arming_Copter::mandatory_deadline_checks(bool display_failure)
{
const AP_GPS &gps = AP::gps();
if(gps.status()<2)
return false;
return true;
if (!copter.deadline_ok())
{
int32_t deadline = 0;

8
ArduCopter/Parameters.cpp

@ -1637,16 +1637,16 @@ const char* Copter::get_sysid_board_id(void) @@ -1637,16 +1637,16 @@ const char* Copter::get_sysid_board_id(void)
switch (type)
{
case 0:
snprintf(buf, sizeof(buf), "Version: zr-v4.0.3 ,ID: RS100%04d%05d",(int)nameValue1,(int)nameValue2);
snprintf(buf, sizeof(buf), "Version: zr-v4.0.4 ,ID: RS100%04d%05d",(int)nameValue1,(int)nameValue2);
break;
case 1:
snprintf(buf, sizeof(buf), "Version: zr-v4.0.3 ,ID: M660%04d%05d",(int)nameValue1,(int)nameValue2);
snprintf(buf, sizeof(buf), "Version: zr-v4.0.4 ,ID: M660%04d%05d",(int)nameValue1,(int)nameValue2);
break;
case 2:
snprintf(buf, sizeof(buf), "Version: zr-v4.0.3 ,ID: RF610%04d%05d",(int)nameValue1,(int)nameValue2);
snprintf(buf, sizeof(buf), "Version: zr-v4.0.4 ,ID: RF610%04d%05d",(int)nameValue1,(int)nameValue2);
break;
case 3:
snprintf(buf, sizeof(buf), "Version: zr-v4.0.3 ,Board ID: ZRZK.20QT2.%d",(int)nameValue2);
snprintf(buf, sizeof(buf), "Version: zr-v4.0.4 ,Board ID: ZRZK.20QT2.%d",(int)nameValue2);
break;
default:

2
ArduCopter/UserCode.cpp

@ -94,7 +94,7 @@ void Copter::userhook_SuperSlowLoop() @@ -94,7 +94,7 @@ void Copter::userhook_SuperSlowLoop()
break;
}
cacl_volt_pst = 100 - cnt;
battery.reset_remaining(1, cacl_volt_pst);
battery.reset_remaining(0, cacl_volt_pst);
}
}
#endif

3
libraries/AP_BattMonitor/AP_BattMonitor.cpp

@ -147,7 +147,8 @@ AP_BattMonitor::init() @@ -147,7 +147,8 @@ AP_BattMonitor::init()
drivers[instance] = new AP_BattMonitor_FuelLevel_PWM(*this, state[instance], _params[instance]);
break;
case AP_BattMonitor_Params::BattMonitor_Serial_BattGo:
drivers[instance] = new AP_BattMonitor_Serial_BattGo(*this, state[instance], _params[instance],instance);//TODO
// drivers[instance] = new AP_BattMonitor_Serial_BattGo(*this, state[instance], _params[instance],instance);//TODO
drivers[instance] = new AP_BattMonitor_Serial_BattGo(*this, state[instance], _params[instance],0);//TODO
break;
case AP_BattMonitor_Params::BattMonitor_TYPE_NONE:
default:

9
libraries/AP_OpticalFlow/OpticalFlow.cpp

@ -168,7 +168,7 @@ void OpticalFlow::update(void) @@ -168,7 +168,7 @@ void OpticalFlow::update(void)
// only healthy if the data is less than 0.5s old
// _flags.healthy = (AP_HAL::millis() - _last_update_ms < 500);
_flags.healthy = (AP_HAL::millis() - _last_update_ms < 500)&&(_state.surface_quality < _threshold);
_flags.healthy = (AP_HAL::millis() - _last_update_ms < 500)&&(_state.surface_quality > _threshold);
}
void OpticalFlow::handle_msg(const mavlink_message_t &msg)
@ -182,17 +182,12 @@ void OpticalFlow::handle_msg(const mavlink_message_t &msg) @@ -182,17 +182,12 @@ void OpticalFlow::handle_msg(const mavlink_message_t &msg)
backend->handle_msg(msg);
}
}
#include <GCS_MAVLink/GCS.h>
void OpticalFlow::update_state(const OpticalFlow_state &state)
{
_state = state;
_last_update_ms = AP_HAL::millis();
// static uint8_t last_quaty;
// if(abs(last_quaty - _state.surface_quality)> 30){
// last_quaty = _state.surface_quality;
// gcs().send_text(MAV_SEVERITY_INFO,"thr:%d,q:%d",_threshold,_state.surface_quality);
// }
if(_state.surface_quality < _threshold )
{
_state.surface_quality = 0;

Loading…
Cancel
Save