2015-05-12 04:00:25 -03:00
|
|
|
/*
|
|
|
|
This program is free software: you can redistribute it and/or modify
|
|
|
|
it under the terms of the GNU General Public License as published by
|
|
|
|
the Free Software Foundation, either version 3 of the License, or
|
|
|
|
(at your option) any later version.
|
|
|
|
|
|
|
|
This program is distributed in the hope that it will be useful,
|
|
|
|
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
|
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
|
|
GNU General Public License for more details.
|
|
|
|
|
|
|
|
You should have received a copy of the GNU General Public License
|
|
|
|
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
|
|
|
*/
|
|
|
|
/*
|
|
|
|
main Rover class, containing all vehicle specific state
|
|
|
|
*/
|
|
|
|
|
2015-05-13 00:16:45 -03:00
|
|
|
#include "Rover.h"
|
2016-05-05 19:10:08 -03:00
|
|
|
#include "version.h"
|
2015-05-13 00:16:45 -03:00
|
|
|
|
2015-05-12 04:00:25 -03:00
|
|
|
Rover::Rover(void) :
|
|
|
|
param_loader(var_info),
|
2016-10-28 13:57:14 -03:00
|
|
|
channel_steer(nullptr),
|
|
|
|
channel_throttle(nullptr),
|
|
|
|
channel_learn(nullptr),
|
2017-06-27 02:24:14 -03:00
|
|
|
DataFlash{FIRMWARE_STRING, g.log_bitmask},
|
2015-05-12 04:00:25 -03:00
|
|
|
modes(&g.mode1),
|
2017-03-19 22:12:34 -03:00
|
|
|
L1_controller(ahrs, nullptr),
|
2015-05-12 04:00:25 -03:00
|
|
|
nav_controller(&L1_controller),
|
|
|
|
steerController(ahrs),
|
2015-05-24 20:24:11 -03:00
|
|
|
mission(ahrs,
|
|
|
|
FUNCTOR_BIND_MEMBER(&Rover::start_command, bool, const AP_Mission::Mission_Command&),
|
2015-07-24 05:20:49 -03:00
|
|
|
FUNCTOR_BIND_MEMBER(&Rover::verify_command_callback, bool, const AP_Mission::Mission_Command&),
|
2015-05-24 20:24:11 -03:00
|
|
|
FUNCTOR_BIND_MEMBER(&Rover::exit_mission, void)),
|
2015-05-13 00:45:36 -03:00
|
|
|
ServoRelayEvents(relay),
|
2015-05-12 04:00:25 -03:00
|
|
|
#if CAMERA == ENABLED
|
|
|
|
camera(&relay),
|
|
|
|
#endif
|
|
|
|
#if MOUNT == ENABLED
|
|
|
|
camera_mount(ahrs, current_loc),
|
|
|
|
#endif
|
2017-07-18 23:19:08 -03:00
|
|
|
control_mode(&mode_initializing),
|
2015-05-12 04:00:25 -03:00
|
|
|
throttle(500),
|
|
|
|
#if FRSKY_TELEM_ENABLED == ENABLED
|
2017-07-13 08:36:44 -03:00
|
|
|
frsky_telemetry(ahrs, battery, rangefinder),
|
2015-05-12 04:00:25 -03:00
|
|
|
#endif
|
2017-01-18 13:34:42 -04:00
|
|
|
do_auto_rotation(false),
|
2017-05-17 20:09:21 -03:00
|
|
|
home(ahrs.get_home()),
|
2017-01-31 05:46:32 -04:00
|
|
|
G_Dt(0.02f)
|
2015-05-12 04:00:25 -03:00
|
|
|
{
|
|
|
|
}
|