Line data    Source code

       1             : #include "Copter.h"
       2             : 
       3             : /*
       4             :   compass/motor interference calibration
       5             :  */
       6             : 
       7             : // setup_compassmot - sets compass's motor interference parameters
       8           1 : MAV_RESULT Copter::mavlink_compassmot(const GCS_MAVLINK &gcs_chan)
       9             : {
      10             : #if FRAME_CONFIG == HELI_FRAME
      11             :     // compassmot not implemented for tradheli
      12           0 :     return MAV_RESULT_UNSUPPORTED;
      13             : #else
      14             :     int8_t   comp_type;                 // throttle or current based compensation
      15           1 :     Vector3f compass_base[COMPASS_MAX_INSTANCES];           // compass vector when throttle is zero
      16           1 :     Vector3f motor_impact[COMPASS_MAX_INSTANCES];           // impact of motors on compass vector
      17           1 :     Vector3f motor_impact_scaled[COMPASS_MAX_INSTANCES];    // impact of motors on compass vector scaled with throttle
      18           1 :     Vector3f motor_compensation[COMPASS_MAX_INSTANCES];     // final compensation to be stored to eeprom
      19             :     float    throttle_pct;              // throttle as a percentage 0.0 ~ 1.0
      20           1 :     float    throttle_pct_max = 0.0f;   // maximum throttle reached (as a percentage 0~1.0)
      21           1 :     float    current_amps_max = 0.0f;   // maximum current reached
      22           1 :     float    interference_pct[COMPASS_MAX_INSTANCES]{};       // interference as a percentage of total mag field (for reporting purposes only)
      23             :     uint32_t last_run_time_ms;
      24             :     uint32_t last_send_time_ms;
      25           1 :     bool     updated = false;           // have we updated the compensation vector at least once
      26           1 :     uint8_t  command_ack_start = command_ack_counter;
      27             : 
      28             :     // exit immediately if we are already in compassmot
      29           1 :     if (ap.compass_mot) {
      30             :         // ignore restart messages
      31           0 :         return MAV_RESULT_TEMPORARILY_REJECTED;
      32             :     } else {
      33           1 :         ap.compass_mot = true;
      34             :     }
      35             : 
      36             :     // check compass is enabled
      37           1 :     if (!AP::compass().available()) {
      38           0 :         gcs_chan.send_text(MAV_SEVERITY_CRITICAL, "Compass disabled");
      39           0 :         ap.compass_mot = false;
      40           0 :         return MAV_RESULT_TEMPORARILY_REJECTED;
      41             :     }
      42             : 
      43             :     // check compass health
      44           1 :     compass.read();
      45           4 :     for (uint8_t i=0; i<compass.get_count(); i++) {
      46           3 :         if (!compass.healthy(i)) {
      47           0 :             gcs_chan.send_text(MAV_SEVERITY_CRITICAL, "Check compass");
      48           0 :             ap.compass_mot = false;
      49           0 :             return MAV_RESULT_TEMPORARILY_REJECTED;
      50             :         }
      51             :     }
      52             : 
      53             :     // check if radio is calibrated
      54           1 :     if (!arming.rc_calibration_checks(true)) {
      55           0 :         gcs_chan.send_text(MAV_SEVERITY_CRITICAL, "RC not calibrated");
      56           0 :         ap.compass_mot = false;
      57           0 :         return MAV_RESULT_TEMPORARILY_REJECTED;
      58             :     }
      59             : 
      60             :     // check throttle is at zero
      61           1 :     read_radio();
      62           1 :     if (channel_throttle->get_control_in() != 0) {
      63           0 :         gcs_chan.send_text(MAV_SEVERITY_CRITICAL, "Throttle not zero");
      64           0 :         ap.compass_mot = false;
      65           0 :         return MAV_RESULT_TEMPORARILY_REJECTED;
      66             :     }
      67             : 
      68             :     // check we are landed
      69           1 :     if (!ap.land_complete) {
      70           0 :         gcs_chan.send_text(MAV_SEVERITY_CRITICAL, "Not landed");
      71           0 :         ap.compass_mot = false;
      72           0 :         return MAV_RESULT_TEMPORARILY_REJECTED;
      73             :     }
      74             : 
      75             :     // disable cpu failsafe
      76           1 :     failsafe_disable();
      77             : 
      78             :     float current;
      79             : 
      80             :     // default compensation type to use current if possible
      81           1 :     if (battery.current_amps(current)) {
      82           1 :         comp_type = AP_COMPASS_MOT_COMP_CURRENT;
      83             :     } else {
      84           0 :         comp_type = AP_COMPASS_MOT_COMP_THROTTLE;
      85             :     }
      86             : 
      87             :     // send back initial ACK
      88           1 :     mavlink_msg_command_ack_send(gcs_chan.get_chan(), MAV_CMD_PREFLIGHT_CALIBRATION,0,
      89             :                                  0, 0, 0, 0);
      90             : 
      91             :     // flash leds
      92           1 :     AP_Notify::flags.esc_calibration = true;
      93             : 
      94             :     // warn user we are starting calibration
      95           1 :     gcs_chan.send_text(MAV_SEVERITY_INFO, "Starting calibration");
      96             : 
      97             :     // inform what type of compensation we are attempting
      98           1 :     if (comp_type == AP_COMPASS_MOT_COMP_CURRENT) {
      99           1 :         gcs_chan.send_text(MAV_SEVERITY_INFO, "Current");
     100             :     } else {
     101           0 :         gcs_chan.send_text(MAV_SEVERITY_INFO, "Throttle");
     102             :     }
     103             : 
     104             :     // disable throttle failsafe
     105           1 :     g.failsafe_throttle.set(FS_THR_Action::DISABLED);
     106             : 
     107             :     // disable motor compensation
     108           1 :     compass.motor_compensation_type(AP_COMPASS_MOT_COMP_DISABLED);
     109           4 :     for (uint8_t i=0; i<compass.get_count(); i++) {
     110           3 :         compass.set_motor_compensation(i, Vector3f{0,0,0});
     111             :     }
     112             : 
     113             :     // get initial compass readings
     114           1 :     compass.read();
     115             : 
     116             :     // store initial x,y,z compass values
     117             :     // initialise interference percentage
     118           4 :     for (uint8_t i=0; i<compass.get_count(); i++) {
     119           3 :         compass_base[i] = compass.get_field(i);
     120           3 :         interference_pct[i] = 0.0f;
     121             :     }
     122             : 
     123           1 :     EXPECT_DELAY_MS(5000);
     124             : 
     125             :     // enable motors and pass through throttle
     126           1 :     motors->output_min();  // output lowest possible value to motors
     127           1 :     motors->armed(true);
     128           1 :     hal.util->set_soft_armed(true);
     129             : 
     130             :     // initialise run time
     131           1 :     last_run_time_ms = millis();
     132           1 :     last_send_time_ms = millis();
     133             : 
     134             :     // main run while there is no user input and the compass is healthy
     135       11319 :     while (command_ack_start == command_ack_counter && compass.healthy() && motors->armed()) {
     136       11318 :         EXPECT_DELAY_MS(5000);
     137             : 
     138             :         // 50hz loop
     139       11318 :         if (millis() - last_run_time_ms < 20) {
     140        9055 :             hal.scheduler->delay(5);
     141        9055 :             continue;
     142             :         }
     143        2263 :         last_run_time_ms = millis();
     144             : 
     145             :         // read radio input
     146        2263 :         read_radio();
     147             : 
     148             :         // pass through throttle to motors
     149        2263 :         auto &srv = AP::srv();
     150        2263 :         srv.cork();
     151        2263 :         motors->set_throttle_passthrough_for_esc_calibration(channel_throttle->get_control_in() * 0.001f);
     152        2263 :         srv.push();
     153             : 
     154             :         // read some compass values
     155        2263 :         compass.read();
     156             : 
     157             :         // read current
     158        2263 :         battery.read();
     159             : 
     160             :         // calculate scaling for throttle
     161        2263 :         throttle_pct = (float)channel_throttle->get_control_in() * 0.001f;
     162        2263 :         throttle_pct = constrain_float(throttle_pct,0.0f,1.0f);
     163             : 
     164             :         // record maximum throttle
     165        2263 :         throttle_pct_max = MAX(throttle_pct_max, throttle_pct);
     166             : 
     167        2263 :         if (!battery.current_amps(current)) {
     168           0 :             current = 0;
     169             :         }
     170        2263 :         current_amps_max = MAX(current_amps_max, current);
     171             : 
     172             :         // if throttle is near zero, update base x,y,z values
     173        2263 :         if (!is_positive(throttle_pct)) {
     174        1032 :             for (uint8_t i=0; i<compass.get_count(); i++) {
     175         774 :                 compass_base[i] = compass_base[i] * 0.99f + compass.get_field(i) * 0.01f;
     176             :             }
     177             :         } else {
     178             : 
     179             :             // calculate diff from compass base and scale with throttle
     180        8020 :             for (uint8_t i=0; i<compass.get_count(); i++) {
     181        6015 :                 motor_impact[i] = compass.get_field(i) - compass_base[i];
     182             :             }
     183             : 
     184             :             // throttle based compensation
     185        2005 :             if (comp_type == AP_COMPASS_MOT_COMP_THROTTLE) {
     186             :                 // for each compass
     187           0 :                 for (uint8_t i=0; i<compass.get_count(); i++) {
     188             :                     // scale by throttle
     189           0 :                     motor_impact_scaled[i] = motor_impact[i] / throttle_pct;
     190             :                     // adjust the motor compensation to negate the impact
     191           0 :                     motor_compensation[i] = motor_compensation[i] * 0.99f - motor_impact_scaled[i] * 0.01f;
     192             :                 }
     193           0 :                 updated = true;
     194             :             } else {
     195             :                 // for each compass
     196        8020 :                 for (uint8_t i=0; i<compass.get_count(); i++) {
     197             :                     // adjust the motor compensation to negate the impact if drawing over 3amps
     198        6015 :                     if (current >= 3.0f) {
     199        4782 :                         motor_impact_scaled[i] = motor_impact[i] / current;
     200        4782 :                         motor_compensation[i] = motor_compensation[i] * 0.99f - motor_impact_scaled[i] * 0.01f;
     201        4782 :                         updated = true;
     202             :                     }
     203             :                 }
     204             :             }
     205             : 
     206             :             // calculate interference percentage at full throttle as % of total mag field
     207        2005 :             if (comp_type == AP_COMPASS_MOT_COMP_THROTTLE) {
     208           0 :                 for (uint8_t i=0; i<compass.get_count(); i++) {
     209             :                     // interference is impact@fullthrottle / mag field * 100
     210           0 :                     interference_pct[i] = motor_compensation[i].length() / (float)arming.compass_magfield_expected() * 100.0f;
     211             :                 }
     212             :             } else {
     213        8020 :                 for (uint8_t i=0; i<compass.get_count(); i++) {
     214             :                     // interference is impact/amp * (max current seen / max throttle seen) / mag field * 100
     215        6015 :                     interference_pct[i] = motor_compensation[i].length() * (current_amps_max/throttle_pct_max) / (float)arming.compass_magfield_expected() * 100.0f;
     216             :                 }
     217             :             }
     218             :         }
     219             : 
     220        2263 :         if (AP_HAL::millis() - last_send_time_ms > 500) {
     221          88 :             last_send_time_ms = AP_HAL::millis();
     222          88 :             mavlink_msg_compassmot_status_send(gcs_chan.get_chan(),
     223          88 :                                                channel_throttle->get_control_in(),
     224             :                                                current,
     225          88 :                                                interference_pct[0],
     226             :                                                motor_compensation[0].x,
     227             :                                                motor_compensation[0].y,
     228             :                                                motor_compensation[0].z);
     229             : #if HAL_WITH_ESC_TELEM
     230             :             // send ESC telemetry to monitor ESC and motor temperatures
     231          88 :             AP::esc_telem().send_esc_telemetry_mavlink(gcs_chan.get_chan());
     232             : #endif
     233             : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
     234             :             // a lot of autotest timeouts are based on receiving system time
     235          88 :             gcs_chan.send_system_time();
     236             :             // autotesting of compassmot wants to see RC channels message
     237          88 :             gcs_chan.send_rc_channels();
     238             : #endif
     239             :         }
     240       11318 :     }
     241             : 
     242             :     // stop motors
     243           1 :     motors->output_min();
     244           1 :     motors->armed(false);
     245           1 :     hal.util->set_soft_armed(false);
     246             : 
     247             :     // set and save motor compensation
     248           1 :     if (updated) {
     249           1 :         compass.motor_compensation_type(comp_type);
     250           4 :         for (uint8_t i=0; i<compass.get_count(); i++) {
     251           3 :             compass.set_motor_compensation(i, motor_compensation[i]);
     252             :         }
     253           1 :         compass.save_motor_compensation();
     254             :         // display success message
     255           1 :         gcs_chan.send_text(MAV_SEVERITY_INFO, "Calibration successful");
     256             :     } else {
     257             :         // compensation vector never updated, report failure
     258           0 :         gcs_chan.send_text(MAV_SEVERITY_NOTICE, "Failed");
     259           0 :         compass.motor_compensation_type(AP_COMPASS_MOT_COMP_DISABLED);
     260             :     }
     261             : 
     262             :     // turn off notify leds
     263           1 :     AP_Notify::flags.esc_calibration = false;
     264             : 
     265             :     // re-enable cpu failsafe
     266           1 :     failsafe_enable();
     267             : 
     268             :     // re-enable failsafes
     269           1 :     g.failsafe_throttle.load();
     270             : 
     271             :     // flag we have completed
     272           1 :     ap.compass_mot = false;
     273             : 
     274           1 :     return MAV_RESULT_ACCEPTED;
     275             : #endif  // FRAME_CONFIG != HELI_FRAME
     276           1 : }