Skip to content

Commit

Permalink
Updated test board for changes to data acquisition
Browse files Browse the repository at this point in the history
  • Loading branch information
bsutherland333 committed Dec 13, 2023
1 parent fdd8e5c commit 3e004db
Show file tree
Hide file tree
Showing 5 changed files with 71 additions and 65 deletions.
16 changes: 8 additions & 8 deletions include/board.h
Original file line number Diff line number Diff line change
Expand Up @@ -71,46 +71,46 @@ class Board
virtual uint16_t num_sensor_errors() = 0;

// IMU
virtual uint8_t imu_has_new_data() = 0;
virtual bool imu_has_new_data() = 0;
virtual bool imu_read(float accel[3], float *temperature, float gyro[3], uint64_t *time) = 0;
virtual void imu_not_responding_error() = 0;

// Mag
virtual bool mag_present() = 0;
virtual uint8_t mag_has_new_data() = 0;
virtual bool mag_has_new_data() = 0;
virtual bool mag_read(float mag[3]) = 0;

// Baro
virtual bool baro_present() = 0;
virtual uint8_t baro_has_new_data() = 0;
virtual bool baro_has_new_data() = 0;
virtual bool baro_read(float *pressure, float *temperature) = 0;

// Pitot
virtual bool diff_pressure_present() = 0;
virtual uint8_t diff_pressure_has_new_data() = 0;
virtual bool diff_pressure_has_new_data() = 0;
virtual bool diff_pressure_read(float *diff_pressure, float *temperature) = 0;

// Sonar
virtual bool sonar_present() = 0;
virtual uint8_t sonar_has_new_data() = 0;
virtual bool sonar_has_new_data() = 0;
virtual bool sonar_read(float *range) = 0;

// GPS
virtual bool gnss_present() = 0;
virtual uint8_t gnss_has_new_data() = 0;
virtual bool gnss_has_new_data() = 0;
virtual bool gnss_read(GNSSData *gnss, GNSSFull *gnss_full) = 0;

// Battery
virtual bool battery_present() = 0;
virtual uint8_t battery_has_new_data() = 0;
virtual bool battery_has_new_data() = 0;
virtual bool battery_read(float *voltage, float *current) = 0;
virtual void battery_voltage_set_multiplier(double multiplier) = 0;
virtual void battery_current_set_multiplier(double multiplier) = 0;

// RC
virtual void rc_init(rc_type_t rc_type) = 0;
virtual bool rc_lost() = 0;
virtual uint8_t rc_has_new_data() = 0;
virtual bool rc_has_new_data() = 0;
virtual float rc_read(uint8_t chan) = 0;

// PWM
Expand Down
2 changes: 0 additions & 2 deletions src/rc.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -263,8 +263,6 @@ void RC::look_for_arm_disarm_signal()

bool RC::run()
{
uint32_t now = RF_.board_.clock_millis();

// Check for rc lost
if (check_rc_lost())
return false;
Expand Down
5 changes: 3 additions & 2 deletions test/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -1,7 +1,8 @@
cmake_minimum_required(VERSION 2.6)
cmake_minimum_required(VERSION 3.8)
project(rosflight_tests)

# C++ standard
add_definitions(-std=c++11)
add_definitions(-std=c++17)

# Strict Compilation
set(CMAKE_CXX_FLAGS "-pedantic -pedantic-errors -Werror -Wall -Wextra \
Expand Down
74 changes: 42 additions & 32 deletions test/test_board.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -42,6 +42,7 @@ void testBoard::set_rc(uint16_t *values)
{
rc_values[i] = values[i];
}
new_rc_ = true;
}

void testBoard::set_time(uint64_t time_us)
Expand Down Expand Up @@ -103,7 +104,7 @@ uint16_t testBoard::num_sensor_errors()
return 0;
}

bool testBoard::new_imu_data()
bool testBoard::imu_has_new_data()
{
if (new_imu_)
{
Expand Down Expand Up @@ -158,76 +159,80 @@ bool testBoard::mag_present()
{
return false;
}
void testBoard::mag_update() {}
void testBoard::mag_read(float mag[3]) {}
bool testBoard::mag_has_new_data()
{
return false;
}
bool testBoard::mag_read(float mag[3]) {
return false;
}

bool testBoard::baro_present()
{
return false;
}
void testBoard::baro_update() {}
void testBoard::baro_read(float *pressure, float *temperature) {}
bool testBoard::baro_has_new_data()
{
return false;
}
bool testBoard::baro_read(float *pressure, float *temperature)
{
return false;
}

bool testBoard::diff_pressure_present()
{
return false;
}
void testBoard::diff_pressure_update() {}
void testBoard::diff_pressure_read(float *diff_pressure, float *temperature) {}

bool testBoard::sonar_present()
bool testBoard::diff_pressure_has_new_data()
{
return false;
}
void testBoard::sonar_update() {}
float testBoard::sonar_read()
bool testBoard::diff_pressure_read(float *diff_pressure, float *temperature)
{
return 0;
return false;
}

bool testBoard::battery_voltage_present() const
bool testBoard::sonar_present()
{
return false;
}
float testBoard::battery_voltage_read() const
bool testBoard::sonar_has_new_data()
{
return 0;
return false;
}
void testBoard::battery_voltage_set_multiplier(double multiplier)
bool testBoard::sonar_read(float *range)
{
(void)multiplier;
return false;
}

bool testBoard::battery_current_present() const
bool testBoard::gnss_present()
{
return false;
}
float testBoard::battery_current_read() const
bool testBoard::gnss_has_new_data()
{
return 0;
return false;
}
void testBoard::battery_current_set_multiplier(double multiplier)
bool testBoard::gnss_read(GNSSData *gnss, GNSSFull *gnss_full)
{
(void)multiplier;
return false;
}

// GNSS is not supported on the test board
GNSSData testBoard::gnss_read()
bool testBoard::battery_present()
{
return {};
return false;
}

// GNSS is not supported on the test board
GNSSFull testBoard::gnss_full_read()
bool testBoard::battery_has_new_data()
{
return {};
return false;
}

// GNSS is not supported on the test board
bool testBoard::gnss_has_new_data()
bool testBoard::battery_read(float *voltage, float *current)
{
return false;
}
void testBoard::battery_voltage_set_multiplier(double multiplier) {}
void testBoard::battery_current_set_multiplier(double multiplier) {}

// PWM
// TODO make these deal in normalized (-1 to 1 or 0 to 1) values (not pwm-specific)
Expand All @@ -236,8 +241,13 @@ bool testBoard::rc_lost()
{
return rc_lost_;
}
bool testBoard::rc_has_new_data()
{
return new_rc_;
}
float testBoard::rc_read(uint8_t channel)
{
new_rc_ = false;
return static_cast<float>(rc_values[channel] - 1000) / 1000.0;
}
void testBoard::pwm_write(uint8_t channel, float value) {}
Expand Down
39 changes: 18 additions & 21 deletions test/test_board.h
Original file line number Diff line number Diff line change
Expand Up @@ -41,6 +41,7 @@ class testBoard : public Board
{
private:
uint16_t rc_values[8] = {1500, 1500, 1000, 1500, 1500, 1500, 1500, 1500};
bool new_rc_ = false;
uint64_t time_us_ = 0;
bool rc_lost_ = false;
float acc_[3] = {0, 0, 0};
Expand Down Expand Up @@ -68,45 +69,42 @@ class testBoard : public Board

// sensors
void sensors_init() override;
uint16_t num_sensor_errors();
uint16_t num_sensor_errors() override;

bool new_imu_data() override;
bool imu_has_new_data() override;
bool imu_read(float accel[3], float *temperature, float gyro[3], uint64_t *time) override;
void imu_not_responding_error() override;

bool mag_present() override;
void mag_update() override;
void mag_read(float mag[3]) override;
bool mag_has_new_data() override;
bool mag_read(float mag[3]) override;

bool baro_present() override;
void baro_update() override;
void baro_read(float *pressure, float *temperature) override;
bool baro_has_new_data() override;
bool baro_read(float *pressure, float *temperature) override;

bool diff_pressure_present() override;
void diff_pressure_update() override;
void diff_pressure_read(float *diff_pressure, float *temperature) override;
bool diff_pressure_has_new_data() override;
bool diff_pressure_read(float *diff_pressure, float *temperature) override;

bool sonar_present() override;
void sonar_update() override;
float sonar_read() override;
bool sonar_has_new_data() override;
bool sonar_read(float *range) override;

bool gnss_present() override { return false; }
void gnss_update() override {}
GNSSData gnss_read() override;
GNSSFull gnss_full_read() override;
bool gnss_present() override;
bool gnss_has_new_data() override;
bool gnss_read(GNSSData *gnss, GNSSFull *gnss_full) override;

bool battery_voltage_present() const override;
float battery_voltage_read() const override;
bool battery_present() override;
bool battery_has_new_data() override;
bool battery_read(float *voltage, float *current) override;
void battery_voltage_set_multiplier(double multiplier) override;

bool battery_current_present() const override;
float battery_current_read() const override;
void battery_current_set_multiplier(double multiplier) override;

// RC
void rc_init(rc_type_t rc_type) override;
bool rc_lost() override;
bool rc_has_new_data() override;
float rc_read(uint8_t channel) override;

// PWM
Expand Down Expand Up @@ -135,8 +133,7 @@ class testBoard : public Board
void backup_memory_clear(size_t len) override;
void backup_memory_clear(); // Not an override

void set_imu(float *acc, float *gyro, uint64_t time_us);
void set_rc(uint16_t *values);
void set_imu(float *acc, float *gyro, uint64_t time_us); void set_rc(uint16_t *values);
void set_time(uint64_t time_us);
void set_pwm_lost(bool lost);
};
Expand Down

0 comments on commit 3e004db

Please sign in to comment.