diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 6408a4db84b25..6370ee85b8704 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -760,7 +760,8 @@ static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = { { run_nav_updates, 8, 80 }, { update_thr_average, 40, 10 }, { three_hz_loop, 133, 9 }, - { compass_accumulate, 8, 42 }, + { compass_accumulate, 4, 42 }, + { compass_cal_update, 4, 40 }, { barometer_accumulate, 8, 25 }, #if FRAME_CONFIG == HELI_FRAME { check_dynamic_flight, 8, 10 }, diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde index b03b829c2c5b7..a973825c5cf87 100644 --- a/ArduCopter/GCS_Mavlink.pde +++ b/ArduCopter/GCS_Mavlink.pde @@ -636,6 +636,14 @@ bool GCS_MAVLINK::try_send_message(enum ap_message id) case MSG_BATTERY2: break; // just here to prevent a warning + + case MSG_MAG_CAL_PROGRESS: + compass.send_mag_cal_progress(chan); + break; + + case MSG_MAG_CAL_REPORT: + compass.send_mag_cal_report(chan); + break; } return true; @@ -854,6 +862,8 @@ GCS_MAVLINK::data_stream_send(void) send_message(MSG_MOUNT_STATUS); send_message(MSG_OPTICAL_FLOW); send_message(MSG_GIMBAL_REPORT); + send_message(MSG_MAG_CAL_REPORT); + send_message(MSG_MAG_CAL_PROGRESS); } } @@ -1305,6 +1315,70 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) break; } + case MAV_CMD_DO_START_MAG_CAL: { + result = MAV_RESULT_ACCEPTED; + if(motors.armed() || packet.param1 < 0 || packet.param1 > 255) { + result = MAV_RESULT_FAILED; + break; + } + + uint8_t mag_mask = packet.param1; + bool retry = packet.param2; + bool autosave = packet.param3; + float delay = packet.param4; + + if (mag_mask == 0) { // 0 means all + compass.start_calibration_all(retry, autosave, delay); + break; + } + + if(!compass.start_calibration_mask(mag_mask, retry, autosave, delay)) { + result = MAV_RESULT_FAILED; + } + + break; + } + + case MAV_CMD_DO_ACCEPT_MAG_CAL: { + result = MAV_RESULT_ACCEPTED; + if(packet.param1 < 0 || packet.param1 > 255) { + result = MAV_RESULT_FAILED; + break; + } + + uint8_t mag_mask = packet.param1; + + if (mag_mask == 0) { // 0 means all + if(!compass.accept_calibration_all()) { + result = MAV_RESULT_FAILED; + } + break; + } + + if(!compass.accept_calibration_mask(mag_mask)) { + result = MAV_RESULT_FAILED; + } + break; + } + + case MAV_CMD_DO_CANCEL_MAG_CAL: { + result = MAV_RESULT_ACCEPTED; + if(packet.param1 < 0 || packet.param1 > 255) { + result = MAV_RESULT_FAILED; + break; + } + + uint8_t mag_mask = packet.param1; + + if (mag_mask == 0) { // 0 means all + compass.cancel_calibration_all(); + break; + } + + compass.cancel_calibration_mask(mag_mask); + break; + } + default: result = MAV_RESULT_UNSUPPORTED; break; diff --git a/ArduCopter/compass_cal.pde b/ArduCopter/compass_cal.pde new file mode 100644 index 0000000000000..4cc87ad610621 --- /dev/null +++ b/ArduCopter/compass_cal.pde @@ -0,0 +1,24 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +#define COMPASS_CAL_STICK_DELAY 2.0f // 2 seconds +#define COMPASS_CAL_DELAY 5.0f + +static void compass_cal_update() { + compass.compass_cal_update(); + + if (compass.is_calibrating()) { + return; + } + + bool stick_gesture_detected = !motors.armed() && g.rc_4.control_in > 4000 && g.rc_3.control_in > 900; + static uint32_t stick_gesture_begin = 0; + uint32_t tnow = millis(); + + if(!stick_gesture_detected) { + stick_gesture_begin = tnow; + } else { + if(tnow-stick_gesture_begin > 1000*COMPASS_CAL_STICK_DELAY) { + compass.start_calibration_all(true,true,COMPASS_CAL_DELAY); + } + } +} diff --git a/ArduCopter/motors.pde b/ArduCopter/motors.pde index 083d6ba7083f7..426aa547d440b 100644 --- a/ArduCopter/motors.pde +++ b/ArduCopter/motors.pde @@ -584,6 +584,13 @@ static bool arm_checks(bool display_failure, bool arming_from_gcs) start_logging(); #endif + if(compass.is_calibrating()) { + if (display_failure) { + gcs_send_text_P(SEVERITY_HIGH,PSTR("Arm: Compass calibration running")); + } + return false; + } + // always check if the current mode allows arming if (!mode_allows_arming(control_mode, arming_from_gcs)) { if (display_failure) { diff --git a/libraries/AP_Compass/AP_Compass_PX4.cpp b/libraries/AP_Compass/AP_Compass_PX4.cpp index 54d5a89122021..4807149d281b5 100644 --- a/libraries/AP_Compass/AP_Compass_PX4.cpp +++ b/libraries/AP_Compass/AP_Compass_PX4.cpp @@ -99,26 +99,16 @@ bool AP_Compass_PX4::read(void) for (uint8_t i=0; i<_num_instances; i++) { // avoid division by zero if we haven't received any mag reports - if (_count[i] == 0) continue; + if (_count[i] == 0) { + continue; + } + // take average of field measurements since last read() _sum[i] /= _count[i]; - _sum[i] *= 1000; - - // apply default board orientation for this compass type. This is - // a noop on most boards - _sum[i].rotate(MAG_BOARD_ORIENTATION); - if (_external[i]) { - // add user selectable orientation - _sum[i].rotate((enum Rotation)_orientation[i].get()); - } else { - // add in board orientation from AHRS - _sum[i].rotate(_board_orientation); - } - + // publish to _field _field[i] = _sum[i]; - apply_corrections(_field[i],i); - + _sum[i].zero(); _count[i] = 0; } @@ -132,12 +122,35 @@ void AP_Compass_PX4::accumulate(void) { struct mag_report mag_report; for (uint8_t i=0; i<_num_instances; i++) { + bool received_sample = false; + // get latest report while (::read(_mag_fd[i], &mag_report, sizeof(mag_report)) == sizeof(mag_report) && mag_report.timestamp != _last_timestamp[i]) { - _sum[i] += Vector3f(mag_report.x, mag_report.y, mag_report.z); - _count[i]++; _last_timestamp[i] = mag_report.timestamp; + received_sample = true; + } + + if(!received_sample) continue; + + _raw_field[i] = Vector3f(mag_report.x, mag_report.y, mag_report.z); + _raw_field[i] *= 1000; + + // apply default board orientation for this compass type. This is + // a noop on most boards + _raw_field[i].rotate(MAG_BOARD_ORIENTATION); + + if (_external[i]) { + // add user selectable orientation + _raw_field[i].rotate((enum Rotation)_orientation[i].get()); + } else { + // add in board orientation from AHRS + _raw_field[i].rotate(_board_orientation); } + + apply_corrections(_raw_field[i],i); + + _sum[i] += _raw_field[i]; + _count[i]++; } } diff --git a/libraries/AP_Compass/AP_Compass_PX4.h b/libraries/AP_Compass/AP_Compass_PX4.h index a01602c132eff..f12f61a5a4eff 100644 --- a/libraries/AP_Compass/AP_Compass_PX4.h +++ b/libraries/AP_Compass/AP_Compass_PX4.h @@ -22,12 +22,18 @@ class AP_Compass_PX4 : public Compass // return the primary compass uint8_t get_primary(void) const; + bool supports_raw_field() const { return true; } + void get_raw_field(uint8_t i, Vector3f &field) const { field = _raw_field[i]; } + uint32_t last_raw_update_us(uint8_t i) const { return _last_timestamp[i]; } + private: uint8_t _num_instances; int _mag_fd[COMPASS_MAX_INSTANCES]; Vector3f _sum[COMPASS_MAX_INSTANCES]; uint32_t _count[COMPASS_MAX_INSTANCES]; uint64_t _last_timestamp[COMPASS_MAX_INSTANCES]; + + Vector3f _raw_field[COMPASS_MAX_INSTANCES]; }; #endif // AP_Compass_PX4_H diff --git a/libraries/AP_Compass/Compass.cpp b/libraries/AP_Compass/Compass.cpp index 5db2c854533f6..62c1d5a872132 100644 --- a/libraries/AP_Compass/Compass.cpp +++ b/libraries/AP_Compass/Compass.cpp @@ -1,6 +1,7 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- #include #include "Compass.h" +#include const AP_Param::GroupInfo Compass::var_info[] PROGMEM = { // index 0 was used for the old orientation matrix @@ -257,6 +258,106 @@ const AP_Param::GroupInfo Compass::var_info[] PROGMEM = { AP_GROUPINFO("EXTERN3",23, Compass, _external[2], 0), #endif + // @Param: DIA_X + // @DisplayName: Compass soft-iron diagonal X component + // @Description: DIA_X in the compass soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]] + // @User: Advanced + + // @Param: DIA_Y + // @DisplayName: Compass soft-iron diagonal Y component + // @Description: DIA_Y in the compass soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]] + // @User: Advanced + + // @Param: DIA_Z + // @DisplayName: Compass soft-iron diagonal Z component + // @Description: DIA_Z in the compass soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]] + // @User: Advanced + AP_GROUPINFO("DIA", 24, Compass, _diagonals[0], 0), + + // @Param: ODI_X + // @DisplayName: Compass soft-iron off-diagonal X component + // @Description: ODI_X in the compass soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]] + // @User: Advanced + + // @Param: ODI_Y + // @DisplayName: Compass soft-iron off-diagonal Y component + // @Description: ODI_Y in the compass soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]] + // @User: Advanced + + // @Param: ODI_Z + // @DisplayName: Compass soft-iron off-diagonal Z component + // @Description: ODI_Z in the compass soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]] + // @User: Advanced + AP_GROUPINFO("ODI", 25, Compass, _offdiagonals[0], 0), + +#if COMPASS_MAX_INSTANCES > 1 + // @Param: DIA2_X + // @DisplayName: Compass2 soft-iron diagonal X component + // @Description: DIA_X in the compass2 soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]] + // @User: Advanced + + // @Param: DIA2_Y + // @DisplayName: Compass2 soft-iron diagonal Y component + // @Description: DIA_Y in the compass2 soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]] + // @User: Advanced + + // @Param: DIA2_Z + // @DisplayName: Compass2 soft-iron diagonal Z component + // @Description: DIA_Z in the compass2 soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]] + // @User: Advanced + AP_GROUPINFO("DIA2", 26, Compass, _diagonals[1], 0), + + // @Param: ODI2_X + // @DisplayName: Compass2 soft-iron off-diagonal X component + // @Description: ODI_X in the compass2 soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]] + // @User: Advanced + + // @Param: ODI2_Y + // @DisplayName: Compass2 soft-iron off-diagonal Y component + // @Description: ODI_Y in the compass2 soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]] + // @User: Advanced + + // @Param: ODI2_Z + // @DisplayName: Compass2 soft-iron off-diagonal Z component + // @Description: ODI_Z in the compass2 soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]] + // @User: Advanced + AP_GROUPINFO("ODI2", 27, Compass, _offdiagonals[1], 0), +#endif + +#if COMPASS_MAX_INSTANCES > 2 + // @Param: DIA3_X + // @DisplayName: Compass3 soft-iron diagonal X component + // @Description: DIA_X in the compass3 soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]] + // @User: Advanced + + // @Param: DIA3_Y + // @DisplayName: Compass3 soft-iron diagonal Y component + // @Description: DIA_Y in the compass3 soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]] + // @User: Advanced + + // @Param: DIA3_Z + // @DisplayName: Compass3 soft-iron diagonal Z component + // @Description: DIA_Z in the compass3 soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]] + // @User: Advanced + AP_GROUPINFO("DIA3", 28, Compass, _diagonals[2], 0), + + // @Param: ODI3_X + // @DisplayName: Compass3 soft-iron off-diagonal X component + // @Description: ODI_X in the compass3 soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]] + // @User: Advanced + + // @Param: ODI3_Y + // @DisplayName: Compass3 soft-iron off-diagonal Y component + // @Description: ODI_Y in the compass3 soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]] + // @User: Advanced + + // @Param: ODI3_Z + // @DisplayName: Compass3 soft-iron off-diagonal Z component + // @Description: ODI_Z in the compass3 soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]] + // @User: Advanced + AP_GROUPINFO("ODI3", 29, Compass, _offdiagonals[2], 0), +#endif + AP_GROUPEND }; @@ -289,6 +390,234 @@ Compass::init() return true; } +void +Compass::compass_cal_update() +{ + AP_Notify::flags.compass_cal_running = 0; + + for(uint8_t i=0; i 0.5f) { + AP_Notify::events.initiated_compass_cal = 1; + } + _calibrator[i].start(retry, autosave, delay); + return true; + } else { + return false; + } +} + +bool +Compass::start_calibration_mask(uint8_t mask, bool retry, bool autosave, float delay) +{ + for(uint8_t i=0; i 1 + _dev_id[i].save(); +#endif + if(!is_calibrating()) { + AP_Notify::events.compass_cal_saved = 1; + } + return true; + } else { + return false; + } +} + +bool +Compass::accept_calibration_mask(uint8_t mask) +{ + for(uint8_t i=0; i #include #include // ArduPilot Mega Declination Helper Library +#include +#include // compass product id #define AP_COMPASS_TYPE_UNKNOWN 0x00 @@ -78,12 +80,32 @@ class Compass /// virtual bool read(void) = 0; - - /// use spare CPU cycles to accumulate values from the compass if /// possible virtual void accumulate(void) = 0; + void compass_cal_update(); + + bool start_calibration(uint8_t i, bool retry=false, bool autosave=false, float delay_sec=0.0f); + bool start_calibration_all(bool retry=false, bool autosave=false, float delay_sec=0.0f); + bool start_calibration_mask(uint8_t mask, bool retry=false, bool autosave=false, float delay_sec=0.0f); + + void cancel_calibration(uint8_t i); + void cancel_calibration_all(); + void cancel_calibration_mask(uint8_t mask); + + bool accept_calibration(uint8_t i); + bool accept_calibration_all(); + bool accept_calibration_mask(uint8_t mask); + + uint8_t get_cal_mask() const; + bool is_calibrating() const; + + uint8_t get_healthy_mask() const; + + void send_mag_cal_progress(mavlink_channel_t chan); + void send_mag_cal_report(mavlink_channel_t chan); + /// Calculate the tilt-compensated heading_ variables. /// /// @param dcm_matrix The current orientation rotation matrix @@ -106,6 +128,9 @@ class Compass /// void set_and_save_offsets(uint8_t i, const Vector3f &offsets); + void set_and_save_diagonals(uint8_t i, const Vector3f &diagonals); + void set_and_save_offdiagonals(uint8_t i, const Vector3f &diagonals); + /// Saves the current offset x/y/z values for one or all compasses /// /// @param i compass instance @@ -123,6 +148,13 @@ class Compass const Vector3f &get_field(uint8_t i) const { return _field[i]; } const Vector3f &get_field(void) const { return get_field(get_primary()); } + virtual bool supports_raw_field() const { return false; } + virtual void get_raw_field(uint8_t i, Vector3f &field) const { } + void get_raw_field(Vector3f &field) const { get_raw_field(get_primary(),field); } + + virtual uint32_t last_raw_update_us(uint8_t i) const { return 0; } + uint32_t last_raw_update_us() const { return last_raw_update_us(get_primary()); } + /// Return the health of a compass bool healthy(uint8_t i) const { return _healthy[i]; } bool healthy(void) const { return healthy(get_primary()); } @@ -263,6 +295,8 @@ class Compass AP_Int8 _orientation[COMPASS_MAX_INSTANCES]; AP_Vector3f _offset[COMPASS_MAX_INSTANCES]; + AP_Vector3f _diagonals[COMPASS_MAX_INSTANCES]; + AP_Vector3f _offdiagonals[COMPASS_MAX_INSTANCES]; AP_Float _declination; AP_Int8 _use_for_yaw[COMPASS_MAX_INSTANCES];/// +#include + +extern const AP_HAL::HAL& hal; + +//////////////////////////////////////////////////////////// +///////////////////// PUBLIC INTERFACE ///////////////////// +//////////////////////////////////////////////////////////// + +CompassCalibrator::CompassCalibrator(): +_tolerance(COMPASS_CAL_DEFAULT_TOLERANCE) +{ + clear(); +} + +void CompassCalibrator::clear() { + set_status(COMPASS_CAL_NOT_STARTED); +} + +void CompassCalibrator::start(bool retry, bool autosave, float delay) { + if(running()) { + return; + } + _autosave = autosave; + _attempt = 1; + _retry = retry; + _delay_start_sec = delay; + _start_time_ms = hal.scheduler->millis(); + set_status(COMPASS_CAL_WAITING_TO_START); +} + +void CompassCalibrator::get_calibration(Vector3f &offsets, Vector3f &diagonals, Vector3f &offdiagonals) { + if (_status != COMPASS_CAL_SUCCESS) { + return; + } + + offsets = _params.offset; + diagonals = _params.diag; + offdiagonals = _params.offdiag; +} + +float CompassCalibrator::get_completion_percent() const { + // first sampling step is 1/3rd of the progress bar + // never return more than 99% unless _status is COMPASS_CAL_SUCCESS + switch(_status) { + case COMPASS_CAL_NOT_STARTED: + case COMPASS_CAL_WAITING_TO_START: + return 0.0f; + case COMPASS_CAL_RUNNING_STEP_ONE: + return 33.3f * _samples_collected/COMPASS_CAL_NUM_SAMPLES; + case COMPASS_CAL_RUNNING_STEP_TWO: + return 33.3f + 65.7f*((float)(_samples_collected-_samples_thinned)/(COMPASS_CAL_NUM_SAMPLES-_samples_thinned)); + case COMPASS_CAL_SUCCESS: + return 100.0f; + case COMPASS_CAL_FAILED: + default: + return 0.0f; + }; +} + +bool CompassCalibrator::check_for_timeout() { + uint32_t tnow = hal.scheduler->millis(); + if(running() && tnow - _last_sample_ms > 1000) { + _retry = false; + set_status(COMPASS_CAL_FAILED); + return true; + } + return false; +} + +void CompassCalibrator::new_sample(const Vector3f& sample) { + _last_sample_ms = hal.scheduler->millis(); + + if(_status == COMPASS_CAL_WAITING_TO_START) { + set_status(COMPASS_CAL_RUNNING_STEP_ONE); + } + + if(running() && _samples_collected < COMPASS_CAL_NUM_SAMPLES && accept_sample(sample)) { + set_sample(sample, _samples_collected); + _samples_collected++; + } +} + +void CompassCalibrator::run_fit_chunk() { + if(!fitting()) { + return; + } + if (_fit_step == 0){ //initialise optimiser for sphere fit + optimise.set_fitness_function(&calc_sphere_residual); + optimise.set_jacobian_function(&calc_sphere_jacob); + optimise.set_preprocess_sample_function(&get_sample); + optimise.set_buffer(_sample_buffer, _samples_collected); + optimise.set_init_params(_params.get_sphere_params(), NULL, COMPASS_CAL_NUM_SPHERE_PARAMS, _fitness, _sphere_lambda); + }else if (_fit_step == 15){ //initialise optimiser for ellipsoid fit + optimise.set_fitness_function(&calc_ellipsoid_residual); + optimise.set_jacobian_function(&calc_ellipsoid_jacob); + optimise.set_preprocess_sample_function(&get_sample); + optimise.set_buffer(_sample_buffer, _samples_collected); + optimise.set_init_params(_params.get_ellipsoid_params(), _params.get_sphere_params(), + COMPASS_CAL_NUM_ELLIPSOID_PARAMS, _fitness, _ellipsoid_lambda); + } + + if(_status == COMPASS_CAL_RUNNING_STEP_ONE) { + if(_fit_step < 10) { + optimise.do_levenberg_marquardt_fit(10.0f); + + _fit_step++; + } else { + if(_fitness == _initial_fitness || isnan(_fitness)) { //if true, means that fitness is diverging instead of converging + set_status(COMPASS_CAL_FAILED); + } + + set_status(COMPASS_CAL_RUNNING_STEP_TWO); + } + } else if(_status == COMPASS_CAL_RUNNING_STEP_TWO) { + if(_fit_step < 15) { + optimise.do_levenberg_marquardt_fit(10.0f); + _fit_step++; + } else if(_fit_step < 35) { + optimise.do_levenberg_marquardt_fit(10.0f); //ellipsoid fit + _fit_step++; + } else { + if(fit_acceptable()) { + set_status(COMPASS_CAL_SUCCESS); + } else { + set_status(COMPASS_CAL_FAILED); + } + } + } +} + +///////////////////////////////////////////////////////////// +////////////////////// PRIVATE METHODS ////////////////////// +///////////////////////////////////////////////////////////// +bool CompassCalibrator::running() const { + return _status == COMPASS_CAL_RUNNING_STEP_ONE || _status == COMPASS_CAL_RUNNING_STEP_TWO; +} + +bool CompassCalibrator::fitting() const { + return running() && _samples_collected == COMPASS_CAL_NUM_SAMPLES; +} + +void CompassCalibrator::initialize_fit() { + //initialize _fitness before starting a fit + if (_samples_collected != 0) { + _fitness = calc_mean_squared_residuals(); + } else { + _fitness = 1.0e30f; + } + _ellipsoid_lambda = 1.0f; + _sphere_lambda = 1.0f; + _initial_fitness = _fitness; + _fit_step = 0; +} + +void CompassCalibrator::reset_state() { + _samples_collected = 0; + _samples_thinned = 0; + _params.radius = 200; + _params.offset.zero(); + _params.diag = Vector3f(1.0f,1.0f,1.0f); + _params.offdiag.zero(); + + initialize_fit(); +} + +bool CompassCalibrator::set_status(compass_cal_status_t status) { + if (status != COMPASS_CAL_NOT_STARTED && _status == status) { + return true; + } + + switch(status) { + case COMPASS_CAL_NOT_STARTED: + reset_state(); + _status = COMPASS_CAL_NOT_STARTED; + + if(_sample_buffer != NULL) { + free(_sample_buffer); + _sample_buffer = NULL; + } + return true; + + case COMPASS_CAL_WAITING_TO_START: + reset_state(); + _status = COMPASS_CAL_WAITING_TO_START; + + set_status(COMPASS_CAL_RUNNING_STEP_ONE); + return true; + + case COMPASS_CAL_RUNNING_STEP_ONE: + if(_status != COMPASS_CAL_WAITING_TO_START) { + return false; + } + + if(_attempt == 1 && (hal.scheduler->millis()-_start_time_ms)*1.0e-3f < _delay_start_sec) { + return false; + } + + if(_sample_buffer != NULL) { + initialize_fit(); + _status = COMPASS_CAL_RUNNING_STEP_ONE; + return true; + } + + _sample_buffer = (Vector3i*)malloc(sizeof(Vector3i)*COMPASS_CAL_NUM_SAMPLES); + + if(_sample_buffer != NULL) { + initialize_fit(); + _status = COMPASS_CAL_RUNNING_STEP_ONE; + return true; + } + + return false; + + case COMPASS_CAL_RUNNING_STEP_TWO: + if(_status != COMPASS_CAL_RUNNING_STEP_ONE) { + return false; + } + thin_samples(); + initialize_fit(); + _status = COMPASS_CAL_RUNNING_STEP_TWO; + return true; + + case COMPASS_CAL_SUCCESS: + if(_status != COMPASS_CAL_RUNNING_STEP_TWO) { + return false; + } + + if(_sample_buffer != NULL) { + free(_sample_buffer); + _sample_buffer = NULL; + } + + _status = COMPASS_CAL_SUCCESS; + return true; + + case COMPASS_CAL_FAILED: + if(_status == COMPASS_CAL_NOT_STARTED) { + return false; + } + + AP_Notify::events.compass_cal_failed = 1; + + if(_retry && set_status(COMPASS_CAL_WAITING_TO_START)) { + _attempt++; + return true; + } + + if(_sample_buffer != NULL) { + free(_sample_buffer); + _sample_buffer = NULL; + } + + _status = COMPASS_CAL_FAILED; + return true; + + default: + return false; + }; +} + +bool CompassCalibrator::fit_acceptable() { + if( !isnan(_fitness) && + _params.radius > 200 && //Earth's magnetic field strength range: 250-650mG + _params.radius < 700 && + fabsf(_params.offset.x) < 1000 && + fabsf(_params.offset.y) < 1000 && + fabsf(_params.offset.z) < 1000 && + fabsf(_params.offdiag.x) < 1.0f && //absolute of sine/cosine output cannot be greater than 1 + fabsf(_params.offdiag.x) < 1.0f && + fabsf(_params.offdiag.x) < 1.0f ){ + + return _fitness <= sq(_tolerance); + } + return false; +} + +void CompassCalibrator::thin_samples() { + if(_sample_buffer == NULL) { + return; + } + + _samples_thinned = 0; + // shuffle the samples http://en.wikipedia.org/wiki/Fisher%E2%80%93Yates_shuffle + // this is so that adjacent samples don't get sequentially eliminated + for(uint16_t i=_samples_collected-1; i>=1; i--) { + uint16_t j = get_random() % (i+1); + Vector3i temp = _sample_buffer[i]; + _sample_buffer[i] = _sample_buffer[j]; + _sample_buffer[j] = temp; + } + + for(uint16_t i=0; i < _samples_collected; i++) { + if(!accept_sample(get_sample(_sample_buffer, i))) { + _sample_buffer[i] = _sample_buffer[_samples_collected-1]; + _samples_collected --; + _samples_thinned ++; + } + } +} + +bool CompassCalibrator::accept_sample(const Vector3f& sample) +{ + if(_sample_buffer == NULL) { + return false; + } + + float max_distance = fabsf(5.38709f * _params.radius / sqrtf((float)COMPASS_CAL_NUM_SAMPLES)) / 3.0f; + + for (uint16_t i = 0; i<_samples_collected; i++){ + float distance = (sample - get_sample(_sample_buffer, i)).length(); + if(distance < max_distance) { + return false; + } + } + return true; +} + + +float CompassCalibrator::calc_mean_squared_residuals() +{ + return optimise.calc_mean_squared_residuals(_params.get_sphere_params()); +} + +float CompassCalibrator::calc_sphere_residual(const Vector3f& sample, const float params[], const float const_params[]) { + float radius = params[0]; + Vector3f offset = Vector3f(params[1], params[2], params[3]); + + return radius - ((sample+offset)).length(); +} + +float CompassCalibrator::calc_ellipsoid_residual(const Vector3f& sample, const float params[], const float const_params[]) { + + const Vector3f &offset = Vector3f(params[0], params[1], params[2]); + const Vector3f &diag = Vector3f(params[3], params[4], params[5]); + const Vector3f &offdiag = Vector3f(params[6], params[7], params[8]); + + Matrix3f softiron( + diag.x , offdiag.x , offdiag.y, + offdiag.x , diag.y , offdiag.z, + offdiag.y , offdiag.z , diag.z + ); + return const_params[0] - (softiron*(sample+offset)).length(); +} + +void CompassCalibrator::calc_sphere_jacob(const Vector3f& sample, const float params[], float* ret, const float const_params[]) { + + const Vector3f &offset = Vector3f(params[1], params[2], params[3]); + const Vector3f &diag = Vector3f(1.0f, 1.0f, 1.0f); + const Vector3f &offdiag = Vector3f(0.0f, 0.0f, 0.0f); + Matrix3f softiron( + diag.x , offdiag.x , offdiag.y, + offdiag.x , diag.y , offdiag.z, + offdiag.y , offdiag.z , diag.z + ); + + float A = (diag.x * (sample.x + offset.x)) + (offdiag.x * (sample.y + offset.y)) + (offdiag.y * (sample.z + offset.z)); + float B = (offdiag.x * (sample.x + offset.x)) + (diag.y * (sample.y + offset.y)) + (offdiag.z * (sample.z + offset.z)); + float C = (offdiag.y * (sample.x + offset.x)) + (offdiag.z * (sample.y + offset.y)) + (diag.z * (sample.z + offset.z)); + float length = (softiron*(sample+offset)).length(); + + // 0: radius + ret[0] = 1; + // 1-3: offsets + ret[1] = -1.0f * (((diag.x * A) + (offdiag.x * B) + (offdiag.y * C))/length); + ret[2] = -1.0f * (((offdiag.x * A) + (diag.y * B) + (offdiag.z * C))/length); + ret[3] = -1.0f * (((offdiag.y * A) + (offdiag.z * B) + (diag.z * C))/length); +} + + +void CompassCalibrator::calc_ellipsoid_jacob(const Vector3f& sample, const float params[], float* ret, const float const_params[]) { + + const Vector3f &offset = Vector3f(params[0], params[1], params[2]); + const Vector3f &diag = Vector3f(params[3], params[4], params[5]); + const Vector3f &offdiag = Vector3f(params[6], params[7], params[8]); + Matrix3f softiron( + diag.x , offdiag.x , offdiag.y, + offdiag.x , diag.y , offdiag.z, + offdiag.y , offdiag.z , diag.z + ); + + float A = (diag.x * (sample.x + offset.x)) + (offdiag.x * (sample.y + offset.y)) + (offdiag.y * (sample.z + offset.z)); + float B = (offdiag.x * (sample.x + offset.x)) + (diag.y * (sample.y + offset.y)) + (offdiag.z * (sample.z + offset.z)); + float C = (offdiag.y * (sample.x + offset.x)) + (offdiag.z * (sample.y + offset.y)) + (diag.z * (sample.z + offset.z)); + float length = (softiron*(sample+offset)).length(); + + // 0-2: offsets + ret[0] = -1.0f * (((diag.x * A) + (offdiag.x * B) + (offdiag.y * C))/length); + ret[1] = -1.0f * (((offdiag.x * A) + (diag.y * B) + (offdiag.z * C))/length); + ret[2] = -1.0f * (((offdiag.y * A) + (offdiag.z * B) + (diag.z * C))/length); + // 3-5: diagonals + ret[3] = -1.0f * ((sample.x + offset.x) * A)/length; + ret[4] = -1.0f * ((sample.y + offset.y) * B)/length; + ret[5] = -1.0f * ((sample.z + offset.z) * C)/length; + // 6-8: off-diagonals + ret[6] = -1.0f * (((sample.y + offset.y) * A) + ((sample.z + offset.z) * B))/length; + ret[7] = -1.0f * (((sample.z + offset.z) * A) + ((sample.x + offset.x) * C))/length; + ret[8] = -1.0f * (((sample.z + offset.z) * B) + ((sample.y + offset.y) * C))/length; +} + +////////////////////////////////////////////////////////// +////////////////////// MATH HELPERS ////////////////////// +////////////////////////////////////////////////////////// + +uint16_t CompassCalibrator::get_random(void) +{ + static uint32_t m_z = 1234; + static uint32_t m_w = 76542; + m_z = 36969 * (m_z & 65535) + (m_z >> 16); + m_w = 18000 * (m_w & 65535) + (m_w >> 16); + return ((m_z << 16) + m_w) & 0xFFFF; +} + +////////////////////////////////////////////////////////// +//////////// CompassSample public interface ////////////// +////////////////////////////////////////////////////////// + +Vector3f CompassCalibrator::get_sample(const Vector3i in[], uint16_t num) { + Vector3f out; + out.x = (float)in[num].x*2048.0f/32700.0f; + out.y = (float)in[num].y*2048.0f/32700.0f; + out.z = (float)in[num].z*2048.0f/32700.0f; + return out; +} + +void CompassCalibrator::set_sample(const Vector3f& in, uint16_t num) { + _sample_buffer[num].x = (int16_t)(in.x*32700.0f/2048.0f + 0.5f); + _sample_buffer[num].y = (int16_t)(in.y*32700.0f/2048.0f + 0.5f); + _sample_buffer[num].z = (int16_t)(in.z*32700.0f/2048.0f + 0.5f); +} diff --git a/libraries/AP_Compass/CompassCalibrator.h b/libraries/AP_Compass/CompassCalibrator.h new file mode 100644 index 0000000000000..3ab29aef9faa7 --- /dev/null +++ b/libraries/AP_Compass/CompassCalibrator.h @@ -0,0 +1,116 @@ +#include + +#define COMPASS_CAL_NUM_SPHERE_PARAMS 4 +#define COMPASS_CAL_NUM_ELLIPSOID_PARAMS 9 +#define COMPASS_CAL_NUM_SAMPLES 300 + +//RMS tolerance +#define COMPASS_CAL_DEFAULT_TOLERANCE 5.0f + +enum compass_cal_status_t { + COMPASS_CAL_NOT_STARTED=0, + COMPASS_CAL_WAITING_TO_START=1, + COMPASS_CAL_RUNNING_STEP_ONE=2, + COMPASS_CAL_RUNNING_STEP_TWO=3, + COMPASS_CAL_SUCCESS=4, + COMPASS_CAL_FAILED=5 +}; + +class CompassCalibrator { +public: + CompassCalibrator(); + + void start(bool retry=false, bool autosave=false, float delay=0.0f); + void clear(); + + void new_sample(const Vector3f &sample); + void run_fit_chunk(); + + bool check_for_timeout(); + + bool running() const; + + void set_tolerance(float tolerance) { _tolerance = tolerance; } + + void get_calibration(Vector3f &offsets, Vector3f &diagonals, Vector3f &offdiagonals); + + float get_completion_percent() const; + enum compass_cal_status_t get_status() const { return _status; } + float get_fitness() const { return sqrtf(_fitness); } + bool get_autosave() const { return _autosave; } + uint8_t get_attempt() const { return _attempt; } + +private: + Optimiser optimise; + + class param_t { + public: + float* get_sphere_params() { + return &radius; + } + + float* get_ellipsoid_params() { + return &offset.x; + } + + float radius; + Vector3f offset; + Vector3f diag; + Vector3f offdiag; + }; + + static Vector3f get_sample(const Vector3i in[], uint16_t num); + void set_sample(const Vector3f& in, uint16_t num); + + enum compass_cal_status_t _status; + + // timeout watchdog state + uint32_t _last_sample_ms; + + // behavioral state + float _delay_start_sec; + uint32_t _start_time_ms; + bool _autosave; + bool _retry; + float _tolerance; + uint8_t _attempt; + + //fit state + struct param_t _params; + uint16_t _fit_step; + Vector3i *_sample_buffer; + float _fitness; // mean squared residuals + float _initial_fitness; + float _sphere_lambda; + float _ellipsoid_lambda; + uint16_t _samples_collected; + uint16_t _samples_thinned; + + bool set_status(compass_cal_status_t status); + + // returns true if sample should be added to buffer + bool accept_sample(const Vector3f &sample); + + // returns true if fit is acceptable + bool fit_acceptable(); + + void reset_state(); + void initialize_fit(); + + bool fitting() const; + + // thins out samples between step one and step two + void thin_samples(); + + static float calc_sphere_residual(const Vector3f& sample,const float params[], const float const_params[]); + static float calc_ellipsoid_residual(const Vector3f& sample,const float params[], const float const_params[]); + + float calc_mean_squared_residuals(); + + static void calc_sphere_jacob(const Vector3f& sample, const float params[], float* ret, const float const_params[]); + + static void calc_ellipsoid_jacob(const Vector3f& sample, const float params[], float* ret, const float const_params[]); + + // math helpers + uint16_t get_random(); +}; diff --git a/libraries/AP_Math/AP_Math.h b/libraries/AP_Math/AP_Math.h index 19a3d8bbbe178..fd7db5798996e 100644 --- a/libraries/AP_Math/AP_Math.h +++ b/libraries/AP_Math/AP_Math.h @@ -19,6 +19,7 @@ #include "quaternion.h" #include "polygon.h" #include "edc.h" +#include "optimiser.h" #ifndef M_PI_F #define M_PI_F 3.141592653589793f @@ -33,6 +34,8 @@ #define DEG_TO_RAD 0.017453292519943295769236907684886f #define RAD_TO_DEG 57.295779513082320876798154814105f + +#define TINY_FLOAT 1e-20 //GPS Specific double precision conversions //The precision here does matter when using the wsg* functions for converting //between LLH and ECEF coordinates. Test code in examlpes/location/location.pde @@ -183,6 +186,8 @@ float sq(float v); float pythagorous2(float a, float b); float pythagorous3(float a, float b, float c); +// matrix algebra +bool inverse(float x[], float y[], uint16_t dim); #ifdef radians #error "Build is including Arduino base headers" #endif diff --git a/libraries/AP_Math/matrix_alg.cpp b/libraries/AP_Math/matrix_alg.cpp new file mode 100644 index 0000000000000..9d14677422ce6 --- /dev/null +++ b/libraries/AP_Math/matrix_alg.cpp @@ -0,0 +1,603 @@ +#include +#include + +extern const AP_HAL::HAL& hal; + +float det6x6(const float C[36]) +{ + float f; + float A[36]; + int8_t ipiv[6]; + int32_t i0; + int32_t j; + int32_t c; + int32_t iy; + int32_t ix; + float smax; + int32_t jy; + float s; + int32_t b_j; + int32_t ijA; + bool isodd; + memcpy(&A[0], &C[0], 36U * sizeof(float)); + for (i0 = 0; i0 < 6; i0++) { + ipiv[i0] = (int8_t)(1 + i0); + } + + for (j = 0; j < 5; j++) { + c = j * 7; + iy = 0; + ix = c; + smax = fabsf(A[c]); + for (jy = 2; jy <= 6 - j; jy++) { + ix++; + s = fabsf(A[ix]); + if (s > smax) { + iy = jy - 1; + smax = s; + } + } + + if (A[c + iy] != 0.0f) { + if (iy != 0) { + ipiv[j] = (int8_t)((j + iy) + 1); + ix = j; + iy += j; + for (jy = 0; jy < 6; jy++) { + smax = A[ix]; + A[ix] = A[iy]; + A[iy] = smax; + ix += 6; + iy += 6; + } + } + + i0 = (c - j) + 6; + for (iy = c + 1; iy + 1 <= i0; iy++) { + A[iy] /= A[c]; + } + } + + iy = c; + jy = c + 6; + for (b_j = 1; b_j <= 5 - j; b_j++) { + smax = A[jy]; + if (A[jy] != 0.0f) { + ix = c + 1; + i0 = (iy - j) + 12; + for (ijA = 7 + iy; ijA + 1 <= i0; ijA++) { + A[ijA] += A[ix] * -smax; + ix++; + } + } + + jy += 6; + iy += 6; + } + } + + f = A[0]; + isodd = false; + for (jy = 0; jy < 5; jy++) { + f *= A[(jy + 6 * (1 + jy)) + 1]; + if (ipiv[jy] > 1 + jy) { + isodd = !isodd; + } + } + + if (isodd) { + f = -f; + } + + return f; +} + +float det9x9(const float C[81]) +{ + float f; + float A[81]; + int8_t ipiv[9]; + int32_t i0; + int32_t j; + int32_t c; + int32_t iy; + int32_t ix; + float smax; + int32_t jy; + float s; + int32_t b_j; + int32_t ijA; + bool isodd; + memcpy(&A[0], &C[0], 81U * sizeof(float)); + for (i0 = 0; i0 < 9; i0++) { + ipiv[i0] = (int8_t)(1 + i0); + } + + for (j = 0; j < 8; j++) { + c = j * 10; + iy = 0; + ix = c; + smax = fabs(A[c]); + for (jy = 2; jy <= 9 - j; jy++) { + ix++; + s = fabs(A[ix]); + if (s > smax) { + iy = jy - 1; + smax = s; + } + } + + if (A[c + iy] != 0.0) { + if (iy != 0) { + ipiv[j] = (int8_t)((j + iy) + 1); + ix = j; + iy += j; + for (jy = 0; jy < 9; jy++) { + smax = A[ix]; + A[ix] = A[iy]; + A[iy] = smax; + ix += 9; + iy += 9; + } + } + + i0 = (c - j) + 9; + for (iy = c + 1; iy + 1 <= i0; iy++) { + A[iy] /= A[c]; + } + } + + iy = c; + jy = c + 9; + for (b_j = 1; b_j <= 8 - j; b_j++) { + smax = A[jy]; + if (A[jy] != 0.0) { + ix = c + 1; + i0 = (iy - j) + 18; + for (ijA = 10 + iy; ijA + 1 <= i0; ijA++) { + A[ijA] += A[ix] * -smax; + ix++; + } + } + + jy += 9; + iy += 9; + } + } + + f = A[0]; + isodd = false; + for (jy = 0; jy < 8; jy++) { + f *= A[(jy + 9 * (1 + jy)) + 1]; + if (ipiv[jy] > 1 + jy) { + isodd = !isodd; + } + } + + if (isodd) { + f = -f; + } + + return f; +} + +bool inverse9x9(const float x[81], float y[81]) +{ + if(fabsf(det9x9(x)) < 1.0e-20f) { + return false; + } + float A[81]; + int32_t i0; + int8_t ipiv[9]; + int32_t j; + int32_t c; + int32_t pipk; + int32_t ix; + float smax; + int32_t k; + float s; + int32_t jy; + int32_t ijA; + int8_t p[9]; + for (i0 = 0; i0 < 81; i0++) { + A[i0] = x[i0]; + y[i0] = 0.0; + } + + for (i0 = 0; i0 < 9; i0++) { + ipiv[i0] = (int8_t)(1 + i0); + } + + for (j = 0; j < 8; j++) { + c = j * 10; + pipk = 0; + ix = c; + smax = fabs(A[c]); + for (k = 2; k <= 9 - j; k++) { + ix++; + s = fabs(A[ix]); + if (s > smax) { + pipk = k - 1; + smax = s; + } + } + + if (A[c + pipk] != 0.0) { + if (pipk != 0) { + ipiv[j] = (int8_t)((j + pipk) + 1); + ix = j; + pipk += j; + for (k = 0; k < 9; k++) { + smax = A[ix]; + A[ix] = A[pipk]; + A[pipk] = smax; + ix += 9; + pipk += 9; + } + } + + i0 = (c - j) + 9; + for (jy = c + 1; jy + 1 <= i0; jy++) { + A[jy] /= A[c]; + } + } + + pipk = c; + jy = c + 9; + for (k = 1; k <= 8 - j; k++) { + smax = A[jy]; + if (A[jy] != 0.0) { + ix = c + 1; + i0 = (pipk - j) + 18; + for (ijA = 10 + pipk; ijA + 1 <= i0; ijA++) { + A[ijA] += A[ix] * -smax; + ix++; + } + } + + jy += 9; + pipk += 9; + } + } + + for (i0 = 0; i0 < 9; i0++) { + p[i0] = (int8_t)(1 + i0); + } + + for (k = 0; k < 8; k++) { + if (ipiv[k] > 1 + k) { + pipk = p[ipiv[k] - 1]; + p[ipiv[k] - 1] = p[k]; + p[k] = (int8_t)pipk; + } + } + + for (k = 0; k < 9; k++) { + y[k + 9 * (p[k] - 1)] = 1.0; + for (j = k; j + 1 < 10; j++) { + if (y[j + 9 * (p[k] - 1)] != 0.0) { + for (jy = j + 1; jy + 1 < 10; jy++) { + y[jy + 9 * (p[k] - 1)] -= y[j + 9 * (p[k] - 1)] * A[jy + 9 * j]; + } + } + } + } + + for (j = 0; j < 9; j++) { + c = 9 * j; + for (k = 8; k > -1; k += -1) { + pipk = 9 * k; + if (y[k + c] != 0.0) { + y[k + c] /= A[k + pipk]; + for (jy = 0; jy + 1 <= k; jy++) { + y[jy + c] -= y[k + c] * A[jy + pipk]; + } + } + } + } + return true; +} + + +bool inverse6x6(const float x[], float y[]) +{ + if(fabsf(det6x6(x)) < 1.0e-20f) { + return false; + } + + float A[36]; + int32_t i0; + int32_t ipiv[6]; + int32_t j; + int32_t c; + int32_t pipk; + int32_t ix; + float smax; + int32_t k; + float s; + int32_t jy; + int32_t ijA; + int32_t p[6]; + for (i0 = 0; i0 < 36; i0++) { + A[i0] = x[i0]; + y[i0] = 0.0f; + } + + for (i0 = 0; i0 < 6; i0++) { + ipiv[i0] = (int32_t)(1 + i0); + } + + for (j = 0; j < 5; j++) { + c = j * 7; + pipk = 0; + ix = c; + smax = fabsf(A[c]); + for (k = 2; k <= 6 - j; k++) { + ix++; + s = fabsf(A[ix]); + if (s > smax) { + pipk = k - 1; + smax = s; + } + } + + if (A[c + pipk] != 0.0f) { + if (pipk != 0) { + ipiv[j] = (int32_t)((j + pipk) + 1); + ix = j; + pipk += j; + for (k = 0; k < 6; k++) { + smax = A[ix]; + A[ix] = A[pipk]; + A[pipk] = smax; + ix += 6; + pipk += 6; + } + } + + i0 = (c - j) + 6; + for (jy = c + 1; jy + 1 <= i0; jy++) { + A[jy] /= A[c]; + } + } + + pipk = c; + jy = c + 6; + for (k = 1; k <= 5 - j; k++) { + smax = A[jy]; + if (A[jy] != 0.0f) { + ix = c + 1; + i0 = (pipk - j) + 12; + for (ijA = 7 + pipk; ijA + 1 <= i0; ijA++) { + A[ijA] += A[ix] * -smax; + ix++; + } + } + + jy += 6; + pipk += 6; + } + } + + for (i0 = 0; i0 < 6; i0++) { + p[i0] = (int32_t)(1 + i0); + } + + for (k = 0; k < 5; k++) { + if (ipiv[k] > 1 + k) { + pipk = p[ipiv[k] - 1]; + p[ipiv[k] - 1] = p[k]; + p[k] = (int32_t)pipk; + } + } + + for (k = 0; k < 6; k++) { + y[k + 6 * (p[k] - 1)] = 1.0; + for (j = k; j + 1 < 7; j++) { + if (y[j + 6 * (p[k] - 1)] != 0.0f) { + for (jy = j + 1; jy + 1 < 7; jy++) { + y[jy + 6 * (p[k] - 1)] -= y[j + 6 * (p[k] - 1)] * A[jy + 6 * j]; + } + } + } + } + + for (j = 0; j < 6; j++) { + c = 6 * j; + for (k = 5; k > -1; k += -1) { + pipk = 6 * k; + if (y[k + c] != 0.0f) { + y[k + c] /= A[k + pipk]; + for (jy = 0; jy + 1 <= k; jy++) { + y[jy + c] -= y[k + c] * A[jy + pipk]; + } + } + } + } + return true; +} + +bool inverse3x3(float m[], float invOut[]) +{ + float inv[9]; + // computes the inverse of a matrix m + float det = m[0] * (m[4] * m[8] - m[7] * m[5]) - + m[1] * (m[3] * m[8] - m[5] * m[6]) + + m[2] * (m[3] * m[7] - m[4] * m[6]); + if(fabsf(det) < 1.0e-20f){ + return false; + } + + float invdet = 1 / det; + + inv[0] = (m[4] * m[8] - m[7] * m[5]) * invdet; + inv[1] = (m[2] * m[7] - m[1] * m[8]) * invdet; + inv[2] = (m[1] * m[5] - m[2] * m[4]) * invdet; + inv[3] = (m[5] * m[6] - m[5] * m[8]) * invdet; + inv[4] = (m[0] * m[8] - m[2] * m[6]) * invdet; + inv[5] = (m[3] * m[2] - m[0] * m[5]) * invdet; + inv[6] = (m[3] * m[7] - m[6] * m[4]) * invdet; + inv[7] = (m[6] * m[1] - m[0] * m[7]) * invdet; + inv[8] = (m[0] * m[4] - m[3] * m[1]) * invdet; + + for(uint8_t i = 0; i < 9; i++){ + invOut[i] = inv[i]; + } + + return true; +} + +/* + * matrix inverse code only for 4x4 square matrix copied from + * gluInvertMatrix implementation in + * opengl for 4x4 matrices. + * + * @param m, input 4x4 matrix + * @param invOut, Output inverted 4x4 matrix + * @returns false = matrix is Singular, true = matrix inversion successful + * Known Issues/ Possible Enhancements: + * -Will need a different implementation for more number + * of parameters like in the case of addition of soft + * iron calibration + */ +bool inverse4x4(float m[],float invOut[]) +{ + float inv[16], det; + uint8_t i; + + inv[0] = m[5] * m[10] * m[15] - + m[5] * m[11] * m[14] - + m[9] * m[6] * m[15] + + m[9] * m[7] * m[14] + + m[13] * m[6] * m[11] - + m[13] * m[7] * m[10]; + + inv[4] = -m[4] * m[10] * m[15] + + m[4] * m[11] * m[14] + + m[8] * m[6] * m[15] - + m[8] * m[7] * m[14] - + m[12] * m[6] * m[11] + + m[12] * m[7] * m[10]; + + inv[8] = m[4] * m[9] * m[15] - + m[4] * m[11] * m[13] - + m[8] * m[5] * m[15] + + m[8] * m[7] * m[13] + + m[12] * m[5] * m[11] - + m[12] * m[7] * m[9]; + + inv[12] = -m[4] * m[9] * m[14] + + m[4] * m[10] * m[13] + + m[8] * m[5] * m[14] - + m[8] * m[6] * m[13] - + m[12] * m[5] * m[10] + + m[12] * m[6] * m[9]; + + inv[1] = -m[1] * m[10] * m[15] + + m[1] * m[11] * m[14] + + m[9] * m[2] * m[15] - + m[9] * m[3] * m[14] - + m[13] * m[2] * m[11] + + m[13] * m[3] * m[10]; + + inv[5] = m[0] * m[10] * m[15] - + m[0] * m[11] * m[14] - + m[8] * m[2] * m[15] + + m[8] * m[3] * m[14] + + m[12] * m[2] * m[11] - + m[12] * m[3] * m[10]; + + inv[9] = -m[0] * m[9] * m[15] + + m[0] * m[11] * m[13] + + m[8] * m[1] * m[15] - + m[8] * m[3] * m[13] - + m[12] * m[1] * m[11] + + m[12] * m[3] * m[9]; + + inv[13] = m[0] * m[9] * m[14] - + m[0] * m[10] * m[13] - + m[8] * m[1] * m[14] + + m[8] * m[2] * m[13] + + m[12] * m[1] * m[10] - + m[12] * m[2] * m[9]; + + inv[2] = m[1] * m[6] * m[15] - + m[1] * m[7] * m[14] - + m[5] * m[2] * m[15] + + m[5] * m[3] * m[14] + + m[13] * m[2] * m[7] - + m[13] * m[3] * m[6]; + + inv[6] = -m[0] * m[6] * m[15] + + m[0] * m[7] * m[14] + + m[4] * m[2] * m[15] - + m[4] * m[3] * m[14] - + m[12] * m[2] * m[7] + + m[12] * m[3] * m[6]; + + inv[10] = m[0] * m[5] * m[15] - + m[0] * m[7] * m[13] - + m[4] * m[1] * m[15] + + m[4] * m[3] * m[13] + + m[12] * m[1] * m[7] - + m[12] * m[3] * m[5]; + + inv[14] = -m[0] * m[5] * m[14] + + m[0] * m[6] * m[13] + + m[4] * m[1] * m[14] - + m[4] * m[2] * m[13] - + m[12] * m[1] * m[6] + + m[12] * m[2] * m[5]; + + inv[3] = -m[1] * m[6] * m[11] + + m[1] * m[7] * m[10] + + m[5] * m[2] * m[11] - + m[5] * m[3] * m[10] - + m[9] * m[2] * m[7] + + m[9] * m[3] * m[6]; + + inv[7] = m[0] * m[6] * m[11] - + m[0] * m[7] * m[10] - + m[4] * m[2] * m[11] + + m[4] * m[3] * m[10] + + m[8] * m[2] * m[7] - + m[8] * m[3] * m[6]; + + inv[11] = -m[0] * m[5] * m[11] + + m[0] * m[7] * m[9] + + m[4] * m[1] * m[11] - + m[4] * m[3] * m[9] - + m[8] * m[1] * m[7] + + m[8] * m[3] * m[5]; + + inv[15] = m[0] * m[5] * m[10] - + m[0] * m[6] * m[9] - + m[4] * m[1] * m[10] + + m[4] * m[2] * m[9] + + m[8] * m[1] * m[6] - + m[8] * m[2] * m[5]; + + det = m[0] * inv[0] + m[1] * inv[4] + m[2] * inv[8] + m[3] * inv[12]; + + if(fabsf(det) < 1.0e-20f){ + return false; + } + + det = 1.0f / det; + + for (i = 0; i < 16; i++) + invOut[i] = inv[i] * det; + return true; +} + + +bool inverse(float x[], float y[], uint16_t dim) +{ + switch(dim){ + case 3: return inverse3x3(x,y); + case 4: return inverse4x4(x,y); + case 6: return inverse6x6(x,y); + case 9: return inverse9x9(x,y); + default: return false; + } +} \ No newline at end of file diff --git a/libraries/AP_Math/optimiser.cpp b/libraries/AP_Math/optimiser.cpp new file mode 100644 index 0000000000000..739d78aaca588 --- /dev/null +++ b/libraries/AP_Math/optimiser.cpp @@ -0,0 +1,124 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +#include +#include "optimiser.h" + +void Optimiser::set_init_params(float params[], const float const_params[], uint16_t num_params, float& fitness, float& lambda) +{ + _params = params; + _const_params = const_params; + _num_params = num_params; + _fitness = &fitness; + _lambda = λ +} + +float Optimiser::calc_mean_squared_residuals(float* params) +{ + if(_sample_buffer_size == 0) { + return 1.0e30f; + } + float sum = 0.0f; + for(uint16_t i=0; i < _sample_buffer_size; i++){ + Vector3f sample = _process_sample(_sample_buffer, i); + float resid = _calc_residual(sample, params, _const_params); + sum += sq(resid); + } + sum /= _sample_buffer_size; + return sum; +} + +void Optimiser::do_levenberg_marquardt_fit(float lma_damping, uint16_t num_iters) +{ + if(_sample_buffer_size == 0) { + return; + } + + float fitness = *_fitness; + float fit1, fit2; + + + float *JTJ = new float[_num_params*_num_params]; + float *JTJ2 = new float[_num_params*_num_params]; + float *JTFI = new float[_num_params]; + float *jacob = new float[_num_params]; + + float *fit1_params = new float[_num_params]; + float *fit2_params = new float[_num_params]; + if (JTJ == NULL || JTJ2 == NULL || JTFI == NULL || jacob == NULL || fit1_params == NULL || fit2_params == NULL ){ + goto lm_ret; + } + + memcpy(fit1_params,_params,sizeof(float)*_num_params); + memcpy(fit2_params,_params,sizeof(float)*_num_params); + + for(uint16_t iters = 0; iters < num_iters; iters++){ + + memset(JTJ,0,sizeof(float)*_num_params*_num_params); + memset(JTJ2,0,sizeof(float)*_num_params*_num_params); + memset(JTFI,0,sizeof(float)*_num_params); + + for(uint16_t k = 0; k<_sample_buffer_size; k++) { + Vector3f sample = _process_sample(_sample_buffer, k); + + _calc_jacob(sample, fit1_params, jacob, _const_params); + + for(uint8_t i = 0;i < _num_params; i++) { + // compute JTJ + for(uint8_t j = 0; j < _num_params; j++) { + JTJ[i*_num_params+j] += jacob[i] * jacob[j]; + JTJ2[i*_num_params+j] += jacob[i] * jacob[j]; //a backup JTJ for LM + } + // compute JTFI + JTFI[i] += jacob[i] * _calc_residual(sample, fit1_params, _const_params); + } + } + + //------------------------Levenberg-part-starts-here---------------------------------// + for(uint8_t i = 0; i < _num_params; i++) { + JTJ[i*_num_params+i] += (*_lambda); + JTJ2[i*_num_params+i] += (*_lambda)/lma_damping; + } + + if(!inverse(JTJ, JTJ, _num_params)) { + goto lm_ret; + } + + if(!inverse(JTJ2, JTJ2, _num_params)) { + goto lm_ret; + } + + for(uint8_t row=0; row < _num_params; row++) { + for(uint8_t col=0; col < _num_params; col++) { + fit1_params[row] -= JTFI[col] * JTJ[row*_num_params+col]; + fit2_params[row] -= JTFI[col] * JTJ2[row*_num_params+col]; + } + } + + fit1 = calc_mean_squared_residuals(fit1_params); + fit2 = calc_mean_squared_residuals(fit2_params); + + if(fit1 > *_fitness && fit2 > *_fitness){ + *_lambda *= lma_damping; + } else if(fit2 < *_fitness && fit2 < fit1) { + *_lambda /= lma_damping; + memcpy(fit1_params, fit2_params, sizeof(float)*_num_params); + fitness = fit2; + } else if(fit1 < *_fitness){ + fitness = fit1; + } + //--------------------Levenberg-part-ends-here------------------------------------// + + if(!isnan(fitness) && fitness < *_fitness) { + *_fitness = fitness; + memcpy(_params,fit1_params,sizeof(float)*_num_params); + } + } + +lm_ret: + delete[] fit1_params; + delete[] fit2_params; + delete[] JTJ; + delete[] JTJ2; + delete[] JTFI; + delete[] jacob; +} diff --git a/libraries/AP_Math/optimiser.h b/libraries/AP_Math/optimiser.h new file mode 100644 index 0000000000000..1b49009aafb40 --- /dev/null +++ b/libraries/AP_Math/optimiser.h @@ -0,0 +1,47 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +#ifndef OPTIMISER_H +#define OPTIMISER_H + + +class Optimiser{ +public: + //Functions to be passed for sample preprocessing, fitness tests and Jacobian calculations + typedef float (*residual)(const Vector3f& sample, const float params[], const float const_params[]); + typedef void (*jacob)(const Vector3f& sample, const float params[], float* ret, const float const_params[]); + typedef Vector3f (*process_sample)(const Vector3i _sample_buffer[], uint16_t num); + + //Funtions to Initialise Optimser + void set_fitness_function(residual calc_residual) { _calc_residual = calc_residual; } + void set_jacobian_function(jacob calc_jacob) { _calc_jacob = calc_jacob; } + void set_preprocess_sample_function(process_sample preprocess_sample) { _process_sample = preprocess_sample; } + void set_buffer( Vector3i sample_buffer[], uint16_t sample_buffer_size) { + _sample_buffer = sample_buffer; + _sample_buffer_size = sample_buffer_size; + } + void set_init_params(float params[], const float const_params[], uint16_t num_params, float& fitness, float& lambda); + + //Optimiser Funtions + void do_levenberg_marquardt_fit(float lma_damping, uint16_t num_iters = 1); + void do_gauss_newton_fit(uint16_t iters = 1); + + float calc_mean_squared_residuals(float *params); + +protected: + + jacob _calc_jacob; + residual _calc_residual; + process_sample _process_sample; + + float *_fitness; + float *_lambda; + + Vector3i *_sample_buffer; + float *_params; + const float *_const_params; + + uint16_t _num_params; + uint16_t _sample_buffer_size; +}; + +#endif //OPTIMISER_H \ No newline at end of file diff --git a/libraries/AP_Notify/AP_Notify.h b/libraries/AP_Notify/AP_Notify.h index 311474073259d..cf5c33405834c 100644 --- a/libraries/AP_Notify/AP_Notify.h +++ b/libraries/AP_Notify/AP_Notify.h @@ -72,6 +72,7 @@ class AP_Notify uint32_t parachute_release : 1; // 1 if parachute is being released uint32_t ekf_bad : 1; // 1 if ekf is reporting problems uint32_t autopilot_mode : 1; // 1 if vehicle is in an autopilot flight mode (only used by OreoLEDs) + uint32_t compass_cal_running: 1; // 1 if a compass calibration is running // additional flags uint32_t external_leds : 1; // 1 if external LEDs are enabled (normally only used for copter) @@ -90,6 +91,9 @@ class AP_Notify uint16_t mission_complete : 1; // 1 when the mission has completed successfully uint16_t waypoint_complete : 1; // 1 as vehicle completes a waypoint uint16_t firmware_update : 1; // 1 just before vehicle firmware is updated + uint16_t initiated_compass_cal : 1; // 1 when user input to begin compass cal was accepted + uint16_t compass_cal_saved : 1; // 1 when compass calibration was just saved + uint16_t compass_cal_failed : 1; // 1 when compass calibration was just saved }; // the notify flags are static to allow direct class access diff --git a/libraries/AP_Notify/ToneAlarm_PX4.cpp b/libraries/AP_Notify/ToneAlarm_PX4.cpp index 2e2b96311cd51..c96b82e2dc0b6 100644 --- a/libraries/AP_Notify/ToneAlarm_PX4.cpp +++ b/libraries/AP_Notify/ToneAlarm_PX4.cpp @@ -63,6 +63,8 @@ const ToneAlarm_PX4::Tone ToneAlarm_PX4::_tones[] { { "MBT200>B#1", true }, #define AP_NOTIFY_PX4_TONE_LOUD_BATTERY_ALERT_CTS 13 { "MBNT255>B#8B#8B#8B#8B#8B#8B#8B#8B#8B#8B#8B#8B#8B#8B#8B#8", true }, + #define AP_NOTIFY_PX4_TONE_QUIET_COMPASS_CALIBRATING_CTS 14 + { "MBNT255msgid = MAVLINK_MSG_ID_MAG_CAL_ACK; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MAG_CAL_ACK_LEN, MAVLINK_MSG_ID_MAG_CAL_ACK_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MAG_CAL_ACK_LEN); +#endif +} + +/** + * @brief Pack a mag_cal_ack message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID + * @param target_component Component ID + * @param compass_id Compass ID + * @param accept_cal 0=Discard the calibration, 1=Save the calibration + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_mag_cal_ack_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,uint8_t compass_id,uint8_t accept_cal) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_MAG_CAL_ACK_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 2, compass_id); + _mav_put_uint8_t(buf, 3, accept_cal); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MAG_CAL_ACK_LEN); +#else + mavlink_mag_cal_ack_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.compass_id = compass_id; + packet.accept_cal = accept_cal; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MAG_CAL_ACK_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_MAG_CAL_ACK; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MAG_CAL_ACK_LEN, MAVLINK_MSG_ID_MAG_CAL_ACK_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MAG_CAL_ACK_LEN); +#endif +} + +/** + * @brief Encode a mag_cal_ack struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param mag_cal_ack C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_mag_cal_ack_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_mag_cal_ack_t* mag_cal_ack) +{ + return mavlink_msg_mag_cal_ack_pack(system_id, component_id, msg, mag_cal_ack->target_system, mag_cal_ack->target_component, mag_cal_ack->compass_id, mag_cal_ack->accept_cal); +} + +/** + * @brief Encode a mag_cal_ack struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param mag_cal_ack C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_mag_cal_ack_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mag_cal_ack_t* mag_cal_ack) +{ + return mavlink_msg_mag_cal_ack_pack_chan(system_id, component_id, chan, msg, mag_cal_ack->target_system, mag_cal_ack->target_component, mag_cal_ack->compass_id, mag_cal_ack->accept_cal); +} + +/** + * @brief Send a mag_cal_ack message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID + * @param target_component Component ID + * @param compass_id Compass ID + * @param accept_cal 0=Discard the calibration, 1=Save the calibration + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_mag_cal_ack_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t compass_id, uint8_t accept_cal) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_MAG_CAL_ACK_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 2, compass_id); + _mav_put_uint8_t(buf, 3, accept_cal); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MAG_CAL_ACK, buf, MAVLINK_MSG_ID_MAG_CAL_ACK_LEN, MAVLINK_MSG_ID_MAG_CAL_ACK_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MAG_CAL_ACK, buf, MAVLINK_MSG_ID_MAG_CAL_ACK_LEN); +#endif +#else + mavlink_mag_cal_ack_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.compass_id = compass_id; + packet.accept_cal = accept_cal; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MAG_CAL_ACK, (const char *)&packet, MAVLINK_MSG_ID_MAG_CAL_ACK_LEN, MAVLINK_MSG_ID_MAG_CAL_ACK_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MAG_CAL_ACK, (const char *)&packet, MAVLINK_MSG_ID_MAG_CAL_ACK_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_MAG_CAL_ACK_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_mag_cal_ack_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t compass_id, uint8_t accept_cal) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 2, compass_id); + _mav_put_uint8_t(buf, 3, accept_cal); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MAG_CAL_ACK, buf, MAVLINK_MSG_ID_MAG_CAL_ACK_LEN, MAVLINK_MSG_ID_MAG_CAL_ACK_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MAG_CAL_ACK, buf, MAVLINK_MSG_ID_MAG_CAL_ACK_LEN); +#endif +#else + mavlink_mag_cal_ack_t *packet = (mavlink_mag_cal_ack_t *)msgbuf; + packet->target_system = target_system; + packet->target_component = target_component; + packet->compass_id = compass_id; + packet->accept_cal = accept_cal; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MAG_CAL_ACK, (const char *)packet, MAVLINK_MSG_ID_MAG_CAL_ACK_LEN, MAVLINK_MSG_ID_MAG_CAL_ACK_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MAG_CAL_ACK, (const char *)packet, MAVLINK_MSG_ID_MAG_CAL_ACK_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE MAG_CAL_ACK UNPACKING + + +/** + * @brief Get field target_system from mag_cal_ack message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_mag_cal_ack_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 0); +} + +/** + * @brief Get field target_component from mag_cal_ack message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_mag_cal_ack_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 1); +} + +/** + * @brief Get field compass_id from mag_cal_ack message + * + * @return Compass ID + */ +static inline uint8_t mavlink_msg_mag_cal_ack_get_compass_id(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 2); +} + +/** + * @brief Get field accept_cal from mag_cal_ack message + * + * @return 0=Discard the calibration, 1=Save the calibration + */ +static inline uint8_t mavlink_msg_mag_cal_ack_get_accept_cal(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 3); +} + +/** + * @brief Decode a mag_cal_ack message into a struct + * + * @param msg The message to decode + * @param mag_cal_ack C-struct to decode the message contents into + */ +static inline void mavlink_msg_mag_cal_ack_decode(const mavlink_message_t* msg, mavlink_mag_cal_ack_t* mag_cal_ack) +{ +#if MAVLINK_NEED_BYTE_SWAP + mag_cal_ack->target_system = mavlink_msg_mag_cal_ack_get_target_system(msg); + mag_cal_ack->target_component = mavlink_msg_mag_cal_ack_get_target_component(msg); + mag_cal_ack->compass_id = mavlink_msg_mag_cal_ack_get_compass_id(msg); + mag_cal_ack->accept_cal = mavlink_msg_mag_cal_ack_get_accept_cal(msg); +#else + memcpy(mag_cal_ack, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_MAG_CAL_ACK_LEN); +#endif +} diff --git a/libraries/GCS_MAVLink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mag_cal_progress.h b/libraries/GCS_MAVLink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mag_cal_progress.h new file mode 100644 index 0000000000000..c15853ff8361f --- /dev/null +++ b/libraries/GCS_MAVLink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mag_cal_progress.h @@ -0,0 +1,393 @@ +// MESSAGE MAG_CAL_PROGRESS PACKING + +#define MAVLINK_MSG_ID_MAG_CAL_PROGRESS 191 + +typedef struct __mavlink_mag_cal_progress_t +{ + float direction_x; ///< Body frame direction vector for display + float direction_y; ///< Body frame direction vector for display + float direction_z; ///< Body frame direction vector for display + uint8_t compass_id; ///< Compass being calibrated + uint8_t cal_mask; ///< Bitmask of compasses being calibrated + uint8_t cal_status; ///< Status (see MAG_CAL_STATUS enum) + uint8_t attempt; ///< Attempt number + uint8_t completion_pct; ///< Completion percentage + uint8_t completion_mask[10]; ///< Bitmask of sphere sections (see http://en.wikipedia.org/wiki/Geodesic_grid) +} mavlink_mag_cal_progress_t; + +#define MAVLINK_MSG_ID_MAG_CAL_PROGRESS_LEN 27 +#define MAVLINK_MSG_ID_191_LEN 27 + +#define MAVLINK_MSG_ID_MAG_CAL_PROGRESS_CRC 92 +#define MAVLINK_MSG_ID_191_CRC 92 + +#define MAVLINK_MSG_MAG_CAL_PROGRESS_FIELD_COMPLETION_MASK_LEN 10 + +#define MAVLINK_MESSAGE_INFO_MAG_CAL_PROGRESS { \ + "MAG_CAL_PROGRESS", \ + 9, \ + { { "direction_x", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_mag_cal_progress_t, direction_x) }, \ + { "direction_y", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_mag_cal_progress_t, direction_y) }, \ + { "direction_z", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_mag_cal_progress_t, direction_z) }, \ + { "compass_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 12, offsetof(mavlink_mag_cal_progress_t, compass_id) }, \ + { "cal_mask", NULL, MAVLINK_TYPE_UINT8_T, 0, 13, offsetof(mavlink_mag_cal_progress_t, cal_mask) }, \ + { "cal_status", NULL, MAVLINK_TYPE_UINT8_T, 0, 14, offsetof(mavlink_mag_cal_progress_t, cal_status) }, \ + { "attempt", NULL, MAVLINK_TYPE_UINT8_T, 0, 15, offsetof(mavlink_mag_cal_progress_t, attempt) }, \ + { "completion_pct", NULL, MAVLINK_TYPE_UINT8_T, 0, 16, offsetof(mavlink_mag_cal_progress_t, completion_pct) }, \ + { "completion_mask", NULL, MAVLINK_TYPE_UINT8_T, 10, 17, offsetof(mavlink_mag_cal_progress_t, completion_mask) }, \ + } \ +} + + +/** + * @brief Pack a mag_cal_progress message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param compass_id Compass being calibrated + * @param cal_mask Bitmask of compasses being calibrated + * @param cal_status Status (see MAG_CAL_STATUS enum) + * @param attempt Attempt number + * @param completion_pct Completion percentage + * @param completion_mask Bitmask of sphere sections (see http://en.wikipedia.org/wiki/Geodesic_grid) + * @param direction_x Body frame direction vector for display + * @param direction_y Body frame direction vector for display + * @param direction_z Body frame direction vector for display + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_mag_cal_progress_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t compass_id, uint8_t cal_mask, uint8_t cal_status, uint8_t attempt, uint8_t completion_pct, const uint8_t *completion_mask, float direction_x, float direction_y, float direction_z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_MAG_CAL_PROGRESS_LEN]; + _mav_put_float(buf, 0, direction_x); + _mav_put_float(buf, 4, direction_y); + _mav_put_float(buf, 8, direction_z); + _mav_put_uint8_t(buf, 12, compass_id); + _mav_put_uint8_t(buf, 13, cal_mask); + _mav_put_uint8_t(buf, 14, cal_status); + _mav_put_uint8_t(buf, 15, attempt); + _mav_put_uint8_t(buf, 16, completion_pct); + _mav_put_uint8_t_array(buf, 17, completion_mask, 10); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MAG_CAL_PROGRESS_LEN); +#else + mavlink_mag_cal_progress_t packet; + packet.direction_x = direction_x; + packet.direction_y = direction_y; + packet.direction_z = direction_z; + packet.compass_id = compass_id; + packet.cal_mask = cal_mask; + packet.cal_status = cal_status; + packet.attempt = attempt; + packet.completion_pct = completion_pct; + mav_array_memcpy(packet.completion_mask, completion_mask, sizeof(uint8_t)*10); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MAG_CAL_PROGRESS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_MAG_CAL_PROGRESS; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MAG_CAL_PROGRESS_LEN, MAVLINK_MSG_ID_MAG_CAL_PROGRESS_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MAG_CAL_PROGRESS_LEN); +#endif +} + +/** + * @brief Pack a mag_cal_progress message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param compass_id Compass being calibrated + * @param cal_mask Bitmask of compasses being calibrated + * @param cal_status Status (see MAG_CAL_STATUS enum) + * @param attempt Attempt number + * @param completion_pct Completion percentage + * @param completion_mask Bitmask of sphere sections (see http://en.wikipedia.org/wiki/Geodesic_grid) + * @param direction_x Body frame direction vector for display + * @param direction_y Body frame direction vector for display + * @param direction_z Body frame direction vector for display + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_mag_cal_progress_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t compass_id,uint8_t cal_mask,uint8_t cal_status,uint8_t attempt,uint8_t completion_pct,const uint8_t *completion_mask,float direction_x,float direction_y,float direction_z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_MAG_CAL_PROGRESS_LEN]; + _mav_put_float(buf, 0, direction_x); + _mav_put_float(buf, 4, direction_y); + _mav_put_float(buf, 8, direction_z); + _mav_put_uint8_t(buf, 12, compass_id); + _mav_put_uint8_t(buf, 13, cal_mask); + _mav_put_uint8_t(buf, 14, cal_status); + _mav_put_uint8_t(buf, 15, attempt); + _mav_put_uint8_t(buf, 16, completion_pct); + _mav_put_uint8_t_array(buf, 17, completion_mask, 10); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MAG_CAL_PROGRESS_LEN); +#else + mavlink_mag_cal_progress_t packet; + packet.direction_x = direction_x; + packet.direction_y = direction_y; + packet.direction_z = direction_z; + packet.compass_id = compass_id; + packet.cal_mask = cal_mask; + packet.cal_status = cal_status; + packet.attempt = attempt; + packet.completion_pct = completion_pct; + mav_array_memcpy(packet.completion_mask, completion_mask, sizeof(uint8_t)*10); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MAG_CAL_PROGRESS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_MAG_CAL_PROGRESS; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MAG_CAL_PROGRESS_LEN, MAVLINK_MSG_ID_MAG_CAL_PROGRESS_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MAG_CAL_PROGRESS_LEN); +#endif +} + +/** + * @brief Encode a mag_cal_progress struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param mag_cal_progress C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_mag_cal_progress_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_mag_cal_progress_t* mag_cal_progress) +{ + return mavlink_msg_mag_cal_progress_pack(system_id, component_id, msg, mag_cal_progress->compass_id, mag_cal_progress->cal_mask, mag_cal_progress->cal_status, mag_cal_progress->attempt, mag_cal_progress->completion_pct, mag_cal_progress->completion_mask, mag_cal_progress->direction_x, mag_cal_progress->direction_y, mag_cal_progress->direction_z); +} + +/** + * @brief Encode a mag_cal_progress struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param mag_cal_progress C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_mag_cal_progress_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mag_cal_progress_t* mag_cal_progress) +{ + return mavlink_msg_mag_cal_progress_pack_chan(system_id, component_id, chan, msg, mag_cal_progress->compass_id, mag_cal_progress->cal_mask, mag_cal_progress->cal_status, mag_cal_progress->attempt, mag_cal_progress->completion_pct, mag_cal_progress->completion_mask, mag_cal_progress->direction_x, mag_cal_progress->direction_y, mag_cal_progress->direction_z); +} + +/** + * @brief Send a mag_cal_progress message + * @param chan MAVLink channel to send the message + * + * @param compass_id Compass being calibrated + * @param cal_mask Bitmask of compasses being calibrated + * @param cal_status Status (see MAG_CAL_STATUS enum) + * @param attempt Attempt number + * @param completion_pct Completion percentage + * @param completion_mask Bitmask of sphere sections (see http://en.wikipedia.org/wiki/Geodesic_grid) + * @param direction_x Body frame direction vector for display + * @param direction_y Body frame direction vector for display + * @param direction_z Body frame direction vector for display + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_mag_cal_progress_send(mavlink_channel_t chan, uint8_t compass_id, uint8_t cal_mask, uint8_t cal_status, uint8_t attempt, uint8_t completion_pct, const uint8_t *completion_mask, float direction_x, float direction_y, float direction_z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_MAG_CAL_PROGRESS_LEN]; + _mav_put_float(buf, 0, direction_x); + _mav_put_float(buf, 4, direction_y); + _mav_put_float(buf, 8, direction_z); + _mav_put_uint8_t(buf, 12, compass_id); + _mav_put_uint8_t(buf, 13, cal_mask); + _mav_put_uint8_t(buf, 14, cal_status); + _mav_put_uint8_t(buf, 15, attempt); + _mav_put_uint8_t(buf, 16, completion_pct); + _mav_put_uint8_t_array(buf, 17, completion_mask, 10); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MAG_CAL_PROGRESS, buf, MAVLINK_MSG_ID_MAG_CAL_PROGRESS_LEN, MAVLINK_MSG_ID_MAG_CAL_PROGRESS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MAG_CAL_PROGRESS, buf, MAVLINK_MSG_ID_MAG_CAL_PROGRESS_LEN); +#endif +#else + mavlink_mag_cal_progress_t packet; + packet.direction_x = direction_x; + packet.direction_y = direction_y; + packet.direction_z = direction_z; + packet.compass_id = compass_id; + packet.cal_mask = cal_mask; + packet.cal_status = cal_status; + packet.attempt = attempt; + packet.completion_pct = completion_pct; + mav_array_memcpy(packet.completion_mask, completion_mask, sizeof(uint8_t)*10); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MAG_CAL_PROGRESS, (const char *)&packet, MAVLINK_MSG_ID_MAG_CAL_PROGRESS_LEN, MAVLINK_MSG_ID_MAG_CAL_PROGRESS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MAG_CAL_PROGRESS, (const char *)&packet, MAVLINK_MSG_ID_MAG_CAL_PROGRESS_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_MAG_CAL_PROGRESS_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_mag_cal_progress_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t compass_id, uint8_t cal_mask, uint8_t cal_status, uint8_t attempt, uint8_t completion_pct, const uint8_t *completion_mask, float direction_x, float direction_y, float direction_z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, direction_x); + _mav_put_float(buf, 4, direction_y); + _mav_put_float(buf, 8, direction_z); + _mav_put_uint8_t(buf, 12, compass_id); + _mav_put_uint8_t(buf, 13, cal_mask); + _mav_put_uint8_t(buf, 14, cal_status); + _mav_put_uint8_t(buf, 15, attempt); + _mav_put_uint8_t(buf, 16, completion_pct); + _mav_put_uint8_t_array(buf, 17, completion_mask, 10); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MAG_CAL_PROGRESS, buf, MAVLINK_MSG_ID_MAG_CAL_PROGRESS_LEN, MAVLINK_MSG_ID_MAG_CAL_PROGRESS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MAG_CAL_PROGRESS, buf, MAVLINK_MSG_ID_MAG_CAL_PROGRESS_LEN); +#endif +#else + mavlink_mag_cal_progress_t *packet = (mavlink_mag_cal_progress_t *)msgbuf; + packet->direction_x = direction_x; + packet->direction_y = direction_y; + packet->direction_z = direction_z; + packet->compass_id = compass_id; + packet->cal_mask = cal_mask; + packet->cal_status = cal_status; + packet->attempt = attempt; + packet->completion_pct = completion_pct; + mav_array_memcpy(packet->completion_mask, completion_mask, sizeof(uint8_t)*10); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MAG_CAL_PROGRESS, (const char *)packet, MAVLINK_MSG_ID_MAG_CAL_PROGRESS_LEN, MAVLINK_MSG_ID_MAG_CAL_PROGRESS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MAG_CAL_PROGRESS, (const char *)packet, MAVLINK_MSG_ID_MAG_CAL_PROGRESS_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE MAG_CAL_PROGRESS UNPACKING + + +/** + * @brief Get field compass_id from mag_cal_progress message + * + * @return Compass being calibrated + */ +static inline uint8_t mavlink_msg_mag_cal_progress_get_compass_id(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 12); +} + +/** + * @brief Get field cal_mask from mag_cal_progress message + * + * @return Bitmask of compasses being calibrated + */ +static inline uint8_t mavlink_msg_mag_cal_progress_get_cal_mask(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 13); +} + +/** + * @brief Get field cal_status from mag_cal_progress message + * + * @return Status (see MAG_CAL_STATUS enum) + */ +static inline uint8_t mavlink_msg_mag_cal_progress_get_cal_status(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 14); +} + +/** + * @brief Get field attempt from mag_cal_progress message + * + * @return Attempt number + */ +static inline uint8_t mavlink_msg_mag_cal_progress_get_attempt(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 15); +} + +/** + * @brief Get field completion_pct from mag_cal_progress message + * + * @return Completion percentage + */ +static inline uint8_t mavlink_msg_mag_cal_progress_get_completion_pct(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 16); +} + +/** + * @brief Get field completion_mask from mag_cal_progress message + * + * @return Bitmask of sphere sections (see http://en.wikipedia.org/wiki/Geodesic_grid) + */ +static inline uint16_t mavlink_msg_mag_cal_progress_get_completion_mask(const mavlink_message_t* msg, uint8_t *completion_mask) +{ + return _MAV_RETURN_uint8_t_array(msg, completion_mask, 10, 17); +} + +/** + * @brief Get field direction_x from mag_cal_progress message + * + * @return Body frame direction vector for display + */ +static inline float mavlink_msg_mag_cal_progress_get_direction_x(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 0); +} + +/** + * @brief Get field direction_y from mag_cal_progress message + * + * @return Body frame direction vector for display + */ +static inline float mavlink_msg_mag_cal_progress_get_direction_y(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 4); +} + +/** + * @brief Get field direction_z from mag_cal_progress message + * + * @return Body frame direction vector for display + */ +static inline float mavlink_msg_mag_cal_progress_get_direction_z(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Decode a mag_cal_progress message into a struct + * + * @param msg The message to decode + * @param mag_cal_progress C-struct to decode the message contents into + */ +static inline void mavlink_msg_mag_cal_progress_decode(const mavlink_message_t* msg, mavlink_mag_cal_progress_t* mag_cal_progress) +{ +#if MAVLINK_NEED_BYTE_SWAP + mag_cal_progress->direction_x = mavlink_msg_mag_cal_progress_get_direction_x(msg); + mag_cal_progress->direction_y = mavlink_msg_mag_cal_progress_get_direction_y(msg); + mag_cal_progress->direction_z = mavlink_msg_mag_cal_progress_get_direction_z(msg); + mag_cal_progress->compass_id = mavlink_msg_mag_cal_progress_get_compass_id(msg); + mag_cal_progress->cal_mask = mavlink_msg_mag_cal_progress_get_cal_mask(msg); + mag_cal_progress->cal_status = mavlink_msg_mag_cal_progress_get_cal_status(msg); + mag_cal_progress->attempt = mavlink_msg_mag_cal_progress_get_attempt(msg); + mag_cal_progress->completion_pct = mavlink_msg_mag_cal_progress_get_completion_pct(msg); + mavlink_msg_mag_cal_progress_get_completion_mask(msg, mag_cal_progress->completion_mask); +#else + memcpy(mag_cal_progress, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_MAG_CAL_PROGRESS_LEN); +#endif +} diff --git a/libraries/GCS_MAVLink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mag_cal_report.h b/libraries/GCS_MAVLink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mag_cal_report.h new file mode 100644 index 0000000000000..f8056b7715cbd --- /dev/null +++ b/libraries/GCS_MAVLink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mag_cal_report.h @@ -0,0 +1,521 @@ +// MESSAGE MAG_CAL_REPORT PACKING + +#define MAVLINK_MSG_ID_MAG_CAL_REPORT 192 + +typedef struct __mavlink_mag_cal_report_t +{ + float fitness; ///< RMS milligauss residuals + float ofs_x; ///< X offset + float ofs_y; ///< Y offset + float ofs_z; ///< Z offset + float diag_x; ///< X diagonal (matrix 11) + float diag_y; ///< Y diagonal (matrix 22) + float diag_z; ///< Z diagonal (matrix 33) + float offdiag_x; ///< X off-diagonal (matrix 12 and 21) + float offdiag_y; ///< Y off-diagonal (matrix 13 and 31) + float offdiag_z; ///< Z off-diagonal (matrix 32 and 23) + uint8_t compass_id; ///< Compass being calibrated + uint8_t cal_mask; ///< Bitmask of compasses being calibrated + uint8_t cal_status; ///< Status (see MAG_CAL_STATUS enum) + uint8_t autosaved; ///< 0=requires a MAV_CMD_DO_ACCEPT_MAG_CAL, 1=saved to parameters +} mavlink_mag_cal_report_t; + +#define MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN 44 +#define MAVLINK_MSG_ID_192_LEN 44 + +#define MAVLINK_MSG_ID_MAG_CAL_REPORT_CRC 36 +#define MAVLINK_MSG_ID_192_CRC 36 + + + +#define MAVLINK_MESSAGE_INFO_MAG_CAL_REPORT { \ + "MAG_CAL_REPORT", \ + 14, \ + { { "fitness", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_mag_cal_report_t, fitness) }, \ + { "ofs_x", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_mag_cal_report_t, ofs_x) }, \ + { "ofs_y", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_mag_cal_report_t, ofs_y) }, \ + { "ofs_z", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_mag_cal_report_t, ofs_z) }, \ + { "diag_x", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_mag_cal_report_t, diag_x) }, \ + { "diag_y", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_mag_cal_report_t, diag_y) }, \ + { "diag_z", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_mag_cal_report_t, diag_z) }, \ + { "offdiag_x", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_mag_cal_report_t, offdiag_x) }, \ + { "offdiag_y", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_mag_cal_report_t, offdiag_y) }, \ + { "offdiag_z", NULL, MAVLINK_TYPE_FLOAT, 0, 36, offsetof(mavlink_mag_cal_report_t, offdiag_z) }, \ + { "compass_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 40, offsetof(mavlink_mag_cal_report_t, compass_id) }, \ + { "cal_mask", NULL, MAVLINK_TYPE_UINT8_T, 0, 41, offsetof(mavlink_mag_cal_report_t, cal_mask) }, \ + { "cal_status", NULL, MAVLINK_TYPE_UINT8_T, 0, 42, offsetof(mavlink_mag_cal_report_t, cal_status) }, \ + { "autosaved", NULL, MAVLINK_TYPE_UINT8_T, 0, 43, offsetof(mavlink_mag_cal_report_t, autosaved) }, \ + } \ +} + + +/** + * @brief Pack a mag_cal_report message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param compass_id Compass being calibrated + * @param cal_mask Bitmask of compasses being calibrated + * @param cal_status Status (see MAG_CAL_STATUS enum) + * @param autosaved 0=requires a MAV_CMD_DO_ACCEPT_MAG_CAL, 1=saved to parameters + * @param fitness RMS milligauss residuals + * @param ofs_x X offset + * @param ofs_y Y offset + * @param ofs_z Z offset + * @param diag_x X diagonal (matrix 11) + * @param diag_y Y diagonal (matrix 22) + * @param diag_z Z diagonal (matrix 33) + * @param offdiag_x X off-diagonal (matrix 12 and 21) + * @param offdiag_y Y off-diagonal (matrix 13 and 31) + * @param offdiag_z Z off-diagonal (matrix 32 and 23) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_mag_cal_report_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t compass_id, uint8_t cal_mask, uint8_t cal_status, uint8_t autosaved, float fitness, float ofs_x, float ofs_y, float ofs_z, float diag_x, float diag_y, float diag_z, float offdiag_x, float offdiag_y, float offdiag_z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN]; + _mav_put_float(buf, 0, fitness); + _mav_put_float(buf, 4, ofs_x); + _mav_put_float(buf, 8, ofs_y); + _mav_put_float(buf, 12, ofs_z); + _mav_put_float(buf, 16, diag_x); + _mav_put_float(buf, 20, diag_y); + _mav_put_float(buf, 24, diag_z); + _mav_put_float(buf, 28, offdiag_x); + _mav_put_float(buf, 32, offdiag_y); + _mav_put_float(buf, 36, offdiag_z); + _mav_put_uint8_t(buf, 40, compass_id); + _mav_put_uint8_t(buf, 41, cal_mask); + _mav_put_uint8_t(buf, 42, cal_status); + _mav_put_uint8_t(buf, 43, autosaved); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN); +#else + mavlink_mag_cal_report_t packet; + packet.fitness = fitness; + packet.ofs_x = ofs_x; + packet.ofs_y = ofs_y; + packet.ofs_z = ofs_z; + packet.diag_x = diag_x; + packet.diag_y = diag_y; + packet.diag_z = diag_z; + packet.offdiag_x = offdiag_x; + packet.offdiag_y = offdiag_y; + packet.offdiag_z = offdiag_z; + packet.compass_id = compass_id; + packet.cal_mask = cal_mask; + packet.cal_status = cal_status; + packet.autosaved = autosaved; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_MAG_CAL_REPORT; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN, MAVLINK_MSG_ID_MAG_CAL_REPORT_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN); +#endif +} + +/** + * @brief Pack a mag_cal_report message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param compass_id Compass being calibrated + * @param cal_mask Bitmask of compasses being calibrated + * @param cal_status Status (see MAG_CAL_STATUS enum) + * @param autosaved 0=requires a MAV_CMD_DO_ACCEPT_MAG_CAL, 1=saved to parameters + * @param fitness RMS milligauss residuals + * @param ofs_x X offset + * @param ofs_y Y offset + * @param ofs_z Z offset + * @param diag_x X diagonal (matrix 11) + * @param diag_y Y diagonal (matrix 22) + * @param diag_z Z diagonal (matrix 33) + * @param offdiag_x X off-diagonal (matrix 12 and 21) + * @param offdiag_y Y off-diagonal (matrix 13 and 31) + * @param offdiag_z Z off-diagonal (matrix 32 and 23) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_mag_cal_report_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t compass_id,uint8_t cal_mask,uint8_t cal_status,uint8_t autosaved,float fitness,float ofs_x,float ofs_y,float ofs_z,float diag_x,float diag_y,float diag_z,float offdiag_x,float offdiag_y,float offdiag_z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN]; + _mav_put_float(buf, 0, fitness); + _mav_put_float(buf, 4, ofs_x); + _mav_put_float(buf, 8, ofs_y); + _mav_put_float(buf, 12, ofs_z); + _mav_put_float(buf, 16, diag_x); + _mav_put_float(buf, 20, diag_y); + _mav_put_float(buf, 24, diag_z); + _mav_put_float(buf, 28, offdiag_x); + _mav_put_float(buf, 32, offdiag_y); + _mav_put_float(buf, 36, offdiag_z); + _mav_put_uint8_t(buf, 40, compass_id); + _mav_put_uint8_t(buf, 41, cal_mask); + _mav_put_uint8_t(buf, 42, cal_status); + _mav_put_uint8_t(buf, 43, autosaved); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN); +#else + mavlink_mag_cal_report_t packet; + packet.fitness = fitness; + packet.ofs_x = ofs_x; + packet.ofs_y = ofs_y; + packet.ofs_z = ofs_z; + packet.diag_x = diag_x; + packet.diag_y = diag_y; + packet.diag_z = diag_z; + packet.offdiag_x = offdiag_x; + packet.offdiag_y = offdiag_y; + packet.offdiag_z = offdiag_z; + packet.compass_id = compass_id; + packet.cal_mask = cal_mask; + packet.cal_status = cal_status; + packet.autosaved = autosaved; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_MAG_CAL_REPORT; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN, MAVLINK_MSG_ID_MAG_CAL_REPORT_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN); +#endif +} + +/** + * @brief Encode a mag_cal_report struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param mag_cal_report C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_mag_cal_report_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_mag_cal_report_t* mag_cal_report) +{ + return mavlink_msg_mag_cal_report_pack(system_id, component_id, msg, mag_cal_report->compass_id, mag_cal_report->cal_mask, mag_cal_report->cal_status, mag_cal_report->autosaved, mag_cal_report->fitness, mag_cal_report->ofs_x, mag_cal_report->ofs_y, mag_cal_report->ofs_z, mag_cal_report->diag_x, mag_cal_report->diag_y, mag_cal_report->diag_z, mag_cal_report->offdiag_x, mag_cal_report->offdiag_y, mag_cal_report->offdiag_z); +} + +/** + * @brief Encode a mag_cal_report struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param mag_cal_report C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_mag_cal_report_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mag_cal_report_t* mag_cal_report) +{ + return mavlink_msg_mag_cal_report_pack_chan(system_id, component_id, chan, msg, mag_cal_report->compass_id, mag_cal_report->cal_mask, mag_cal_report->cal_status, mag_cal_report->autosaved, mag_cal_report->fitness, mag_cal_report->ofs_x, mag_cal_report->ofs_y, mag_cal_report->ofs_z, mag_cal_report->diag_x, mag_cal_report->diag_y, mag_cal_report->diag_z, mag_cal_report->offdiag_x, mag_cal_report->offdiag_y, mag_cal_report->offdiag_z); +} + +/** + * @brief Send a mag_cal_report message + * @param chan MAVLink channel to send the message + * + * @param compass_id Compass being calibrated + * @param cal_mask Bitmask of compasses being calibrated + * @param cal_status Status (see MAG_CAL_STATUS enum) + * @param autosaved 0=requires a MAV_CMD_DO_ACCEPT_MAG_CAL, 1=saved to parameters + * @param fitness RMS milligauss residuals + * @param ofs_x X offset + * @param ofs_y Y offset + * @param ofs_z Z offset + * @param diag_x X diagonal (matrix 11) + * @param diag_y Y diagonal (matrix 22) + * @param diag_z Z diagonal (matrix 33) + * @param offdiag_x X off-diagonal (matrix 12 and 21) + * @param offdiag_y Y off-diagonal (matrix 13 and 31) + * @param offdiag_z Z off-diagonal (matrix 32 and 23) + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_mag_cal_report_send(mavlink_channel_t chan, uint8_t compass_id, uint8_t cal_mask, uint8_t cal_status, uint8_t autosaved, float fitness, float ofs_x, float ofs_y, float ofs_z, float diag_x, float diag_y, float diag_z, float offdiag_x, float offdiag_y, float offdiag_z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN]; + _mav_put_float(buf, 0, fitness); + _mav_put_float(buf, 4, ofs_x); + _mav_put_float(buf, 8, ofs_y); + _mav_put_float(buf, 12, ofs_z); + _mav_put_float(buf, 16, diag_x); + _mav_put_float(buf, 20, diag_y); + _mav_put_float(buf, 24, diag_z); + _mav_put_float(buf, 28, offdiag_x); + _mav_put_float(buf, 32, offdiag_y); + _mav_put_float(buf, 36, offdiag_z); + _mav_put_uint8_t(buf, 40, compass_id); + _mav_put_uint8_t(buf, 41, cal_mask); + _mav_put_uint8_t(buf, 42, cal_status); + _mav_put_uint8_t(buf, 43, autosaved); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MAG_CAL_REPORT, buf, MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN, MAVLINK_MSG_ID_MAG_CAL_REPORT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MAG_CAL_REPORT, buf, MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN); +#endif +#else + mavlink_mag_cal_report_t packet; + packet.fitness = fitness; + packet.ofs_x = ofs_x; + packet.ofs_y = ofs_y; + packet.ofs_z = ofs_z; + packet.diag_x = diag_x; + packet.diag_y = diag_y; + packet.diag_z = diag_z; + packet.offdiag_x = offdiag_x; + packet.offdiag_y = offdiag_y; + packet.offdiag_z = offdiag_z; + packet.compass_id = compass_id; + packet.cal_mask = cal_mask; + packet.cal_status = cal_status; + packet.autosaved = autosaved; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MAG_CAL_REPORT, (const char *)&packet, MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN, MAVLINK_MSG_ID_MAG_CAL_REPORT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MAG_CAL_REPORT, (const char *)&packet, MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_mag_cal_report_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t compass_id, uint8_t cal_mask, uint8_t cal_status, uint8_t autosaved, float fitness, float ofs_x, float ofs_y, float ofs_z, float diag_x, float diag_y, float diag_z, float offdiag_x, float offdiag_y, float offdiag_z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, fitness); + _mav_put_float(buf, 4, ofs_x); + _mav_put_float(buf, 8, ofs_y); + _mav_put_float(buf, 12, ofs_z); + _mav_put_float(buf, 16, diag_x); + _mav_put_float(buf, 20, diag_y); + _mav_put_float(buf, 24, diag_z); + _mav_put_float(buf, 28, offdiag_x); + _mav_put_float(buf, 32, offdiag_y); + _mav_put_float(buf, 36, offdiag_z); + _mav_put_uint8_t(buf, 40, compass_id); + _mav_put_uint8_t(buf, 41, cal_mask); + _mav_put_uint8_t(buf, 42, cal_status); + _mav_put_uint8_t(buf, 43, autosaved); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MAG_CAL_REPORT, buf, MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN, MAVLINK_MSG_ID_MAG_CAL_REPORT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MAG_CAL_REPORT, buf, MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN); +#endif +#else + mavlink_mag_cal_report_t *packet = (mavlink_mag_cal_report_t *)msgbuf; + packet->fitness = fitness; + packet->ofs_x = ofs_x; + packet->ofs_y = ofs_y; + packet->ofs_z = ofs_z; + packet->diag_x = diag_x; + packet->diag_y = diag_y; + packet->diag_z = diag_z; + packet->offdiag_x = offdiag_x; + packet->offdiag_y = offdiag_y; + packet->offdiag_z = offdiag_z; + packet->compass_id = compass_id; + packet->cal_mask = cal_mask; + packet->cal_status = cal_status; + packet->autosaved = autosaved; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MAG_CAL_REPORT, (const char *)packet, MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN, MAVLINK_MSG_ID_MAG_CAL_REPORT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MAG_CAL_REPORT, (const char *)packet, MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE MAG_CAL_REPORT UNPACKING + + +/** + * @brief Get field compass_id from mag_cal_report message + * + * @return Compass being calibrated + */ +static inline uint8_t mavlink_msg_mag_cal_report_get_compass_id(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 40); +} + +/** + * @brief Get field cal_mask from mag_cal_report message + * + * @return Bitmask of compasses being calibrated + */ +static inline uint8_t mavlink_msg_mag_cal_report_get_cal_mask(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 41); +} + +/** + * @brief Get field cal_status from mag_cal_report message + * + * @return Status (see MAG_CAL_STATUS enum) + */ +static inline uint8_t mavlink_msg_mag_cal_report_get_cal_status(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 42); +} + +/** + * @brief Get field autosaved from mag_cal_report message + * + * @return 0=requires a MAV_CMD_DO_ACCEPT_MAG_CAL, 1=saved to parameters + */ +static inline uint8_t mavlink_msg_mag_cal_report_get_autosaved(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 43); +} + +/** + * @brief Get field fitness from mag_cal_report message + * + * @return RMS milligauss residuals + */ +static inline float mavlink_msg_mag_cal_report_get_fitness(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 0); +} + +/** + * @brief Get field ofs_x from mag_cal_report message + * + * @return X offset + */ +static inline float mavlink_msg_mag_cal_report_get_ofs_x(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 4); +} + +/** + * @brief Get field ofs_y from mag_cal_report message + * + * @return Y offset + */ +static inline float mavlink_msg_mag_cal_report_get_ofs_y(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Get field ofs_z from mag_cal_report message + * + * @return Z offset + */ +static inline float mavlink_msg_mag_cal_report_get_ofs_z(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Get field diag_x from mag_cal_report message + * + * @return X diagonal (matrix 11) + */ +static inline float mavlink_msg_mag_cal_report_get_diag_x(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 16); +} + +/** + * @brief Get field diag_y from mag_cal_report message + * + * @return Y diagonal (matrix 22) + */ +static inline float mavlink_msg_mag_cal_report_get_diag_y(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 20); +} + +/** + * @brief Get field diag_z from mag_cal_report message + * + * @return Z diagonal (matrix 33) + */ +static inline float mavlink_msg_mag_cal_report_get_diag_z(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 24); +} + +/** + * @brief Get field offdiag_x from mag_cal_report message + * + * @return X off-diagonal (matrix 12 and 21) + */ +static inline float mavlink_msg_mag_cal_report_get_offdiag_x(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 28); +} + +/** + * @brief Get field offdiag_y from mag_cal_report message + * + * @return Y off-diagonal (matrix 13 and 31) + */ +static inline float mavlink_msg_mag_cal_report_get_offdiag_y(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 32); +} + +/** + * @brief Get field offdiag_z from mag_cal_report message + * + * @return Z off-diagonal (matrix 32 and 23) + */ +static inline float mavlink_msg_mag_cal_report_get_offdiag_z(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 36); +} + +/** + * @brief Decode a mag_cal_report message into a struct + * + * @param msg The message to decode + * @param mag_cal_report C-struct to decode the message contents into + */ +static inline void mavlink_msg_mag_cal_report_decode(const mavlink_message_t* msg, mavlink_mag_cal_report_t* mag_cal_report) +{ +#if MAVLINK_NEED_BYTE_SWAP + mag_cal_report->fitness = mavlink_msg_mag_cal_report_get_fitness(msg); + mag_cal_report->ofs_x = mavlink_msg_mag_cal_report_get_ofs_x(msg); + mag_cal_report->ofs_y = mavlink_msg_mag_cal_report_get_ofs_y(msg); + mag_cal_report->ofs_z = mavlink_msg_mag_cal_report_get_ofs_z(msg); + mag_cal_report->diag_x = mavlink_msg_mag_cal_report_get_diag_x(msg); + mag_cal_report->diag_y = mavlink_msg_mag_cal_report_get_diag_y(msg); + mag_cal_report->diag_z = mavlink_msg_mag_cal_report_get_diag_z(msg); + mag_cal_report->offdiag_x = mavlink_msg_mag_cal_report_get_offdiag_x(msg); + mag_cal_report->offdiag_y = mavlink_msg_mag_cal_report_get_offdiag_y(msg); + mag_cal_report->offdiag_z = mavlink_msg_mag_cal_report_get_offdiag_z(msg); + mag_cal_report->compass_id = mavlink_msg_mag_cal_report_get_compass_id(msg); + mag_cal_report->cal_mask = mavlink_msg_mag_cal_report_get_cal_mask(msg); + mag_cal_report->cal_status = mavlink_msg_mag_cal_report_get_cal_status(msg); + mag_cal_report->autosaved = mavlink_msg_mag_cal_report_get_autosaved(msg); +#else + memcpy(mag_cal_report, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN); +#endif +} diff --git a/libraries/GCS_MAVLink/include/mavlink/v1.0/ardupilotmega/testsuite.h b/libraries/GCS_MAVLink/include/mavlink/v1.0/ardupilotmega/testsuite.h index 5c9797d065ca8..014efc76e3193 100644 --- a/libraries/GCS_MAVLink/include/mavlink/v1.0/ardupilotmega/testsuite.h +++ b/libraries/GCS_MAVLink/include/mavlink/v1.0/ardupilotmega/testsuite.h @@ -1764,6 +1764,113 @@ static void mavlink_test_led_control(uint8_t system_id, uint8_t component_id, ma MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); } +static void mavlink_test_mag_cal_progress(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +{ + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_mag_cal_progress_t packet_in = { + 17.0,45.0,73.0,41,108,175,242,53,{ 120, 121, 122, 123, 124, 125, 126, 127, 128, 129 } + }; + mavlink_mag_cal_progress_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.direction_x = packet_in.direction_x; + packet1.direction_y = packet_in.direction_y; + packet1.direction_z = packet_in.direction_z; + packet1.compass_id = packet_in.compass_id; + packet1.cal_mask = packet_in.cal_mask; + packet1.cal_status = packet_in.cal_status; + packet1.attempt = packet_in.attempt; + packet1.completion_pct = packet_in.completion_pct; + + mav_array_memcpy(packet1.completion_mask, packet_in.completion_mask, sizeof(uint8_t)*10); + + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_mag_cal_progress_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_mag_cal_progress_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_mag_cal_progress_pack(system_id, component_id, &msg , packet1.compass_id , packet1.cal_mask , packet1.cal_status , packet1.attempt , packet1.completion_pct , packet1.completion_mask , packet1.direction_x , packet1.direction_y , packet1.direction_z ); + mavlink_msg_mag_cal_progress_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_mag_cal_progress_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.compass_id , packet1.cal_mask , packet1.cal_status , packet1.attempt , packet1.completion_pct , packet1.completion_mask , packet1.direction_x , packet1.direction_y , packet1.direction_z ); + mavlink_msg_mag_cal_progress_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iEmpty Empty + + + Initiate a magnetometer calibration + uint8_t bitmask of magnetometers (0 means all) + Automatically retry on failure (0=no retry, 1=retry). + Save without user input (0=require input, 1=autosave). + Delay (seconds) + Empty + Empty + Empty + + + + Initiate a magnetometer calibration + uint8_t bitmask of magnetometers (0 means all) + Empty + Empty + Empty + Empty + Empty + Empty + + + + Cancel a running magnetometer calibration + uint8_t bitmask of magnetometers (0 means all) + Empty + Empty + Empty + Empty + Empty + Empty + @@ -122,6 +155,15 @@ Custom Pattern using custom bytes fields + + + + + + + + + @@ -452,7 +494,7 @@ Camera Capture Feedback Image timestamp (microseconds since UNIX epoch), as passed in by CAMERA_STATUS message (or autopilot if no CCB) - System ID + System ID Camera ID Image index Latitude in (deg * 1E7) @@ -529,6 +571,37 @@ Custom Byte Length Custom Bytes + + + Reports progress of compass calibration. + Compass being calibrated + Bitmask of compasses being calibrated + Status (see MAG_CAL_STATUS enum) + Attempt number + Completion percentage + Bitmask of sphere sections (see http://en.wikipedia.org/wiki/Geodesic_grid) + Body frame direction vector for display + Body frame direction vector for display + Body frame direction vector for display + + + + Reports results of completed compass calibration. Sent until MAG_CAL_ACK received. + Compass being calibrated + Bitmask of compasses being calibrated + Status (see MAG_CAL_STATUS enum) + 0=requires a MAV_CMD_DO_ACCEPT_MAG_CAL, 1=saved to parameters + RMS milligauss residuals + X offset + Y offset + Z offset + X diagonal (matrix 11) + Y diagonal (matrix 22) + Z diagonal (matrix 33) + X off-diagonal (matrix 12 and 21) + Y off-diagonal (matrix 13 and 31) + Z off-diagonal (matrix 32 and 23) +