aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Rover: Support new battery interface

Browse Source
This commit is contained in:
Michael du Breuil 2018-03-01 20:30:40 -07:00 committed by Francisco Ferreira
parent 6ebe954d51
commit b761a57da3
5 changed files with 74 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
APMrover2/APMrover2.cpp
View File

@ -97,6 +97,8 @@ const AP_Scheduler::Task Rover::scheduler_tasks[] = {
#endif
};
constexpr int8_t Rover::_failsafe_priorities[7];
#if STATS_ENABLED == ENABLED
/*
update AP_Stats

2
APMrover2/GCS_Mavlink.h
View File

@ -3,7 +3,7 @@
#include <GCS_MAVLink/GCS.h>
// default sensors are present and healthy: gyro, accelerometer, rate_control, attitude_stabilization, yaw_position, altitude control, x/y position control, motor_control
#define MAVLINK_SENSOR_PRESENT_DEFAULT (MAV_SYS_STATUS_SENSOR_3D_GYRO | MAV_SYS_STATUS_SENSOR_3D_ACCEL | MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL | MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION | MAV_SYS_STATUS_SENSOR_YAW_POSITION | MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL | MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS | MAV_SYS_STATUS_AHRS)
#define MAVLINK_SENSOR_PRESENT_DEFAULT (MAV_SYS_STATUS_SENSOR_3D_GYRO | MAV_SYS_STATUS_SENSOR_3D_ACCEL | MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL | MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION | MAV_SYS_STATUS_SENSOR_YAW_POSITION | MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL | MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS | MAV_SYS_STATUS_AHRS | MAV_SYS_STATUS_SENSOR_BATTERY)
class GCS_MAVLINK_Rover : public GCS_MAVLINK
{

27
APMrover2/Rover.h
View File

@ -292,7 +292,9 @@ private:
aux_switch_pos aux_ch7;
// Battery Sensors
AP_BattMonitor battery{MASK_LOG_CURRENT};
AP_BattMonitor battery{MASK_LOG_CURRENT,
FUNCTOR_BIND_MEMBER(&Rover::handle_battery_failsafe, void, const char*, const int8_t),
_failsafe_priorities};
#if FRSKY_TELEM_ENABLED == ENABLED
// FrSky telemetry support
@ -457,6 +459,7 @@ private:
// failsafe.cpp
void failsafe_trigger(uint8_t failsafe_type, bool on);
void handle_battery_failsafe(const char* type_str, const int8_t action);
#if ADVANCED_FAILSAFE == ENABLED
void afs_fs_check(void);
#endif
@ -553,6 +556,28 @@ private:
bool disarm_motors(void);
bool is_boat() const;
enum Failsafe_Action {
Failsafe_Action_None = 0,
Failsafe_Action_RTL = 1,
Failsafe_Action_Hold = 2,
Failsafe_Action_SmartRTL = 3,
Failsafe_Action_SmartRTL_Hold = 4,
Failsafe_Action_Terminate = 5
};
static constexpr int8_t _failsafe_priorities[] = {
Failsafe_Action_Terminate,
Failsafe_Action_Hold,
Failsafe_Action_RTL,
Failsafe_Action_SmartRTL_Hold,
Failsafe_Action_SmartRTL,
Failsafe_Action_None,
-1 // the priority list must end with a sentinel of -1
};
static_assert(_failsafe_priorities[ARRAY_SIZE(_failsafe_priorities) - 1] == -1,
"_failsafe_priorities is missing the sentinel");
public:
void mavlink_delay_cb();
void failsafe_check();

35
APMrover2/failsafe.cpp
View File

@ -96,6 +96,41 @@ void Rover::failsafe_trigger(uint8_t failsafe_type, bool on)
}
}
void Rover::handle_battery_failsafe(const char* type_str, const int8_t action)
{
switch ((Failsafe_Action)action) {
case Failsafe_Action_None:
break;
case Failsafe_Action_SmartRTL:
if (set_mode(mode_smartrtl, MODE_REASON_FAILSAFE)) {
break;
}
FALLTHROUGH;
case Failsafe_Action_RTL:
if (set_mode(mode_rtl, MODE_REASON_FAILSAFE)) {
break;
}
FALLTHROUGH;
case Failsafe_Action_Hold:
set_mode(mode_hold, MODE_REASON_FAILSAFE);
break;
case Failsafe_Action_SmartRTL_Hold:
if (!set_mode(mode_smartrtl, MODE_REASON_FAILSAFE)) {
set_mode(mode_hold, MODE_REASON_FAILSAFE);
}
break;
case Failsafe_Action_Terminate:
#if ADVANCED_FAILSAFE == ENABLED
char battery_type_str[17];
snprintf(battery_type_str, 17, "%s battery", type_str);
afs.gcs_terminate(true, battery_type_str);
#else
disarm_motors();
#endif // ADVANCED_FAILSAFE == ENABLED
break;
}
}
#if ADVANCED_FAILSAFE == ENABLED
/*
check for AFS failsafe check

11
APMrover2/sensors.cpp
View File

@ -293,7 +293,8 @@ void Rover::update_sensor_status_flags(void)
~MAV_SYS_STATUS_SENSOR_YAW_POSITION &
~MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL &
~MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS &
~MAV_SYS_STATUS_LOGGING);
~MAV_SYS_STATUS_LOGGING &
~MAV_SYS_STATUS_SENSOR_BATTERY);
if (control_mode->attitude_stabilized()) {
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL; // 3D angular rate control
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION; // 3D angular rate control
@ -312,6 +313,10 @@ void Rover::update_sensor_status_flags(void)
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS;
}
if (battery.num_instances() > 0) {
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_BATTERY;
}
// default to all healthy except compass and gps which we set individually
control_sensors_health = control_sensors_present & (~MAV_SYS_STATUS_SENSOR_3D_MAG & ~MAV_SYS_STATUS_SENSOR_GPS);
if (g.compass_enabled && compass.healthy(0) && ahrs.use_compass()) {
@ -352,6 +357,10 @@ void Rover::update_sensor_status_flags(void)
control_sensors_health &= ~MAV_SYS_STATUS_LOGGING;
}
if (!battery.healthy() || battery.has_failsafed()) {
control_sensors_enabled &= ~MAV_SYS_STATUS_SENSOR_BATTERY;
}
if (!initialised || ins.calibrating()) {
// while initialising the gyros and accels are not enabled
control_sensors_enabled &= ~(MAV_SYS_STATUS_SENSOR_3D_GYRO | MAV_SYS_STATUS_SENSOR_3D_ACCEL);