Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-09 09:24:01 -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

28
libraries/AP_HAL_SITL/SITL_State.cpp
View File

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

3
libraries/AP_HAL_SITL/SITL_State.h
View File

@ -36,7 +36,8 @@ public:
enum vehicle_type { enum vehicle_type {
ArduCopter, ArduCopter,
APMrover2, APMrover2,
ArduPlane ArduPlane,
ArduSub
}; };
int gps_pipe(void); 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) // set right default throttle for rover (allowing for reverse)
pwm_input[2] = 1500; pwm_input[2] = 1500;
} else if (strcmp(SKETCH, "ArduSub") == 0) {
_vehicle = ArduSub;
} else { } else {
_vehicle = ArduPlane; _vehicle = ArduPlane;
if (_framerate == 0) { if (_framerate == 0) {