AP_Airspeed: update data with logging

This commit is contained in:
DOMINATOR\Eugene 2018-11-16 18:16:23 +02:00 committed by Andrew Tridgell
parent 802ae94e2c
commit c81f9e6baa
3 changed files with 17 additions and 12 deletions

View File

@ -22,6 +22,7 @@
#include <AP_Math/AP_Math.h>
#include <GCS_MAVLink/GCS.h>
#include <SRV_Channel/SRV_Channel.h>
#include <DataFlash/DataFlash.h>
#include <utility>
#include "AP_Airspeed.h"
#include "AP_Airspeed_MS4525.h"
@ -197,6 +198,12 @@ const AP_Param::GroupInfo AP_Airspeed::var_info[] = {
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()
{
@ -211,14 +218,6 @@ AP_Airspeed::AP_Airspeed()
_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()
{
// cope with upgrade from old system
@ -415,7 +414,7 @@ void AP_Airspeed::read(uint8_t i)
}
// 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++) {
read(i);
@ -428,6 +427,13 @@ void AP_Airspeed::read(void)
}
#endif
if (log) {
DataFlash_Class *_dataflash = DataFlash_Class::instance();
if (_dataflash != nullptr) {
_dataflash->Log_Write_Airspeed(*this);
}
}
// setup primary
if (healthy(primary_sensor.get())) {
primary = primary_sensor.get();

View File

@ -43,7 +43,7 @@ public:
void init(void);
// read the analog source and update airspeed
void read(void);
void update(bool log);
// calibrate the airspeed. This must be called on startup if the
// altitude/climb_rate/acceleration interfaces are ever used

View File

@ -29,7 +29,6 @@ void loop();
const AP_HAL::HAL& hal = AP_HAL::get_HAL();
float temperature;
AP_Airspeed airspeed;
static AP_BoardConfig board_config;
@ -65,7 +64,7 @@ void loop(void)
static uint32_t timer;
if ((AP_HAL::millis() - timer) > 100) {
timer = AP_HAL::millis();
airspeed.read();
airspeed.update(false);
airspeed.get_temperature(temperature);
hal.console->printf("airspeed %5.2f temperature %6.2f healthy = %u\n",