Got unit tests compiling and running with sensor changes

include/sensors.h

@@ -46,15 +46,15 @@ namespace rosflight_firmware
 
 typedef struct
 {
-  uint8_t imu;
-  uint8_t gnss;
-  uint8_t gnss_full;
-  uint8_t baro;
-  uint8_t mag;
-  uint8_t diff_pressure;
-  uint8_t sonar;
-  uint8_t battery;
-  uint8_t rc;
+  bool imu;
+  bool gnss;
+  bool gnss_full;
+  bool baro;
+  bool mag;
+  bool diff_pressure;
+  bool sonar;
+  bool battery;
+  bool rc;
 } got_flags;
 
 enum GNSSFixType

src/comm_manager.cpp

@@ -552,13 +552,8 @@ void CommManager::stream(got_flags got)
   // GPS full data (not needed)
   if (got.gnss_full)
     send_gnss_full();
-  if (got.rc) // report at half the S.Bus rate.
-  {
-    static uint64_t rc_count = 0;
-    // RC (S.Bus) inputs, scaled 1000-2000
-    if (!((rc_count++) % 2))
-      send_rc_raw();
-  }
+  if (got.rc)
+    send_rc_raw();
 
   {
     static uint64_t next_heartbeat = 0, next_status = 0;

src/rc.cpp

@@ -267,15 +267,17 @@ bool RC::run()
   if (check_rc_lost())
     return false;
 
+  // read and normalize stick values
   for (uint8_t channel = 0; channel < static_cast<uint8_t>(STICKS_COUNT); channel++)
   {
+    float pwm = RF_.board_.rc_read(sticks[channel].channel);
     if (sticks[channel].one_sided) // generally only F is one_sided
     {
-      stick_values[channel] = RF_.board_.rc_read(sticks[channel].channel);
+      stick_values[channel] = pwm;
     }
     else
     {
-      stick_values[channel] = 2.0 * (RF_.board_.rc_read(sticks[channel].channel) - 0.5);
+      stick_values[channel] = 2.0 * (pwm - 0.5);
     }
   }
 
@@ -284,13 +286,14 @@ bool RC::run()
   {
     if (switches[channel].mapped)
     {
+      float pwm = RF_.board_.rc_read(switches[channel].channel);
       if (switches[channel].direction < 0)
       {
-        switch_values[channel] = RF_.board_.rc_read(switches[channel].channel) < 0.2;
+        switch_values[channel] = pwm < 0.2;
       }
       else
       {
-        switch_values[channel] = RF_.board_.rc_read(switches[channel].channel) >= 0.8;
+        switch_values[channel] = pwm >= 0.8;
       }
     }
     else

src/sensors.cpp

@@ -131,13 +131,14 @@ got_flags Sensors::run()
   memset(&got, 0, sizeof(got));
 
   // IMU:
-  if ((got.imu = rf_.board_.imu_has_new_data()) > 0)
+  if (rf_.board_.imu_has_new_data())
   {
+    got.imu = true;
     rf_.state_manager_.clear_error(StateManager::ERROR_IMU_NOT_RESPONDING);
     last_imu_update_ms_ = rf_.board_.clock_millis();
 
     if (!rf_.board_.imu_read(accel_, &data_.imu_temperature, gyro_, &data_.imu_time))
-      got.imu = 0;
+      got.imu = false;
 
     // Move data into local copy
     data_.accel.x = accel_[0];
@@ -171,8 +172,9 @@ got_flags Sensors::run()
   if (rf_.board_.gnss_present())
   {
     data_.gnss_present = true;
-    if ((got.gnss = rf_.board_.gnss_has_new_data()) > 0)
+    if (rf_.board_.gnss_has_new_data())
     {
+      got.gnss = true;
       rf_.board_.gnss_read(&this->data_.gnss_data, &this->data_.gnss_full);
     }
     got.gnss_full = got.gnss; // bot come with the pvt GPS data
@@ -182,8 +184,9 @@ got_flags Sensors::run()
   if (rf_.board_.baro_present())
   {
     data_.baro_present = true;
-    if ((got.baro = rf_.board_.baro_has_new_data()) > 0)
+    if (rf_.board_.baro_has_new_data())
     {
+      got.baro = true;
       float raw_pressure;
       float raw_temp;
       rf_.board_.baro_read(&raw_pressure, &raw_temp);
@@ -201,8 +204,9 @@ got_flags Sensors::run()
   {
     data_.mag_present = true;
     float mag[3];
-    if ((got.mag = rf_.board_.mag_has_new_data()) > 0)
+    if (rf_.board_.mag_has_new_data())
     {
+      got.mag = true;
       rf_.board_.mag_read(mag);
       data_.mag.x = mag[0];
       data_.mag.y = mag[1];
@@ -215,8 +219,9 @@ got_flags Sensors::run()
   if (rf_.board_.diff_pressure_present())
   {
     data_.diff_pressure_present = true;
-    if ((got.diff_pressure = rf_.board_.diff_pressure_has_new_data()) > 0)
+    if (rf_.board_.diff_pressure_has_new_data())
     {
+      got.diff_pressure = true;
       float raw_pressure;
       float raw_temp;
       rf_.board_.diff_pressure_read(&raw_pressure, &raw_temp);
@@ -233,24 +238,27 @@ got_flags Sensors::run()
   if (rf_.board_.sonar_present())
   {
     data_.sonar_present = true;
-    if ((got.sonar = rf_.board_.sonar_has_new_data()) > 0)
+    if (rf_.board_.sonar_has_new_data())
     {
+      got.sonar = true;
       float raw_distance;
       rf_.board_.sonar_read(&raw_distance);
       data_.sonar_range_valid = sonar_outlier_filt_.update(raw_distance, &data_.sonar_range);
     }
   }
 
   // BATTERY_MONITOR:
-  if ((got.battery = rf_.board_.battery_has_new_data()) > 0)
+  if (rf_.board_.battery_has_new_data())
   {
+    got.battery = true;
     last_battery_monitor_update_ms_ = rf_.board_.clock_millis();
     update_battery_monitor();
   }
 
   // RC
-  if ((got.rc = rf_.board_.rc_has_new_data()) > 0)
+  if (rf_.board_.rc_has_new_data())
   {
+    got.rc = true;
     rf_.board_.rc_read(0);
   }
 

test/command_manager_test.cpp

@@ -25,6 +25,7 @@ class CommandManagerTest : public ::testing::Test
   testBoard board;
   Mavlink mavlink;
   ROSflight rf;
+  float last_set_rc = 0;
 
   uint16_t rc_values[8];
   float max_roll, max_pitch, max_yawrate;
@@ -69,6 +70,11 @@ class CommandManagerTest : public ::testing::Test
     float dummy_gyro[3] = {0, 0, 0};
     while (board.clock_micros() < start_time_us + us)
     {
+      if (board.clock_millis() > last_set_rc + 20)
+      {
+        last_set_rc = board.clock_millis();
+        board.set_rc(rc_values);
+      }
       board.set_imu(dummy_acc, dummy_gyro, board.clock_micros() + 1000);
       rf.run();
     }
@@ -77,7 +83,6 @@ class CommandManagerTest : public ::testing::Test
 
 TEST_F(CommandManagerTest, Default)
 {
-  board.set_rc(rc_values);
   stepFirmware(20000);
 
   control_t output = rf.command_manager_.combined_control();
@@ -97,8 +102,7 @@ TEST_F(CommandManagerTest, RCCommands)
   rc_values[1] = 1000;
   rc_values[2] = 1500;
   rc_values[3] = 1250;
-  board.set_rc(rc_values);
-  stepFirmware(20000);
+  stepFirmware(50000);
 
   control_t output = rf.command_manager_.combined_control();
   EXPECT_EQ(output.x.type, ANGLE);
@@ -116,7 +120,6 @@ TEST_F(CommandManagerTest, ArmWithSticksByDefault)
   EXPECT_EQ(rf.state_manager_.state().armed, false);
   rc_values[2] = 1000;
   rc_values[3] = 2000;
-  board.set_rc(rc_values);
   stepFirmware(500000);
   EXPECT_EQ(rf.state_manager_.state().armed,
             false); // need to wait 1 second, shouldn't be armed yet
@@ -129,7 +132,6 @@ TEST_F(CommandManagerTest, DontArmWithSticksWhenUsingSwitch)
   rf.params_.set_param_int(PARAM_RC_ARM_CHANNEL, 4);
   rc_values[2] = 1000; // throttle low
   rc_values[3] = 2000; // yaw right
-  board.set_rc(rc_values);
   stepFirmware(1100000);
   EXPECT_EQ(rf.state_manager_.state().armed, false);
 }
@@ -140,7 +142,6 @@ TEST_F(CommandManagerTest, DisarmWithSticksByDefault)
   EXPECT_EQ(rf.state_manager_.state().armed, true);
   rc_values[2] = 1000; // throttle low
   rc_values[3] = 1000; // yaw left
-  board.set_rc(rc_values);
   stepFirmware(1100000);
   EXPECT_EQ(rf.state_manager_.state().armed, false);
 }
@@ -150,7 +151,6 @@ TEST_F(CommandManagerTest, ArmWithSwitch)
   rf.params_.set_param_int(PARAM_RC_ARM_CHANNEL, 4);
   rc_values[2] = 1000;     // throttle low
   rc_values[4] = CHN_HIGH; // switch on
-  board.set_rc(rc_values);
   stepFirmware(50000); // Immediate
   EXPECT_EQ(rf.state_manager_.state().armed, true);
 }
@@ -160,7 +160,6 @@ TEST_F(CommandManagerTest, DisarmWithStick)
   rf.params_.set_param_int(PARAM_RC_ARM_CHANNEL, 4);
   rf.state_manager_.set_event(StateManager::EVENT_REQUEST_ARM);
   rc_values[4] = CHN_LOW; // throttle low
-  board.set_rc(rc_values);
   stepFirmware(50000); // Immediate
   EXPECT_EQ(rf.state_manager_.state().armed, false);
 }
@@ -171,7 +170,6 @@ TEST_F(CommandManagerTest, DontDisarmWithSticksWhenUsingSwitch)
   rc_values[4] = CHN_HIGH; // switch on
   rc_values[2] = 1000;     // throttle low
   rf.state_manager_.set_event(StateManager::EVENT_REQUEST_ARM);
-  board.set_rc(rc_values);
   stepFirmware(50000);
   EXPECT_EQ(rf.state_manager_.state().armed, true);
 
@@ -187,7 +185,6 @@ TEST_F(CommandManagerTest, ArmStickReversed)
   rf.params_.set_param_int(PARAM_RC_ARM_CHANNEL, 4);
   rc_values[2] = 1000;    // throttle low
   rc_values[4] = CHN_LOW; // switch on
-  board.set_rc(rc_values);
   stepFirmware(50000); // Immediate
   EXPECT_EQ(rf.state_manager_.state().armed, true);
 }
@@ -201,14 +198,12 @@ TEST_F(CommandManagerTest, DisarmStickReversed)
 
   rc_values[2] = 1000;     // throttle low
   rc_values[4] = CHN_HIGH; // switch on
-  board.set_rc(rc_values);
   stepFirmware(50000); // Immediate
   EXPECT_EQ(rf.state_manager_.state().armed, false);
 }
 
 TEST_F(CommandManagerTest, DefaultRCOutputd)
 {
-  board.set_rc(rc_values);
   stepFirmware(600000);
 
   // Check the output
@@ -229,7 +224,6 @@ TEST_F(CommandManagerTest, RCOutput)
   rc_values[1] = 1750;
   rc_values[2] = 1500;
   rc_values[3] = 2000;
-  board.set_rc(rc_values);
   stepFirmware(600000);
 
   // Check the output
@@ -248,7 +242,7 @@ TEST_F(CommandManagerTest, RCOutput)
 TEST_F(CommandManagerTest, LoseRCDisarmed)
 {
   board.set_pwm_lost(true);
-  stepFirmware(20000);
+  stepFirmware(50000);
 
   control_t output = rf.command_manager_.combined_control();
   EXPECT_EQ(output.x.type, ANGLE);
@@ -280,7 +274,6 @@ TEST_F(CommandManagerTest, RegainRCDisarmed)
 
 TEST_F(CommandManagerTest, LoseRCArmed)
 {
-  board.set_rc(rc_values);
   stepFirmware(50000);
 
   rf.state_manager_.set_event(StateManager::EVENT_REQUEST_ARM);
@@ -307,7 +300,6 @@ TEST_F(CommandManagerTest, LoseRCArmed)
 
 TEST_F(CommandManagerTest, RegainRCArmed)
 {
-  board.set_rc(rc_values);
   stepFirmware(50000);
   rf.state_manager_.set_event(StateManager::EVENT_REQUEST_ARM);
 
@@ -360,7 +352,6 @@ TEST_F(CommandManagerTest, OffboardCommandMuxRollDeviation)
   stepFirmware(1100000); // Get past LAG_TIME
   rf.params_.set_param_int(PARAM_RC_OVERRIDE_TAKE_MIN_THROTTLE, true);
   rc_values[0] = 1250;
-  board.set_rc(rc_values);
   rf.command_manager_.set_new_offboard_command(offboard_command);
   stepFirmware(40000);
 
@@ -376,7 +367,6 @@ TEST_F(CommandManagerTest, OffboardCommandMuxPitchDeviation)
   stepFirmware(1100000); // Get past LAG_TIME
   rf.params_.set_param_int(PARAM_RC_OVERRIDE_TAKE_MIN_THROTTLE, true);
   rc_values[1] = 1750;
-  board.set_rc(rc_values);
   rf.command_manager_.set_new_offboard_command(offboard_command);
   stepFirmware(40000);
 
@@ -392,7 +382,6 @@ TEST_F(CommandManagerTest, OffboardCommandMuxYawrateDeviation)
   stepFirmware(1100000); // Get past LAG_TIME
   rf.params_.set_param_int(PARAM_RC_OVERRIDE_TAKE_MIN_THROTTLE, true);
   rc_values[3] = 1250;
-  board.set_rc(rc_values);
   rf.command_manager_.set_new_offboard_command(offboard_command);
   stepFirmware(40000);
 
@@ -408,15 +397,13 @@ TEST_F(CommandManagerTest, OffboardCommandMuxLag)
   stepFirmware(1100000); // Get past LAG_TIME
   rf.params_.set_param_int(PARAM_RC_OVERRIDE_TAKE_MIN_THROTTLE, true);
   rc_values[0] = 1250;
-  board.set_rc(rc_values);
   setOffboard(offboard_command);
   stepFirmware(40000);
 
   control_t output = rf.command_manager_.combined_control();
   EXPECT_CLOSE(output.x.value, -0.5 * rf.params_.get_param_float(PARAM_RC_MAX_ROLL));
 
   rc_values[0] = 1500; // return stick to center
-  board.set_rc(rc_values);
 
   stepFirmware(500000);
   setOffboard(offboard_command);

test/state_machine_test.cpp

@@ -24,6 +24,15 @@ class StateMachineTest : public ::testing::Test
     rf.params_.set_param_int(PARAM_MIXER, 10);
     rf.params_.set_param_int(PARAM_CALIBRATE_GYRO_ON_ARM, false); // default to turning this off
     rf.params_.set_param_float(PARAM_FAILSAFE_THROTTLE, 0.0f);
+
+    uint16_t rc_values[8];
+    for (int i = 0; i < 8; i++)
+    {
+      rc_values[i] = 1500;
+    }
+    rc_values[2] = 1000;
+    board.set_rc(rc_values);
+
     stepFirmware(100000);
   }
 

test/test_board.h

@@ -133,7 +133,8 @@ 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);
 };