@ -1,8 +1,8 @@
@@ -1,8 +1,8 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/*****************************************************************************
The init_ardupilot function processes everything we need for an in - air restart
We will determine later if we are actually on the ground and process a
ground start in that case .
We will determine later if we are actually on the ground and process a
ground start in that case .
* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
@ -14,26 +14,26 @@ The init_ardupilot function processes everything we need for an in - air restart
@@ -14,26 +14,26 @@ The init_ardupilot function processes everything we need for an in - air restart
// This is the help function
int8_t Rover : : main_menu_help ( uint8_t argc , const Menu : : arg * argv )
{
cliSerial - > printf ( " Commands: \n "
" logs log readback/setup mode \n "
" setup setup mode \n "
" test test mode \n "
" \n "
" Move the slide switch and reset to FLY. \n "
" \n " ) ;
return ( 0 ) ;
cliSerial - > printf ( " Commands: \n "
" logs log readback/setup mode \n "
" setup setup mode \n "
" test test mode \n "
" \n "
" Move the slide switch and reset to FLY. \n "
" \n " ) ;
return ( 0 ) ;
}
// Command/function table for the top-level menu.
static const struct Menu : : command main_menu_commands [ ] = {
// command function called
// command function called
// ======= ===============
{ " logs " , MENU_FUNC ( process_logs ) } ,
{ " setup " , MENU_FUNC ( setup_mode ) } ,
{ " test " , MENU_FUNC ( test_mode ) } ,
{ " logs " , MENU_FUNC ( process_logs ) } ,
{ " setup " , MENU_FUNC ( setup_mode ) } ,
{ " test " , MENU_FUNC ( test_mode ) } ,
{ " reboot " , MENU_FUNC ( reboot_board ) } ,
{ " help " , MENU_FUNC ( main_menu_help ) }
{ " help " , MENU_FUNC ( main_menu_help ) }
} ;
// Create the top-level menu object.
@ -80,14 +80,14 @@ void Rover::init_ardupilot()
@@ -80,14 +80,14 @@ void Rover::init_ardupilot()
// initialise console serial port
serial_manager . init_console ( ) ;
cliSerial - > printf ( " \n \n Init " FIRMWARE_STRING
" \n \n Free RAM: %u \n " ,
hal . util - > available_memory ( ) ) ;
//
// Check the EEPROM format version before loading any parameters from EEPROM.
//
cliSerial - > printf ( " \n \n Init " FIRMWARE_STRING
" \n \n Free RAM: %u \n " ,
hal . util - > available_memory ( ) ) ;
//
// Check the EEPROM format version before loading any parameters from EEPROM.
//
load_parameters ( ) ;
BoardConfig . init ( ) ;
@ -108,11 +108,11 @@ void Rover::init_ardupilot()
@@ -108,11 +108,11 @@ void Rover::init_ardupilot()
// init baro before we start the GCS, so that the CLI baro test works
barometer . init ( ) ;
// init the GCS
// init the GCS
gcs [ 0 ] . setup_uart ( serial_manager , AP_SerialManager : : SerialProtocol_Console , 0 ) ;
// we start by assuming USB connected, as we initialed the serial
// port with SERIAL0_BAUD. check_usb_mux() fixes this if need be.
// port with SERIAL0_BAUD. check_usb_mux() fixes this if need be.
usb_connected = true ;
check_usb_mux ( ) ;
@ -130,7 +130,7 @@ void Rover::init_ardupilot()
@@ -130,7 +130,7 @@ void Rover::init_ardupilot()
frsky_telemetry . init ( serial_manager ) ;
# endif
mavlink_system . sysid = g . sysid_this_mav ;
mavlink_system . sysid = g . sysid_this_mav ;
# if LOGGING_ENABLED == ENABLED
log_init ( ) ;
@ -142,29 +142,29 @@ void Rover::init_ardupilot()
@@ -142,29 +142,29 @@ void Rover::init_ardupilot()
// more than 5ms remaining in your call to hal.scheduler->delay
hal . scheduler - > register_delay_callback ( mavlink_delay_cb_static , 5 ) ;
if ( g . compass_enabled = = true ) {
if ( ! compass . init ( ) | | ! compass . read ( ) ) {
if ( g . compass_enabled = = true ) {
if ( ! compass . init ( ) | | ! compass . read ( ) ) {
cliSerial - > println ( " Compass initialisation failed! " ) ;
g . compass_enabled = false ;
} else {
ahrs . set_compass ( & compass ) ;
//compass.get_offsets(); // load offsets to account for airframe magnetic interference
//compass.get_offsets(); // load offsets to account for airframe magnetic interference
}
}
}
// initialise sonar
// initialise sonar
init_sonar ( ) ;
// and baro for EKF
init_barometer ( ) ;
// Do GPS init
// Do GPS init
gps . init ( & DataFlash , serial_manager ) ;
rc_override_active = hal . rcin - > set_overrides ( rc_override , 8 ) ;
init_rc_in ( ) ; // sets up rc channels from radio
init_rc_out ( ) ; // sets up the timer libs
init_rc_in ( ) ; // sets up rc channels from radio
init_rc_out ( ) ; // sets up the timer libs
relay . init ( ) ;
@ -181,12 +181,12 @@ void Rover::init_ardupilot()
@@ -181,12 +181,12 @@ void Rover::init_ardupilot()
# if CLI_ENABLED == ENABLED
// If the switch is in 'menu' mode, run the main menu.
//
// Since we can't be sure that the setup or test mode won't leave
// the system in an odd state, we don't let the user exit the top
// menu; they must reset in order to fly.
//
// If the switch is in 'menu' mode, run the main menu.
//
// Since we can't be sure that the setup or test mode won't leave
// the system in an odd state, we don't let the user exit the top
// menu; they must reset in order to fly.
//
if ( g . cli_enabled = = 1 ) {
const char * msg = " \n Press ENTER 3 times to start interactive setup \n " ;
cliSerial - > println ( msg ) ;
@ -199,15 +199,15 @@ void Rover::init_ardupilot()
@@ -199,15 +199,15 @@ void Rover::init_ardupilot()
}
# endif
init_capabilities ( ) ;
init_capabilities ( ) ;
startup_ground ( ) ;
startup_ground ( ) ;
set_mode ( ( enum mode ) g . initial_mode . get ( ) ) ;
// set the correct flight mode
// ---------------------------
reset_control_switch ( ) ;
// set the correct flight mode
// ---------------------------
reset_control_switch ( ) ;
}
//********************************************************************************
@ -216,23 +216,23 @@ void Rover::init_ardupilot()
@@ -216,23 +216,23 @@ void Rover::init_ardupilot()
void Rover : : startup_ground ( void )
{
set_mode ( INITIALISING ) ;
gcs_send_text ( MAV_SEVERITY_INFO , " <startup_ground> Ground start " ) ;
# if(GROUND_START_DELAY > 0)
gcs_send_text ( MAV_SEVERITY_NOTICE , " <startup_ground> With delay " ) ;
delay ( GROUND_START_DELAY * 1000 ) ;
# endif
gcs_send_text ( MAV_SEVERITY_INFO , " <startup_ground> Ground start " ) ;
# if(GROUND_START_DELAY > 0)
gcs_send_text ( MAV_SEVERITY_NOTICE , " <startup_ground> With delay " ) ;
delay ( GROUND_START_DELAY * 1000 ) ;
# endif
//IMU ground start
//------------------------
//IMU ground start
//------------------------
//
startup_INS_ground ( ) ;
startup_INS_ground ( ) ;
// read the radio to set trims
// ---------------------------
trim_radio ( ) ;
// read the radio to set trims
// ---------------------------
trim_radio ( ) ;
// initialise mission library
mission . init ( ) ;
@ -244,7 +244,7 @@ void Rover::startup_ground(void)
@@ -244,7 +244,7 @@ void Rover::startup_ground(void)
ins . set_raw_logging ( should_log ( MASK_LOG_IMU_RAW ) ) ;
ins . set_dataflash ( & DataFlash ) ;
gcs_send_text ( MAV_SEVERITY_INFO , " Ready to drive " ) ;
gcs_send_text ( MAV_SEVERITY_INFO , " Ready to drive " ) ;
}
/*
@ -256,23 +256,24 @@ void Rover::set_reverse(bool reverse)
@@ -256,23 +256,24 @@ void Rover::set_reverse(bool reverse)
if ( in_reverse = = reverse ) {
return ;
}
g . pidSpeedThrottle . reset_I ( ) ;
g . pidSpeedThrottle . reset_I ( ) ;
in_reverse = reverse ;
}
void Rover : : set_mode ( enum mode mode )
{
{
if ( control_mode = = mode ) {
// don't switch modes if we are already in the correct mode.
return ;
}
if ( control_mode = = mode ) {
// don't switch modes if we are already in the correct mode.
return ;
}
// If we are changing out of AUTO mode reset the loiter timer
if ( control_mode = = AUTO )
if ( control_mode = = AUTO ) {
loiter_time = 0 ;
}
control_mode = mode ;
control_mode = mode ;
throttle_last = 0 ;
throttle = 500 ;
set_reverse ( false ) ;
@ -281,45 +282,44 @@ void Rover::set_mode(enum mode mode)
@@ -281,45 +282,44 @@ void Rover::set_mode(enum mode mode)
if ( control_mode ! = AUTO ) {
auto_triggered = false ;
}
switch ( control_mode )
{
case MANUAL :
case HOLD :
case LEARNING :
case STEERING :
auto_throttle_mode = false ;
break ;
case AUTO :
auto_throttle_mode = true ;
rtl_complete = false ;
restart_nav ( ) ;
break ;
case RTL :
auto_throttle_mode = true ;
do_RTL ( ) ;
break ;
case GUIDED :
auto_throttle_mode = true ;
rtl_complete = false ;
/*
when entering guided mode we set the target as the current
location . This matches the behaviour of the copter code .
*/
guided_WP = current_loc ;
set_guided_WP ( ) ;
break ;
default :
auto_throttle_mode = true ;
do_RTL ( ) ;
break ;
}
switch ( control_mode ) {
case MANUAL :
case HOLD :
case LEARNING :
case STEERING :
auto_throttle_mode = false ;
break ;
case AUTO :
auto_throttle_mode = true ;
rtl_complete = false ;
restart_nav ( ) ;
break ;
case RTL :
auto_throttle_mode = true ;
do_RTL ( ) ;
break ;
case GUIDED :
auto_throttle_mode = true ;
rtl_complete = false ;
/*
when entering guided mode we set the target as the current
location . This matches the behaviour of the copter code .
*/
guided_WP = current_loc ;
set_guided_WP ( ) ;
break ;
default :
auto_throttle_mode = true ;
do_RTL ( ) ;
break ;
}
if ( should_log ( MASK_LOG_MODE ) ) {
if ( should_log ( MASK_LOG_MODE ) ) {
DataFlash . Log_Write_Mode ( control_mode ) ;
}
}
@ -344,7 +344,7 @@ bool Rover::mavlink_set_mode(uint8_t mode)
@@ -344,7 +344,7 @@ bool Rover::mavlink_set_mode(uint8_t mode)
}
/*
called to set / unset a failsafe event .
called to set / unset a failsafe event .
*/
void Rover : : failsafe_trigger ( uint8_t failsafe_type , bool on )
{
@ -365,8 +365,8 @@ void Rover::failsafe_trigger(uint8_t failsafe_type, bool on)
@@ -365,8 +365,8 @@ void Rover::failsafe_trigger(uint8_t failsafe_type, bool on)
failsafe . triggered & = failsafe . bits ;
if ( failsafe . triggered = = 0 & &
failsafe . bits ! = 0 & &
if ( failsafe . triggered = = 0 & &
failsafe . bits ! = 0 & &
millis ( ) - failsafe . start_time > g . fs_timeout * 1000 & &
control_mode ! = RTL & &
control_mode ! = HOLD ) {
@ -388,15 +388,15 @@ void Rover::failsafe_trigger(uint8_t failsafe_type, bool on)
@@ -388,15 +388,15 @@ void Rover::failsafe_trigger(uint8_t failsafe_type, bool on)
void Rover : : startup_INS_ground ( void )
{
gcs_send_text ( MAV_SEVERITY_INFO , " Warming up ADC " ) ;
mavlink_delay ( 500 ) ;
mavlink_delay ( 500 ) ;
// Makes the servos wiggle twice - about to begin INS calibration - HOLD LEVEL AND STILL!!
// -----------------------
// Makes the servos wiggle twice - about to begin INS calibration - HOLD LEVEL AND STILL!!
// -----------------------
gcs_send_text ( MAV_SEVERITY_INFO , " Beginning INS calibration. Do not move vehicle " ) ;
mavlink_delay ( 1000 ) ;
mavlink_delay ( 1000 ) ;
ahrs . init ( ) ;
ahrs . set_fly_forward ( true ) ;
ahrs . set_fly_forward ( true ) ;
ahrs . set_vehicle_class ( AHRS_VEHICLE_GROUND ) ;
ins . init ( scheduler . get_loop_rate_hz ( ) ) ;
@ -411,9 +411,9 @@ void Rover::update_notify()
@@ -411,9 +411,9 @@ void Rover::update_notify()
}
void Rover : : resetPerfData ( void ) {
mainLoop_count = 0 ;
G_Dt_max = 0 ;
perf_mon_timer = millis ( ) ;
mainLoop_count = 0 ;
G_Dt_max = 0 ;
perf_mon_timer = millis ( ) ;
}