rosflight.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2017, James Jackson and Daniel Koch, BYU MAGICC Lab
3  *
4  * All rights reserved.
5  *
6  * Redistribution and use in source and binary forms, with or without
7  * modification, are permitted provided that the following conditions are met:
8  *
9  * * Redistributions of source code must retain the above copyright notice, this
10  * list of conditions and the following disclaimer.
11  *
12  * * Redistributions in binary form must reproduce the above copyright notice,
13  * this list of conditions and the following disclaimer in the documentation
14  * and/or other materials provided with the distribution.
15  *
16  * * Neither the name of the copyright holder nor the names of its
17  * contributors may be used to endorse or promote products derived from
18  * this software without specific prior written permission.
19  *
20  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
21  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
22  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
23  * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
24  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
25  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
26  * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
27  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
28  * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
29  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
30  */
31 
32 #include "rosflight.h"
33 
35 
36 namespace rosflight_firmware
37 {
39  board_(board),
40  comm_manager_(*this, comm_link),
41  params_(*this),
42  command_manager_(*this),
43  controller_(*this),
44  estimator_(*this),
45  mixer_(*this),
46  rc_(*this),
47  sensors_(*this),
48  state_manager_(*this)
49 {
50  comm_link.set_listener(&comm_manager_);
52 }
53 
54 // Initialization Routine
56 {
57  // Initialize the arming finite state machine
59 
60  // Read EEPROM to get initial params
61  params_.init();
62 
63  // Initialize Mixer
64  mixer_.init();
65 
66  /***********************/
67  /*** Hardware Setup ***/
68  /***********************/
69 
70  // Initialize PWM and RC
71  rc_.init();
72 
73  // Initialize MAVlink Communication
75 
76  // Initialize Sensors
77  sensors_.init();
78 
79  /***********************/
80  /*** Software Setup ***/
81  /***********************/
82 
83  // Initialize Estimator
84  estimator_.init();
85 
86  // Initialize Controller
87  controller_.init();
88 
89  // Initialize the command muxer
91 
92  /***************************/
93  /*** Hardfault Recovery ***/
94  /***************************/
95 
97 }
98 
99 // Main loop
101 {
102  /*********************/
103  /*** Control Loop ***/
104  /*********************/
105  uint64_t start = board_.clock_micros();
106  if (sensors_.run())
107  {
108  // If I have new IMU data, then perform control
109  estimator_.run();
110  controller_.run();
111  mixer_.mix_output();
112  loop_time_us = board_.clock_micros() - start;
113  }
114 
115  /*********************/
116  /*** Post-Process ***/
117  /*********************/
118  // internal timers figure out what and when to send
120 
121  // receive mavlink messages
123 
124  // update the state machine, an internal timer runs this at a fixed rate
126 
127  // get RC, an internal timer runs this every 20 ms (50 Hz)
128  rc_.run();
129 
130  // update commands (internal logic tells whether or not we should do anything or not)
132 }
133 
135 {
136  return loop_time_us;
137 }
138 
139 } // namespace rosflight_firmware
ROSCPP_DECL void start()
static constexpr size_t num_param_listeners_
ParamListenerInterface *const param_listeners_[num_param_listeners_]
void run()
Main loop for the ROSflight autopilot flight stack.
Definition: rosflight.cpp:100
virtual uint64_t clock_micros()=0
void set_listeners(ParamListenerInterface *const listeners[], size_t num_listeners)
Specify listeners for parameter changes.
Definition: param.cpp:292
virtual void set_listener(ListenerInterface *listener)=0
void init()
Initialize parameter values.
Definition: param.cpp:99
void init()
Main initialization routine for the ROSflight autopilot flight stack.
Definition: rosflight.cpp:55
ROSflight(Board &board, CommLinkInterface &comm_link)
Definition: rosflight.cpp:38


rosflight_firmware
Author(s): Daniel Koch , James Jackson
autogenerated on Sat May 9 2020 03:16:53