mirror of https://github.com/ArduPilot/ardupilot
parent
74c8c8aafa
commit
c7df0eaf2a
|
@ -36,6 +36,12 @@ float ADCSource::read_latest() {
|
|||
case 0:
|
||||
return _sitlState->airspeed_pin_value;
|
||||
|
||||
case 12:
|
||||
return _sitlState->current_pin_value;
|
||||
|
||||
case 13:
|
||||
return _sitlState->voltage_pin_value;
|
||||
|
||||
case ANALOG_INPUT_NONE:
|
||||
default:
|
||||
return 0.0;
|
||||
|
|
|
@ -59,6 +59,8 @@ pid_t SITL_State::_parent_pid;
|
|||
uint32_t SITL_State::_update_count;
|
||||
bool SITL_State::_motors_on;
|
||||
uint16_t SITL_State::airspeed_pin_value;
|
||||
uint16_t SITL_State::voltage_pin_value;
|
||||
uint16_t SITL_State::current_pin_value;
|
||||
|
||||
AP_Baro_HIL *SITL_State::_barometer;
|
||||
AP_InertialSensor_HIL *SITL_State::_ins;
|
||||
|
@ -478,6 +480,15 @@ void SITL_State::_simulator_output(void)
|
|||
}
|
||||
}
|
||||
|
||||
float throttle = _motors_on?(control.pwm[2]-1000) / 1000.0f:0;
|
||||
// lose 0.7V at full throttle
|
||||
float voltage = 12.6 - 0.7f*throttle;
|
||||
// assume 50A at full throttle
|
||||
float current = 50.0 * throttle;
|
||||
// assume 3DR power brick
|
||||
voltage_pin_value = ((voltage / 10.1) / 5.0) * 1024;
|
||||
current_pin_value = ((current / 17.0) / 5.0) * 1024;
|
||||
|
||||
// setup wind control
|
||||
control.speed = _sitl->wind_speed * 100;
|
||||
float direction = _sitl->wind_direction;
|
||||
|
|
|
@ -44,6 +44,8 @@ public:
|
|||
|
||||
// simulated airspeed
|
||||
static uint16_t airspeed_pin_value;
|
||||
static uint16_t voltage_pin_value;
|
||||
static uint16_t current_pin_value;
|
||||
|
||||
private:
|
||||
void _parse_command_line(int argc, char * const argv[]);
|
||||
|
|
Loading…
Reference in New Issue