mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-09 01:18:29 -04:00
APM: added AMP_OFFSET option
thanks to Alexey Kozin!
This commit is contained in:
parent
61f2e18e2e
commit
8f108e2eb3
@ -113,6 +113,7 @@ public:
|
|||||||
k_param_ahrs, // AHRS group
|
k_param_ahrs, // AHRS group
|
||||||
k_param_barometer, // barometer ground calibration
|
k_param_barometer, // barometer ground calibration
|
||||||
k_param_airspeed, // AP_Airspeed parameters
|
k_param_airspeed, // AP_Airspeed parameters
|
||||||
|
k_param_curr_amp_offset,
|
||||||
|
|
||||||
//
|
//
|
||||||
// 150: Navigation parameters
|
// 150: Navigation parameters
|
||||||
@ -341,6 +342,7 @@ public:
|
|||||||
AP_Int8 flap_2_speed;
|
AP_Int8 flap_2_speed;
|
||||||
AP_Float volt_div_ratio;
|
AP_Float volt_div_ratio;
|
||||||
AP_Float curr_amp_per_volt;
|
AP_Float curr_amp_per_volt;
|
||||||
|
AP_Float curr_amp_offset;
|
||||||
AP_Float input_voltage;
|
AP_Float input_voltage;
|
||||||
AP_Int32 pack_capacity; // Battery pack capacity less reserve
|
AP_Int32 pack_capacity; // Battery pack capacity less reserve
|
||||||
AP_Int8 rssi_pin;
|
AP_Int8 rssi_pin;
|
||||||
|
@ -527,9 +527,29 @@ const AP_Param::Info var_info[] PROGMEM = {
|
|||||||
|
|
||||||
GSCALAR(battery_monitoring, "BATT_MONITOR", 0),
|
GSCALAR(battery_monitoring, "BATT_MONITOR", 0),
|
||||||
GSCALAR(volt_div_ratio, "VOLT_DIVIDER", VOLT_DIV_RATIO),
|
GSCALAR(volt_div_ratio, "VOLT_DIVIDER", VOLT_DIV_RATIO),
|
||||||
|
|
||||||
|
// @Param: APM_PER_VOLT
|
||||||
|
// @DisplayName: Apms per volt
|
||||||
|
// @Description: Number of amps that a 1V reading on the current sensor corresponds to
|
||||||
|
// @Units: A/V
|
||||||
|
// @User: Standard
|
||||||
GSCALAR(curr_amp_per_volt, "AMP_PER_VOLT", CURR_AMP_PER_VOLT),
|
GSCALAR(curr_amp_per_volt, "AMP_PER_VOLT", CURR_AMP_PER_VOLT),
|
||||||
|
|
||||||
|
// @Param: AMP_OFFSET
|
||||||
|
// @DisplayName: AMP offset
|
||||||
|
// @Description: Voltage offset at zero current on current sensor
|
||||||
|
// @Units: Volts
|
||||||
|
// @User: Standard
|
||||||
|
GSCALAR(curr_amp_offset, "AMP_OFFSET", 0),
|
||||||
|
|
||||||
GSCALAR(input_voltage, "INPUT_VOLTS", INPUT_VOLTAGE),
|
GSCALAR(input_voltage, "INPUT_VOLTS", INPUT_VOLTAGE),
|
||||||
GSCALAR(pack_capacity, "BATT_CAPACITY", HIGH_DISCHARGE),
|
|
||||||
|
// @Param: BATT_CAPACITY
|
||||||
|
// @DisplayName: Battery capacity
|
||||||
|
// @Description: Capacity of the battery in mAh when full
|
||||||
|
// @Units: mAh
|
||||||
|
// @User: Standard
|
||||||
|
GSCALAR(pack_capacity, "BATT_CAPACITY", 1760),
|
||||||
|
|
||||||
// @Param: BATT_VOLT_PIN
|
// @Param: BATT_VOLT_PIN
|
||||||
// @DisplayName: Battery Voltage sensing pin
|
// @DisplayName: Battery Voltage sensing pin
|
||||||
|
@ -234,13 +234,6 @@
|
|||||||
//# define CURR_AMP_PER_VOLT 13.66 // This is the proper value for the AttoPilot 13.6V/45A sensor
|
//# define CURR_AMP_PER_VOLT 13.66 // This is the proper value for the AttoPilot 13.6V/45A sensor
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#ifndef CURR_AMPS_OFFSET
|
|
||||||
# define CURR_AMPS_OFFSET 0.0
|
|
||||||
#endif
|
|
||||||
#ifndef HIGH_DISCHARGE
|
|
||||||
# define HIGH_DISCHARGE 1760
|
|
||||||
#endif
|
|
||||||
|
|
||||||
//////////////////////////////////////////////////////////////////////////////
|
//////////////////////////////////////////////////////////////////////////////
|
||||||
// INPUT_VOLTAGE
|
// INPUT_VOLTAGE
|
||||||
//
|
//
|
||||||
|
@ -201,7 +201,7 @@ enum gcs_severity {
|
|||||||
|
|
||||||
|
|
||||||
#define BATTERY_VOLTAGE(x) (x*(g.input_voltage/1024.0))*g.volt_div_ratio
|
#define BATTERY_VOLTAGE(x) (x*(g.input_voltage/1024.0))*g.volt_div_ratio
|
||||||
#define CURRENT_AMPS(x) ((x*(g.input_voltage/1024.0))-CURR_AMPS_OFFSET)*g.curr_amp_per_volt
|
#define CURRENT_AMPS(x) ((x*(g.input_voltage/1024.0))-g.curr_amp_offset)*g.curr_amp_per_volt
|
||||||
|
|
||||||
#define RELAY_PIN 47
|
#define RELAY_PIN 47
|
||||||
|
|
||||||
|
Loading…
Reference in New Issue
Block a user