sil_board.h
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2017 Daniel Koch, James Jackson and Gary Ellingson, BYU MAGICC Lab.
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright notice, this
9  * list of conditions and the following disclaimer.
10  *
11  * * Redistributions in binary form must reproduce the above copyright notice,
12  * this list of conditions and the following disclaimer in the documentation
13  * and/or other materials provided with the distribution.
14  *
15  * * Neither the name of the copyright holder nor the names of its
16  * contributors may be used to endorse or promote products derived from
17  * this software without specific prior written permission.
18  *
19  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
20  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
21  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
22  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
23  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
24  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
25  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
26  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
27  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
28  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
29  * POSSIBILITY OF SUCH DAMAGE.
30  */
31 
32 #ifndef ROSFLIGHT_SIM_SIL_BOARD_H
33 #define ROSFLIGHT_SIM_SIL_BOARD_H
34 
35 #include <cmath>
36 #include <cstdbool>
37 #include <cstddef>
38 #include <cstdint>
39 
40 #include <gazebo/common/Plugin.hh>
41 #include <gazebo/common/common.hh>
42 #include <gazebo/gazebo.hh>
43 #include <gazebo/physics/physics.hh>
44 
45 #include <ros/ros.h>
46 #include <rosflight_msgs/RCRaw.h>
47 
49 
51 
52 namespace rosflight_sim
53 {
55 {
56 private:
58 
60 
61  double gyro_stdev_;
64 
65  double acc_stdev_;
68 
70  double baro_stdev_;
72 
74  double mag_stdev_;
76 
80 
81  double sonar_stdev_;
84 
88 
92  double baro_bias_;
94 
95  std::default_random_engine random_generator_;
96  std::normal_distribution<double> normal_distribution_;
97  std::uniform_real_distribution<double> uniform_distribution_;
98 
103 
104  gazebo::physics::WorldPtr world_;
105  gazebo::physics::ModelPtr model_;
106  gazebo::physics::LinkPtr link_;
107 
110  rosflight_msgs::RCRaw latestRC_;
113 
114  std::string mav_type_;
115  int pwm_outputs_[14]; // assumes maximum of 14 channels
116 
117  // Time variables
118  gazebo::common::Time boot_time_;
121 
122  void RCCallback(const rosflight_msgs::RCRaw& msg);
123  bool motors_spinning();
124 
128  gazebo::common::Time last_time_;
129 
132  static constexpr size_t BACKUP_SRAM_SIZE{1024};
134 
135 public:
136  SIL_Board();
137 
138  // setup
139  void init_board(void) override;
140  void board_reset(bool bootloader) override;
141 
142  // clock
143  uint32_t clock_millis() override;
144  uint64_t clock_micros() override;
145  void clock_delay(uint32_t milliseconds) override;
146 
147  // sensors
148  void sensors_init() override;
149  uint16_t num_sensor_errors(void) override;
150 
151  bool new_imu_data() override;
152  bool imu_read(float accel[3], float* temperature, float gyro[3], uint64_t* time_us) override;
153  void imu_not_responding_error() override;
154 
155  bool mag_present(void) override;
156  void mag_read(float mag[3]) override;
157  void mag_update(void) override{};
158 
159  bool baro_present(void) override;
160  void baro_read(float* pressure, float* temperature) override;
161  void baro_update(void) override{};
162 
163  bool diff_pressure_present(void) override;
164  void diff_pressure_read(float* diff_pressure, float* temperature) override;
165  void diff_pressure_update(void) override{};
166 
167  bool sonar_present(void) override;
168  float sonar_read(void) override;
169  void sonar_update(void) override{};
170 
171  // PWM
172  // TODO make these deal in normalized (-1 to 1 or 0 to 1) values (not pwm-specific)
173  void pwm_init(uint32_t refresh_rate, uint16_t idle_pwm) override;
174  void pwm_write(uint8_t channel, float value) override;
175  void pwm_disable(void) override;
176 
177  // RC
178  float rc_read(uint8_t channel) override;
179  void rc_init(rc_type_t rc_type) override;
180  bool rc_lost(void) override;
181 
182  // non-volatile memory
183  void memory_init(void) override;
184  bool memory_read(void* dest, size_t len) override;
185  bool memory_write(const void* src, size_t len) override;
186 
187  // LEDs
188  void led0_on(void) override;
189  void led0_off(void) override;
190  void led0_toggle(void) override;
191 
192  void led1_on(void) override;
193  void led1_off(void) override;
194  void led1_toggle(void) override;
195 
196  // Backup Memory
197  void backup_memory_init() override;
198  bool backup_memory_read(void* dest, size_t len) override;
199  void backup_memory_write(const void* src, size_t len) override;
200  void backup_memory_clear(size_t len) override;
201 
202  bool gnss_present() override;
203  void gnss_update() override;
204 
206  bool gnss_has_new_data() override;
208 
209  bool battery_voltage_present() const override;
210  float battery_voltage_read() const override;
211  void battery_voltage_set_multiplier(double multiplier) override;
212 
213  bool battery_current_present() const override;
214  float battery_current_read() const override;
215  void battery_current_set_multiplier(double multiplier) override;
216 
217  // Gazebo stuff
218  void gazebo_setup(gazebo::physics::LinkPtr link,
219  gazebo::physics::WorldPtr world,
220  gazebo::physics::ModelPtr model,
221  ros::NodeHandle* nh,
222  std::string mav_type);
223  inline const int* get_outputs() const { return pwm_outputs_; }
224 #if GAZEBO_MAJOR_VERSION >= 9
225  gazebo::common::SphericalCoordinates sph_coord_;
226 #endif
227 };
228 
229 } // namespace rosflight_sim
230 
231 #endif // ROSFLIGHT_SIM_SIL_BOARD_H
uint8_t backup_memory_[BACKUP_SRAM_SIZE]
Definition: sil_board.h:133
static constexpr size_t BACKUP_SRAM_SIZE
Definition: sil_board.h:132
gazebo::common::Time last_time_
Definition: sil_board.h:128
gazebo::physics::WorldPtr world_
Definition: sil_board.h:104
bool battery_voltage_present() const override
Definition: sil_board.cpp:403
void baro_read(float *pressure, float *temperature) override
Definition: sil_board.cpp:329
bool gnss_has_new_data() override
Definition: sil_board.cpp:630
float pressure
std::uniform_real_distribution< double > uniform_distribution_
Definition: sil_board.h:97
void clock_delay(uint32_t milliseconds) override
Definition: sil_board.cpp:165
bool memory_write(const void *src, size_t len) override
Definition: sil_board.cpp:499
void memory_init(void) override
Definition: sil_board.cpp:480
float temperature
bool diff_pressure_present(void) override
Definition: sil_board.cpp:353
gazebo::math::Vector3 GazeboVector
Definition: gz_compat.h:72
bool sonar_present(void) override
Definition: sil_board.cpp:381
GazeboVector prev_vel_3_
Definition: sil_board.h:127
void led1_off(void) override
Definition: sil_board.cpp:533
void mag_read(float mag[3]) override
Definition: sil_board.cpp:294
const int * get_outputs() const
Definition: sil_board.h:223
rosflight_firmware::GNSSData gnss_read() override
Definition: sil_board.cpp:574
void mag_update(void) override
Definition: sil_board.h:157
bool rc_lost(void) override
Definition: sil_board.cpp:472
void sensors_init() override
Definition: sil_board.cpp:170
bool backup_memory_read(void *dest, size_t len) override
Definition: sil_board.cpp:538
GazeboVector gyro_bias_
Definition: sil_board.h:89
bool gnss_present() override
Definition: sil_board.cpp:568
uint64_t next_imu_update_time_us_
Definition: sil_board.h:119
void battery_voltage_set_multiplier(double multiplier) override
Definition: sil_board.cpp:413
void sonar_update(void) override
Definition: sil_board.h:169
void gazebo_setup(gazebo::physics::LinkPtr link, gazebo::physics::WorldPtr world, gazebo::physics::ModelPtr model, ros::NodeHandle *nh, std::string mav_type)
Definition: sil_board.cpp:56
ros::Subscriber rc_sub_
Definition: sil_board.h:109
float sonar_read(void) override
Definition: sil_board.cpp:386
ros::Time last_rc_message_
Definition: sil_board.h:112
void backup_memory_write(const void *src, size_t len) override
Definition: sil_board.cpp:549
void led1_on(void) override
Definition: sil_board.cpp:532
GazeboVector prev_vel_2_
Definition: sil_board.h:126
float battery_current_read() const override
Definition: sil_board.cpp:423
void backup_memory_clear(size_t len) override
Definition: sil_board.cpp:555
void diff_pressure_read(float *diff_pressure, float *temperature) override
Definition: sil_board.cpp:361
void pwm_init(uint32_t refresh_rate, uint16_t idle_pwm) override
Definition: sil_board.cpp:434
void led0_on(void) override
Definition: sil_board.cpp:528
std::normal_distribution< double > normal_distribution_
Definition: sil_board.h:96
uint32_t clock_millis() override
Definition: sil_board.cpp:153
bool imu_read(float accel[3], float *temperature, float gyro[3], uint64_t *time_us) override
Definition: sil_board.cpp:218
rosflight_msgs::RCRaw latestRC_
Definition: sil_board.h:110
gazebo::common::Time boot_time_
Definition: sil_board.h:118
GazeboVector acc_bias_
Definition: sil_board.h:90
void backup_memory_init() override
Definition: sil_board.cpp:536
void board_reset(bool bootloader) override
Definition: sil_board.cpp:149
gazebo::physics::LinkPtr link_
Definition: sil_board.h:106
void imu_not_responding_error() override
Definition: sil_board.cpp:289
bool new_imu_data() override
Definition: sil_board.cpp:204
double airspeed_bias_walk_stdev_
Definition: sil_board.h:77
bool battery_current_present() const override
Definition: sil_board.cpp:418
void led1_toggle(void) override
Definition: sil_board.cpp:534
void RCCallback(const rosflight_msgs::RCRaw &msg)
Definition: sil_board.cpp:561
void gnss_update() override
Definition: sil_board.cpp:572
void pwm_disable(void) override
Definition: sil_board.cpp:467
float battery_voltage_read() const override
Definition: sil_board.cpp:408
uint64_t clock_micros() override
Definition: sil_board.cpp:159
void battery_current_set_multiplier(double multiplier) override
Definition: sil_board.cpp:428
GazeboVector prev_vel_1_
Definition: sil_board.h:125
GazeboVector mag_bias_
Definition: sil_board.h:91
uint16_t num_sensor_errors(void) override
Definition: sil_board.cpp:199
GazeboVector gravity_
Definition: sil_board.h:99
void diff_pressure_update(void) override
Definition: sil_board.h:165
bool baro_present(void) override
Definition: sil_board.cpp:324
float rc_read(uint8_t channel) override
Definition: sil_board.cpp:449
void baro_update(void) override
Definition: sil_board.h:161
GazeboVector inertial_magnetic_field_
Definition: sil_board.h:57
bool mag_present(void) override
Definition: sil_board.cpp:319
std::default_random_engine random_generator_
Definition: sil_board.h:95
void init_board(void) override
Definition: sil_board.cpp:42
gazebo::physics::ModelPtr model_
Definition: sil_board.h:105
uint64_t imu_update_period_us_
Definition: sil_board.h:120
void rc_init(rc_type_t rc_type) override
Definition: sil_board.cpp:477
ros::NodeHandle * nh_
Definition: sil_board.h:108
rosflight_firmware::GNSSFull gnss_full_read() override
Definition: sil_board.cpp:634
void led0_off(void) override
Definition: sil_board.cpp:529
void led0_toggle(void) override
Definition: sil_board.cpp:530
void pwm_write(uint8_t channel, float value) override
Definition: sil_board.cpp:463
bool memory_read(void *dest, size_t len) override
Definition: sil_board.cpp:482


rosflight_sim
Author(s): James Jackson, Gary Ellingson, Daniel Koch
autogenerated on Thu Apr 15 2021 05:09:58