Browse Source

SITL: Create a second voltage to monitor for battery voltages

master
Michael du Breuil 7 years ago committed by Francisco Ferreira
parent
commit
8dd55a85cd
  1. 6
      libraries/AP_HAL_SITL/AnalogIn.cpp
  2. 3
      libraries/AP_HAL_SITL/SITL_State.cpp
  3. 2
      libraries/AP_HAL_SITL/SITL_State.h

6
libraries/AP_HAL_SITL/AnalogIn.cpp

@ -46,6 +46,12 @@ float ADCSource::read_latest() {
case 13: case 13:
return _sitlState->voltage_pin_value; return _sitlState->voltage_pin_value;
case 14:
return _sitlState->current2_pin_value;
case 15:
return _sitlState->voltage2_pin_value;
case ANALOG_INPUT_NONE: case ANALOG_INPUT_NONE:
default: default:
return 0.0f; return 0.0f;

3
libraries/AP_HAL_SITL/SITL_State.cpp

@ -464,6 +464,9 @@ void SITL_State::_simulator_servos(SITL::Aircraft::sitl_input &input)
// assume 3DR power brick // assume 3DR power brick
voltage_pin_value = ((voltage / 10.1f) / 5.0f) * 1024; voltage_pin_value = ((voltage / 10.1f) / 5.0f) * 1024;
current_pin_value = ((_current / 17.0f) / 5.0f) * 1024; current_pin_value = ((_current / 17.0f) / 5.0f) * 1024;
// fake battery2 as just a 25% gain on the first one
voltage2_pin_value = ((voltage * 0.25f / 10.1f) / 5.0f) * 1024;
current2_pin_value = ((_current * 0.25f / 17.0f) / 5.0f) * 1024;
} }
void SITL_State::init(int argc, char * const argv[]) void SITL_State::init(int argc, char * const argv[])

2
libraries/AP_HAL_SITL/SITL_State.h

@ -67,6 +67,8 @@ public:
uint16_t airspeed_2_pin_value; // pin 2 uint16_t airspeed_2_pin_value; // pin 2
uint16_t voltage_pin_value; // pin 13 uint16_t voltage_pin_value; // pin 13
uint16_t current_pin_value; // pin 12 uint16_t current_pin_value; // pin 12
uint16_t voltage2_pin_value; // pin 15
uint16_t current2_pin_value; // pin 14
// return TCP client address for uartC // return TCP client address for uartC
const char *get_client_address(void) const { return _client_address; } const char *get_client_address(void) const { return _client_address; }

Loading…
Cancel
Save