Line data    Source code

       1             : #include "Tracker.h"
       2             : 
       3             : // mission storage
       4             : static const StorageAccess wp_storage(StorageManager::StorageMission);
       5             : 
       6          33 : void Tracker::init_ardupilot()
       7             : {
       8             :     // initialise notify
       9          33 :     notify.init();
      10          33 :     AP_Notify::flags.pre_arm_check = true;
      11          33 :     AP_Notify::flags.pre_arm_gps_check = true;
      12             : 
      13             :     // initialise battery
      14          33 :     battery.init();
      15             : 
      16             :     // init baro before we start the GCS, so that the CLI baro test works
      17          33 :     barometer.set_log_baro_bit(MASK_LOG_IMU);
      18          33 :     barometer.init();
      19             : 
      20             :     // setup telem slots with serial ports
      21          33 :     gcs().setup_uarts();
      22             :     // update_send so that if the first packet we receive happens to
      23             :     // be an arm message we don't trigger an internal error when we
      24             :     // try to initialise stream rates in the main loop.
      25          33 :     gcs().update_send();
      26             : 
      27             :     // initialise compass
      28          33 :     AP::compass().set_log_bit(MASK_LOG_COMPASS);
      29          33 :     AP::compass().init();
      30             : 
      31             :     // GPS Initialization
      32          33 :     gps.set_log_gps_bit(MASK_LOG_GPS);
      33          33 :     gps.init();
      34             : 
      35          33 :     ahrs.init();
      36          33 :     ahrs.set_fly_forward(false);
      37             : 
      38          33 :     ins.init(scheduler.get_loop_rate_hz());
      39          33 :     ahrs.reset();
      40             : 
      41          33 :     barometer.calibrate();
      42             : 
      43             : #if HAL_LOGGING_ENABLED
      44             :     // initialise AP_Logger library
      45          32 :     logger.setVehicle_Startup_Writer(FUNCTOR_BIND(&tracker, &Tracker::Log_Write_Vehicle_Startup_Messages, void));
      46             : #endif
      47             : 
      48             :     // initialise rc channels including setting mode
      49          32 :     rc().init();
      50             : 
      51             :     // initialise servos
      52          32 :     init_servos();
      53             : 
      54             :     // use given start positions - useful for indoor testing, and
      55             :     // while waiting for GPS lock
      56             :     // sanity check location
      57          32 :     if (fabsf(g.start_latitude) <= 90.0f && fabsf(g.start_longitude) <= 180.0f) {
      58          32 :         current_loc.lat = g.start_latitude * 1.0e7f;
      59          32 :         current_loc.lng = g.start_longitude * 1.0e7f;
      60             :     } else {
      61           0 :         gcs().send_text(MAV_SEVERITY_NOTICE, "Ignoring invalid START_LATITUDE or START_LONGITUDE parameter");
      62             :     }
      63             : 
      64             :     // see if EEPROM has a default location as well
      65          32 :     if (current_loc.lat == 0 && current_loc.lng == 0) {
      66          32 :         get_home_eeprom(current_loc);
      67             :     }
      68             : 
      69          32 :     hal.scheduler->delay(1000); // Why????
      70             : 
      71          32 :     Mode *newmode = mode_from_mode_num((Mode::Number)g.initial_mode.get());
      72          32 :     if (newmode == nullptr) {
      73           0 :         newmode = &mode_manual;
      74             :     }
      75          32 :     set_mode(*newmode, ModeReason::STARTUP);
      76             : 
      77          32 :     if (g.startup_delay > 0) {
      78             :         // arm servos with trim value to allow them to start up (required
      79             :         // for some servos)
      80           0 :         prepare_servos();
      81             :     }
      82          32 : }
      83             : 
      84             : /*
      85             :   fetch HOME from EEPROM
      86             : */
      87          32 : bool Tracker::get_home_eeprom(Location &loc) const
      88             : {
      89             :     // Find out proper location in memory by using the start_byte position + the index
      90             :     // --------------------------------------------------------------------------------
      91          32 :     if (g.command_total.get() == 0) {
      92           7 :         return false;
      93             :     }
      94             : 
      95             :     // read WP position
      96          25 :     loc = {
      97          25 :         int32_t(wp_storage.read_uint32(5)),
      98          25 :         int32_t(wp_storage.read_uint32(9)),
      99          25 :         int32_t(wp_storage.read_uint32(1)),
     100             :         Location::AltFrame::ABSOLUTE
     101             :     };
     102             : 
     103          25 :     return true;
     104             : }
     105             : 
     106         305 : bool Tracker::set_home_eeprom(const Location &temp)
     107             : {
     108         305 :     wp_storage.write_byte(0, 0);
     109         305 :     wp_storage.write_uint32(1, temp.alt);
     110         305 :     wp_storage.write_uint32(5, temp.lat);
     111         305 :     wp_storage.write_uint32(9, temp.lng);
     112             : 
     113             :     // Now have a home location in EEPROM
     114         305 :     g.command_total.set_and_save(1); // At most 1 entry for HOME
     115         305 :     return true;
     116             : }
     117             : 
     118           2 : bool Tracker::set_home_to_current_location(bool lock)
     119             : {
     120           2 :     return set_home(AP::gps().location(), lock);
     121             : }
     122             : 
     123         305 : bool Tracker::set_home(const Location &temp, bool lock)
     124             : {
     125             :     // check EKF origin has been set
     126         305 :     Location ekf_origin;
     127         305 :     if (ahrs.get_origin(ekf_origin)) {
     128          25 :         if (!ahrs.set_home(temp)) {
     129           0 :             return false;
     130             :         }
     131             :     }
     132             : 
     133         305 :     if (!set_home_eeprom(temp)) {
     134           0 :         return false;
     135             :     }
     136             : 
     137         305 :     current_loc = temp;
     138             : 
     139         305 :     return true;
     140             : }
     141             : 
     142          43 : void Tracker::arm_servos()
     143             : {
     144          43 :     hal.util->set_soft_armed(true);
     145             : #if HAL_LOGGING_ENABLED
     146          43 :     logger.set_vehicle_armed(true);
     147             : #endif
     148          43 : }
     149             : 
     150          32 : void Tracker::disarm_servos()
     151             : {
     152          32 :     hal.util->set_soft_armed(false);
     153             : #if HAL_LOGGING_ENABLED
     154          32 :     logger.set_vehicle_armed(false);
     155             : #endif
     156          32 : }
     157             : 
     158             : /*
     159             :   setup servos to trim value after initialising
     160             :  */
     161           0 : void Tracker::prepare_servos()
     162             : {
     163           0 :     start_time_ms = AP_HAL::millis();
     164           0 :     SRV_Channels::set_output_limit(SRV_Channel::k_tracker_yaw, SRV_Channel::Limit::TRIM);
     165           0 :     SRV_Channels::set_output_limit(SRV_Channel::k_tracker_pitch, SRV_Channel::Limit::TRIM);
     166           0 :     SRV_Channels::calc_pwm();
     167           0 :     SRV_Channels::output_ch_all();
     168           0 : }
     169             : 
     170          68 : void Tracker::set_mode(Mode &newmode, const ModeReason reason)
     171             : {
     172          68 :     control_mode_reason = reason;
     173             : 
     174          68 :     if (mode == &newmode) {
     175             :         // don't switch modes if we are already in the correct mode.
     176          24 :         return;
     177             :     }
     178          44 :     mode = &newmode;
     179             : 
     180          44 :     if (mode->requires_armed_servos()) {
     181          43 :         arm_servos();
     182             :     } else {
     183           1 :         disarm_servos();
     184             :     }
     185             : 
     186             : #if HAL_LOGGING_ENABLED
     187             :         // log mode change
     188          44 :         logger.Write_Mode((uint8_t)mode->number(), reason);
     189             : #endif
     190          44 :     gcs().send_message(MSG_HEARTBEAT);
     191             : 
     192          44 :     nav_status.bearing = ahrs.get_yaw_deg();
     193             : }
     194             : 
     195          26 : bool Tracker::set_mode(const uint8_t new_mode, const ModeReason reason)
     196             : {
     197          26 :     Mode *fred = nullptr;
     198          26 :     switch ((Mode::Number)new_mode) {
     199           0 :     case Mode::Number::INITIALISING:
     200           0 :         return false;
     201          20 :     case Mode::Number::AUTO:
     202          20 :         fred = &mode_auto;
     203          20 :         break;
     204           4 :     case Mode::Number::MANUAL:
     205           4 :         fred = &mode_manual;
     206           4 :         break;
     207           1 :     case Mode::Number::SCAN:
     208           1 :         fred = &mode_scan;
     209           1 :         break;
     210           0 :     case Mode::Number::SERVOTEST:
     211           0 :         fred = &mode_servotest;
     212           0 :         break;
     213           0 :     case Mode::Number::STOP:
     214           0 :         fred = &mode_stop;
     215           0 :         break;
     216           1 :     case Mode::Number::GUIDED:
     217           1 :         fred = &mode_guided;
     218           1 :         break;
     219             :     }
     220          26 :     if (fred == nullptr) {
     221           0 :         return false;
     222             :     }
     223          26 :     set_mode(*fred, reason);
     224          26 :     return true;
     225             : }
     226             : 
     227             : #if HAL_LOGGING_ENABLED
     228             : /*
     229             :   should we log a message type now?
     230             :  */
     231       69176 : bool Tracker::should_log(uint32_t mask)
     232             : {
     233       69176 :     if (!logger.should_log(mask)) {
     234           0 :         return false;
     235             :     }
     236       69176 :     return true;
     237             : }
     238             : #endif
     239             : 
     240             : #include <AP_AdvancedFailsafe/AP_AdvancedFailsafe.h>
     241             : #include <AP_Avoidance/AP_Avoidance.h>
     242             : #include <AP_ADSB/AP_ADSB.h>
     243             : 
     244             : #if AP_ADVANCEDFAILSAFE_ENABLED
     245             : // dummy method to avoid linking AFS
     246           0 : bool AP_AdvancedFailsafe::gcs_terminate(bool should_terminate, const char *reason) {return false;}
     247           3 : AP_AdvancedFailsafe *AP::advancedfailsafe() { return nullptr; }
     248             : #endif  // AP_ADVANCEDFAILSAFE_ENABLED
     249             : #if AP_ADSB_AVOIDANCE_ENABLED
     250             : // dummy method to avoid linking AP_Avoidance
     251           0 : AP_Avoidance *AP::ap_avoidance() { return nullptr; }
     252             : #endif