//******************************************************************************
//This function does all the calibrations, etc. that we need during a ground start
//******************************************************************************
void Copter::startup_ground(bool force_gyro_cal)
{
gcs_send_text_P(SEVERITY_LOW,PSTR("GROUND START"));
// initialise ahrs (may push imu calibration into the mpu6000 if using that device).
ahrs.init();
ahrs.set_vehicle_class(AHRS_VEHICLE_COPTER);
// Warm up and read Gyro offsets
// -----------------------------
ins.init(force_gyro_cal?AP_InertialSensor::COLD_START:AP_InertialSensor::WARM_START,
ins_sample_rate);
#if CLI_ENABLED == ENABLED
report_ins();
#endif
// reset ahrs gyro bias
if (force_gyro_cal) {
ahrs.reset_gyro_drift();
}
// set landed flag
set_land_complete(true);
set_land_complete_maybe(true);
}
// init_disarm_motors - disarm motors
void Copter::init_disarm_motors()
{
// return immediately if we are already disarmed
if (!motors.armed()) {
return;
}
#if HIL_MODE != HIL_MODE_DISABLED || CONFIG_HAL_BOARD == HAL_BOARD_SITL
gcs_send_text(MAV_SEVERITY_INFO, "Disarming motors");
#endif
// save compass offsets learned by the EKF if enabled
if (ahrs.use_compass() && compass.get_learn_type() == Compass::LEARN_EKF) {
for(uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) {
Vector3f magOffsets;
if (ahrs.getMagOffsets(i, magOffsets)) {
compass.set_and_save_offsets(i, magOffsets);
}
}
}
#if AUTOTUNE_ENABLED == ENABLED
// save auto tuned parameters
autotune_save_tuning_gains();
#endif
// we are not in the air
set_land_complete(true);
set_land_complete_maybe(true);
// log disarm to the dataflash
Log_Write_Event(DATA_DISARMED);
// send disarm command to motors
motors.armed(false);
// reset the mission
mission.reset();
// suspend logging
if (!DataFlash.log_while_disarmed()) {
DataFlash.EnableWrites(false);
}
DataFlash_Class::instance()->set_vehicle_armed(false);
// disable gps velocity based centrefugal force compensation
ahrs.set_correct_centrifugal(false);
hal.util->set_soft_armed(false);
ap.in_arming_delay = false;
}
// init_disarm_motors - disarm motors
void Copter::init_disarm_motors()
{
// return immediately if we are already disarmed
if (!motors.armed()) {
return;
}
#if HIL_MODE != HIL_MODE_DISABLED || CONFIG_HAL_BOARD == HAL_BOARD_SITL
gcs_send_text(MAV_SEVERITY_INFO, "DISARMING MOTORS");
#endif
// save compass offsets learned by the EKF
Vector3f magOffsets;
if (ahrs.use_compass() && ahrs.getMagOffsets(magOffsets)) {
compass.set_and_save_offsets(compass.get_primary(), magOffsets);
}
#if AUTOTUNE_ENABLED == ENABLED
// save auto tuned parameters
autotune_save_tuning_gains();
#endif
// we are not in the air
set_land_complete(true);
set_land_complete_maybe(true);
// log disarm to the dataflash
Log_Write_Event(DATA_DISARMED);
// send disarm command to motors
motors.armed(false);
// reset the mission
mission.reset();
// suspend logging
if (!(g.log_bitmask & MASK_LOG_WHEN_DISARMED)) {
DataFlash.EnableWrites(false);
}
// disable gps velocity based centrefugal force compensation
ahrs.set_correct_centrifugal(false);
hal.util->set_soft_armed(false);
}
void Copter::init_ardupilot()
{
if (!hal.gpio->usb_connected()) {
// USB is not connected, this means UART0 may be a Xbee, with
// its darned bricking problem. We can't write to it for at
// least one second after powering up. Simplest solution for
// now is to delay for 1 second. Something more elegant may be
// added later
delay(1000);
}
// initialise serial port
serial_manager.init_console();
// init vehicle capabilties
init_capabilities();
cliSerial->printf("\n\nInit " FIRMWARE_STRING
"\n\nFree RAM: %u\n",
(unsigned)hal.util->available_memory());
//
// Report firmware version code expect on console (check of actual EEPROM format version is done in load_parameters function)
//
report_version();
// load parameters from EEPROM
load_parameters();
BoardConfig.init();
// initialise serial port
serial_manager.init();
// init EPM cargo gripper
#if EPM_ENABLED == ENABLED
epm.init();
#endif
// initialise notify system
// disable external leds if epm is enabled because of pin conflict on the APM
notify.init(true);
// initialise battery monitor
battery.init();
// Init RSSI
rssi.init();
barometer.init();
// Register the mavlink service callback. This will run
// anytime there are more than 5ms remaining in a call to
// hal.scheduler->delay.
hal.scheduler->register_delay_callback(mavlink_delay_cb_static, 5);
// we start by assuming USB connected, as we initialed the serial
// port with SERIAL0_BAUD. check_usb_mux() fixes this if need be.
ap.usb_connected = true;
check_usb_mux();
// init the GCS connected to the console
gcs[0].setup_uart(serial_manager, AP_SerialManager::SerialProtocol_Console, 0);
// init telemetry port
gcs[1].setup_uart(serial_manager, AP_SerialManager::SerialProtocol_MAVLink, 0);
// setup serial port for telem2
gcs[2].setup_uart(serial_manager, AP_SerialManager::SerialProtocol_MAVLink, 1);
// setup serial port for fourth telemetry port (not used by default)
gcs[3].setup_uart(serial_manager, AP_SerialManager::SerialProtocol_MAVLink, 2);
#if FRSKY_TELEM_ENABLED == ENABLED
// setup frsky
frsky_telemetry.init(serial_manager);
#endif
// identify ourselves correctly with the ground station
mavlink_system.sysid = g.sysid_this_mav;
#if LOGGING_ENABLED == ENABLED
log_init();
#endif
GCS_MAVLINK::set_dataflash(&DataFlash);
// update motor interlock state
update_using_interlock();
#if FRAME_CONFIG == HELI_FRAME
// trad heli specific initialisation
heli_init();
#endif
init_rc_in(); // sets up rc channels from radio
init_rc_out(); // sets up motors and output to escs
// initialise which outputs Servo and Relay events can use
ServoRelayEvents.set_channel_mask(~motors.get_motor_mask());
relay.init();
/*
* setup the 'main loop is dead' check. Note that this relies on
* the RC library being initialised.
*/
hal.scheduler->register_timer_failsafe(failsafe_check_static, 1000);
// Do GPS init
gps.init(&DataFlash, serial_manager);
if(g.compass_enabled)
init_compass();
#if OPTFLOW == ENABLED
// make optflow available to AHRS
ahrs.set_optflow(&optflow);
#endif
// init Location class
Location_Class::set_ahrs(&ahrs);
#if AP_TERRAIN_AVAILABLE && AC_TERRAIN
Location_Class::set_terrain(&terrain);
wp_nav.set_terrain(&terrain);
#endif
pos_control.set_dt(MAIN_LOOP_SECONDS);
// init the optical flow sensor
init_optflow();
#if MOUNT == ENABLED
// initialise camera mount
camera_mount.init(&DataFlash, serial_manager);
#endif
#if PRECISION_LANDING == ENABLED
// initialise precision landing
init_precland();
#endif
#ifdef USERHOOK_INIT
USERHOOK_INIT
#endif
#if CLI_ENABLED == ENABLED
if (g.cli_enabled) {
const char *msg = "\nPress ENTER 3 times to start interactive setup\n";
cliSerial->println(msg);
if (gcs[1].initialised && (gcs[1].get_uart() != NULL)) {
gcs[1].get_uart()->println(msg);
}
if (num_gcs > 2 && gcs[2].initialised && (gcs[2].get_uart() != NULL)) {
gcs[2].get_uart()->println(msg);
}
}
#endif // CLI_ENABLED
#if HIL_MODE != HIL_MODE_DISABLED
while (barometer.get_last_update() == 0) {
// the barometer begins updating when we get the first
// HIL_STATE message
gcs_send_text(MAV_SEVERITY_WARNING, "Waiting for first HIL_STATE message");
delay(1000);
}
// set INS to HIL mode
ins.set_hil_mode();
#endif
// read Baro pressure at ground
//-----------------------------
init_barometer(true);
// initialise sonar
#if CONFIG_SONAR == ENABLED
init_sonar();
#endif
// initialise AP_RPM library
rpm_sensor.init();
// initialise mission library
mission.init();
// initialise the flight mode and aux switch
// ---------------------------
reset_control_switch();
init_aux_switches();
startup_INS_ground();
// set landed flags
set_land_complete(true);
set_land_complete_maybe(true);
// we don't want writes to the serial port to cause us to pause
// mid-flight, so set the serial ports non-blocking once we are
// ready to fly
serial_manager.set_blocking_writes_all(false);
// enable CPU failsafe
failsafe_enable();
ins.set_raw_logging(should_log(MASK_LOG_IMU_RAW));
ins.set_dataflash(&DataFlash);
cliSerial->print("\nReady to FLY ");
// flag that initialisation has completed
ap.initialised = true;
}
// update_land_detector - checks if we have landed and updates the ap.land_complete flag
// called at MAIN_LOOP_RATE
void Copter::update_land_detector()
{
// land detector can not use the following sensors because they are unreliable during landing
// barometer altitude : ground effect can cause errors larger than 4m
// EKF vertical velocity or altitude : poor barometer and large acceleration from ground impact
// earth frame angle or angle error : landing on an uneven surface will force the airframe to match the ground angle
// gyro output : on uneven surface the airframe may rock back an forth after landing
// range finder : tend to be problematic at very short distances
// input throttle : in slow land the input throttle may be only slightly less than hover
if (!motors.armed()) {
// if disarmed, always landed.
set_land_complete(true);
} else if (ap.land_complete) {
#if FRAME_CONFIG == HELI_FRAME
// if rotor speed and collective pitch are high then clear landing flag
if (motors.get_throttle() > get_non_takeoff_throttle() && motors.rotor_runup_complete()) {
#else
// if throttle output is high then clear landing flag
if (motors.get_throttle() > get_non_takeoff_throttle()) {
#endif
set_land_complete(false);
}
} else {
#if FRAME_CONFIG == HELI_FRAME
// check that collective pitch is on lower limit (should be constrained by LAND_COL_MIN)
bool motor_at_lower_limit = motors.limit.throttle_lower;
#else
// check that the average throttle output is near minimum (less than 12.5% hover throttle)
bool motor_at_lower_limit = motors.limit.throttle_lower && motors.is_throttle_mix_min();
#endif
// check that the airframe is not accelerating (not falling or breaking after fast forward flight)
bool accel_stationary = (land_accel_ef_filter.get().length() <= LAND_DETECTOR_ACCEL_MAX);
if (motor_at_lower_limit && accel_stationary) {
// landed criteria met - increment the counter and check if we've triggered
if( land_detector_count < ((float)LAND_DETECTOR_TRIGGER_SEC)*MAIN_LOOP_RATE) {
land_detector_count++;
} else {
set_land_complete(true);
}
} else {
// we've sensed movement up or down so reset land_detector
land_detector_count = 0;
}
}
set_land_complete_maybe(ap.land_complete || (land_detector_count >= LAND_DETECTOR_MAYBE_TRIGGER_SEC*MAIN_LOOP_RATE));
}
void Copter::set_land_complete(bool b)
{
// if no change, exit immediately
if( ap.land_complete == b )
return;
land_detector_count = 0;
if(b){
Log_Write_Event(DATA_LAND_COMPLETE);
} else {
Log_Write_Event(DATA_NOT_LANDED);
}
ap.land_complete = b;
}
// set land complete maybe flag
void Copter::set_land_complete_maybe(bool b)
{
// if no change, exit immediately
if (ap.land_complete_maybe == b)
return;
if (b) {
Log_Write_Event(DATA_LAND_COMPLETE_MAYBE);
}
ap.land_complete_maybe = b;
}
// update_throttle_thr_mix - sets motors throttle_low_comp value depending upon vehicle state
// low values favour pilot/autopilot throttle over attitude control, high values favour attitude control over throttle
// has no effect when throttle is above hover throttle
void Copter::update_throttle_thr_mix()
{
#if FRAME_CONFIG != HELI_FRAME
// if disarmed or landed prioritise throttle
if(!motors.armed() || ap.land_complete) {
motors.set_throttle_mix_min();
return;
}
if (mode_has_manual_throttle(control_mode)) {
// manual throttle
if(channel_throttle->get_control_in() <= 0) {
motors.set_throttle_mix_min();
} else {
motors.set_throttle_mix_mid();
}
} else {
// autopilot controlled throttle
// check for aggressive flight requests - requested roll or pitch angle below 15 degrees
const Vector3f angle_target = attitude_control.get_att_target_euler_cd();
bool large_angle_request = (norm(angle_target.x, angle_target.y) > 1500.0f);
// check for large external disturbance - angle error over 30 degrees
const Vector3f angle_error = attitude_control.get_att_error_rot_vec_cd();
bool large_angle_error = (norm(angle_error.x, angle_error.y) > 3000.0f);
// check for large acceleration - falling or high turbulence
Vector3f accel_ef = ahrs.get_accel_ef_blended();
accel_ef.z += GRAVITY_MSS;
bool accel_moving = (accel_ef.length() > 3.0f);
// check for requested decent
bool descent_not_demanded = pos_control.get_desired_velocity().z >= 0.0f;
if ( large_angle_request || large_angle_error || accel_moving || descent_not_demanded) {
motors.set_throttle_mix_max();
} else {
motors.set_throttle_mix_min();
}
}
#endif
}
