Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-03 14:38:44 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_HAL_SITL: Add current and voltage monitoring implementation for Sub

Browse Source
This commit is contained in:
Jacob Walser 2017-10-27 13:24:53 -04:00
parent 07f2d5ac79
commit 68d9116840
3 changed files with 25 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
libraries/AP_HAL_SITL/SITL_State.cpp
View File

@ -407,6 +407,19 @@ void SITL_State::_simulator_servos(SITL::Aircraft::sitl_input &input)
if (_sitl != nullptr) {
if (_sitl->state.battery_voltage <= 0) {
if (_vehicle == ArduSub) {
voltage = _sitl->batt_voltage;
for (i = 0; i < 6; i++) {
float pwm = input.servos[i];
//printf("i: %d, pwm: %.2f\n", i, pwm);
float fraction = fabsf((pwm - 1500) / 500.0f);
voltage -= fraction * 0.5f;
float draw = fraction * 15;
_current += draw;
}
} else {
// simulate simple battery setup
float throttle = motors_on?(input.servos[2]-1000) / 1000.0f:0;
// lose 0.7V at full throttle
@ -414,6 +427,7 @@ void SITL_State::_simulator_servos(SITL::Aircraft::sitl_input &input)
// assume 50A at full throttle
_current = 50.0f * fabsf(throttle);
}
} else {
// FDM provides voltage and current
voltage = _sitl->state.battery_voltage;

3
libraries/AP_HAL_SITL/SITL_State.h
View File

@ -36,7 +36,8 @@ public:
enum vehicle_type {
ArduCopter,
APMrover2,
ArduPlane
ArduPlane,
ArduSub
};
int gps_pipe(void);

2
libraries/AP_HAL_SITL/SITL_cmdline.cpp
View File

@ -371,6 +371,8 @@ void SITL_State::_parse_command_line(int argc, char * const argv[])
}
// set right default throttle for rover (allowing for reverse)
pwm_input[2] = 1500;
} else if (strcmp(SKETCH, "ArduSub") == 0) {
_vehicle = ArduSub;
} else {
_vehicle = ArduPlane;
if (_framerate == 0) {