|
|
@ -22,6 +22,7 @@ |
|
|
|
#include <AP_Math/AP_Math.h> |
|
|
|
#include <AP_Math/AP_Math.h> |
|
|
|
#include <GCS_MAVLink/GCS.h> |
|
|
|
#include <GCS_MAVLink/GCS.h> |
|
|
|
#include <SRV_Channel/SRV_Channel.h> |
|
|
|
#include <SRV_Channel/SRV_Channel.h> |
|
|
|
|
|
|
|
#include <DataFlash/DataFlash.h> |
|
|
|
#include <utility> |
|
|
|
#include <utility> |
|
|
|
#include "AP_Airspeed.h" |
|
|
|
#include "AP_Airspeed.h" |
|
|
|
#include "AP_Airspeed_MS4525.h" |
|
|
|
#include "AP_Airspeed_MS4525.h" |
|
|
@ -197,6 +198,12 @@ const AP_Param::GroupInfo AP_Airspeed::var_info[] = { |
|
|
|
AP_GROUPEND |
|
|
|
AP_GROUPEND |
|
|
|
}; |
|
|
|
}; |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
/*
|
|
|
|
|
|
|
|
this scaling factor converts from the old system where we used a |
|
|
|
|
|
|
|
0 to 4095 raw ADC value for 0-5V to the new system which gets the |
|
|
|
|
|
|
|
voltage in volts directly from the ADC driver |
|
|
|
|
|
|
|
*/ |
|
|
|
|
|
|
|
#define SCALING_OLD_CALIBRATION 819 // 4095/5
|
|
|
|
|
|
|
|
|
|
|
|
AP_Airspeed::AP_Airspeed() |
|
|
|
AP_Airspeed::AP_Airspeed() |
|
|
|
{ |
|
|
|
{ |
|
|
@ -211,14 +218,6 @@ AP_Airspeed::AP_Airspeed() |
|
|
|
_singleton = this; |
|
|
|
_singleton = this; |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
/*
|
|
|
|
|
|
|
|
this scaling factor converts from the old system where we used a |
|
|
|
|
|
|
|
0 to 4095 raw ADC value for 0-5V to the new system which gets the |
|
|
|
|
|
|
|
voltage in volts directly from the ADC driver |
|
|
|
|
|
|
|
*/ |
|
|
|
|
|
|
|
#define SCALING_OLD_CALIBRATION 819 // 4095/5
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
void AP_Airspeed::init() |
|
|
|
void AP_Airspeed::init() |
|
|
|
{ |
|
|
|
{ |
|
|
|
// cope with upgrade from old system
|
|
|
|
// cope with upgrade from old system
|
|
|
@ -415,7 +414,7 @@ void AP_Airspeed::read(uint8_t i) |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
// read all airspeed sensors
|
|
|
|
// read all airspeed sensors
|
|
|
|
void AP_Airspeed::read(void) |
|
|
|
void AP_Airspeed::update(bool log) |
|
|
|
{ |
|
|
|
{ |
|
|
|
for (uint8_t i=0; i<AIRSPEED_MAX_SENSORS; i++) { |
|
|
|
for (uint8_t i=0; i<AIRSPEED_MAX_SENSORS; i++) { |
|
|
|
read(i); |
|
|
|
read(i); |
|
|
@ -428,6 +427,13 @@ void AP_Airspeed::read(void) |
|
|
|
} |
|
|
|
} |
|
|
|
#endif |
|
|
|
#endif |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
if (log) { |
|
|
|
|
|
|
|
DataFlash_Class *_dataflash = DataFlash_Class::instance(); |
|
|
|
|
|
|
|
if (_dataflash != nullptr) { |
|
|
|
|
|
|
|
_dataflash->Log_Write_Airspeed(*this); |
|
|
|
|
|
|
|
} |
|
|
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
// setup primary
|
|
|
|
// setup primary
|
|
|
|
if (healthy(primary_sensor.get())) { |
|
|
|
if (healthy(primary_sensor.get())) { |
|
|
|
primary = primary_sensor.get(); |
|
|
|
primary = primary_sensor.get(); |
|
|
|