AntennaTracker/system.cpp

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