From 6aa663766e7827cdaffe62cc823cd3118fca1b95 Mon Sep 17 00:00:00 2001 From: bugobliterator Date: Wed, 27 Nov 2024 12:27:46 +1100 Subject: [PATCH] AP_Periph: initial add support for AHRS --- AP_Periph/AP_Periph.cpp | 11 ++- AP_Periph/AP_Periph.h | 8 +- AP_Periph/GPS_Base.cpp | 12 +-- AP_Periph/MAVLink.cpp | 192 ++++++++++++++++++++++++++++++++++++++- AP_Periph/MAVLink.h | 14 ++- AP_Periph/Parameters.cpp | 4 + AP_Periph/Parameters.h | 3 +- AP_Periph/sensor_imu.cpp | 24 ++++- AP_Periph/wscript | 5 + Here4/hwdef.dat | 8 +- HerePro/hwdef.dat | 12 ++- 11 files changed, 272 insertions(+), 21 deletions(-) diff --git a/AP_Periph/AP_Periph.cpp b/AP_Periph/AP_Periph.cpp index b81add1..a7f0385 100644 --- a/AP_Periph/AP_Periph.cpp +++ b/AP_Periph/AP_Periph.cpp @@ -141,7 +141,8 @@ void AP_Periph_FW::init() #if AP_INERTIALSENSOR_ENABLED if (g.imu_sample_rate) { imu.init(g.imu_sample_rate); - hal.scheduler->thread_create(FUNCTOR_BIND(dronecan, &AP_Periph_DroneCAN::can_imu_update, void), "IMU_UPDATE", 2048, AP_HAL::Scheduler::PRIORITY_CAN, 0); + ahrs.init(); + hal.scheduler->thread_create(FUNCTOR_BIND(dronecan, &AP_Periph_DroneCAN::can_imu_update, void), "IMU_UPDATE", 16384, AP_HAL::Scheduler::PRIORITY_CAN, 0); } #endif @@ -313,6 +314,14 @@ void AP_Periph_FW::update() mavlink.update(); } + static uint32_t tenhz_last_update_ms; + if (now - tenhz_last_update_ms >= 100) { + // update at 10Hz + tenhz_last_update_ms = now; + compass.send_mag_cal_progress(mavlink); + compass.send_mag_cal_report(mavlink); + } + rcout_update(); #ifdef I2C_SLAVE_ENABLED diff --git a/AP_Periph/AP_Periph.h b/AP_Periph/AP_Periph.h index 9bad860..57450cb 100644 --- a/AP_Periph/AP_Periph.h +++ b/AP_Periph/AP_Periph.h @@ -35,7 +35,7 @@ #include "GPS_Base.h" #include "GPS_Rover.h" #include - +#include #include "Parameters.h" @@ -246,6 +246,12 @@ class AP_Periph_FW { ObjectBuffer log_buffer{20}; ExpandingString uart_info; + + MAVLink_Periph* accel_cal_gcs = nullptr; + uint8_t accel_cal_sysid; + uint8_t accel_cal_compid; + bool initialise_accel_cal; + AP_AHRS ahrs; }; diff --git a/AP_Periph/GPS_Base.cpp b/AP_Periph/GPS_Base.cpp index 3a5a868..bcd6d9d 100644 --- a/AP_Periph/GPS_Base.cpp +++ b/AP_Periph/GPS_Base.cpp @@ -775,14 +775,14 @@ void GPS_Base::handle_param_request_list(const mavlink_message_t &msg) mavlink_param_request_list_t packet; mavlink_msg_param_request_list_decode(&msg, &packet); char key[AP_MAX_NAME_SIZE+1] = "FORMAT_VERSION"; - uint8_t index = 0; + uint8_t index = 1; ap_var_type var_type; // set format_version AP_Param *vp = AP_Param::find("FORMAT_VERSION", &var_type); if (vp == nullptr) { return; } - mavlink_msg_param_value_send(periph.mavlink.get_channel(), + mavlink_msg_param_value_send(periph.mavlink.get_chan(), key, vp->cast_to_float(var_type), mav_param_type(var_type), @@ -799,7 +799,7 @@ void GPS_Base::handle_param_request_list(const mavlink_message_t &msg) if (vp == nullptr) { continue; } - mavlink_msg_param_value_send(periph.mavlink.get_channel(), + mavlink_msg_param_value_send(periph.mavlink.get_chan(), key, vp->cast_to_float(var_type), mav_param_type(var_type), @@ -809,7 +809,7 @@ void GPS_Base::handle_param_request_list(const mavlink_message_t &msg) } else { // just send enable strcat(key, "_ENABLE"); - mavlink_msg_param_value_send(periph.mavlink.get_channel(), + mavlink_msg_param_value_send(periph.mavlink.get_chan(), key, (float)_enabled.get(), MAV_PARAM_TYPE_INT8, @@ -874,7 +874,7 @@ void GPS_Base::handle_param_set(const mavlink_message_t &msg) // send back the new value - mavlink_msg_param_value_send(periph.mavlink.get_channel(), + mavlink_msg_param_value_send(periph.mavlink.get_chan(), key, vp->cast_to_float(var_type), mav_param_type(var_type), @@ -908,7 +908,7 @@ void GPS_Base::handle_param_request_read(const mavlink_message_t &msg) float value = vp->cast_to_float(var_type); // send parameter value mavlink_msg_param_value_send( - periph.mavlink.get_channel(), + periph.mavlink.get_chan(), key, value, mav_param_type(var_type), diff --git a/AP_Periph/MAVLink.cpp b/AP_Periph/MAVLink.cpp index a81d27b..f15a660 100644 --- a/AP_Periph/MAVLink.cpp +++ b/AP_Periph/MAVLink.cpp @@ -256,7 +256,7 @@ void MAVLink_Periph::send_version() const mavlink_msg_autopilot_version_send( chan, - MAV_PROTOCOL_CAPABILITY_MAVLINK2, + MAV_PROTOCOL_CAPABILITY_MAVLINK2 | MAV_PROTOCOL_CAPABILITY_COMPASS_CALIBRATION, flight_sw_version, middleware_sw_version, version.os_sw_version, @@ -271,11 +271,28 @@ void MAVLink_Periph::send_version() const ); } +static void convert_command_long_to_command_int(const mavlink_command_long_t &packet_long, mavlink_command_int_t &packet_int) +{ + packet_int.target_system = packet_long.target_system; + packet_int.target_component = packet_long.target_component; + packet_int.command = packet_long.command; + packet_int.current = 0; + packet_int.autocontinue = 0; + packet_int.param1 = packet_long.param1; + packet_int.param2 = packet_long.param2; + packet_int.param3 = packet_long.param3; + packet_int.param4 = packet_long.param4; + packet_int.x = packet_long.param5; + packet_int.y = packet_long.param6; + packet_int.z = packet_long.param7; +} + void MAVLink_Periph::handle_command_long(const mavlink_message_t &msg) { // decode mavlink long mavlink_command_long_t packet; mavlink_msg_command_long_decode(&msg, &packet); + Debug("MAVLink_Periph::handle_command_long %d", packet.command); switch (packet.command) { case MAV_CMD_REQUEST_MESSAGE: if ((uint16_t)(packet.param1) == MAVLINK_MSG_ID_AUTOPILOT_VERSION) { @@ -287,11 +304,138 @@ void MAVLink_Periph::handle_command_long(const mavlink_message_t &msg) periph.prepare_reboot(); NVIC_SystemReset(); } + break; + case MAV_CMD_PREFLIGHT_CALIBRATION: + { + // convert to mavlink_command_int_t + mavlink_command_int_t packet_int = {}; + convert_command_long_to_command_int(packet, packet_int); + handle_command_preflight_calibration(packet_int, msg); + } + break; + case MAV_CMD_ACCELCAL_VEHICLE_POS: + { + // convert to mavlink_command_int_t + mavlink_command_int_t packet_int = {}; + convert_command_long_to_command_int(packet, packet_int); + handle_command_accelcal_vehicle_pos(packet_int, msg); + } + break; + + case MAV_CMD_DO_START_MAG_CAL: + case MAV_CMD_DO_ACCEPT_MAG_CAL: + case MAV_CMD_DO_CANCEL_MAG_CAL: + { + // convert to mavlink_command_int_t + mavlink_command_int_t packet_int = {}; + convert_command_long_to_command_int(packet, packet_int); + periph.compass.handle_mag_cal_command(packet_int); + } default: break; } } +void MAVLink_Periph::handle_command_int(const mavlink_message_t &msg) +{ + // decode mavlink int + mavlink_command_int_t packet; + mavlink_msg_command_int_decode(&msg, &packet); + switch (packet.command) { + case MAV_CMD_PREFLIGHT_CALIBRATION: + handle_command_preflight_calibration(packet, msg); + break; + case MAV_CMD_ACCELCAL_VEHICLE_POS: + handle_command_accelcal_vehicle_pos(packet, msg); + break; + case MAV_CMD_DO_START_MAG_CAL: + case MAV_CMD_DO_ACCEPT_MAG_CAL: + case MAV_CMD_DO_CANCEL_MAG_CAL: + periph.compass.handle_mag_cal_command(packet); + break; + default: + break; + } +} + +void MAVLink_Periph::handle_command_accelcal_vehicle_pos(const mavlink_command_int_t &packet, const mavlink_message_t &msg) +{ + MAV_RESULT result = MAV_RESULT_ACCEPTED; + if (AP::ins().get_acal() == nullptr || + !AP::ins().get_acal()->gcs_vehicle_position(packet.param1)) { + result = MAV_RESULT_FAILED; + } + + mavlink_msg_command_ack_send(chan, packet.command, result, + 0, 0, + msg.sysid, + msg.compid); + +} + +void GCS_MAVLINK::handle_command_ack(const mavlink_message_t &msg) +{ + mavlink_command_ack_t packet; + mavlink_msg_command_ack_decode(&msg, &packet); + + AP_AccelCal *accelcal = AP::ins().get_acal(); + if (accelcal != nullptr) { + accelcal->handle_command_ack(packet); + } +} + +void MAVLink_Periph::handle_command_preflight_calibration(const mavlink_command_int_t &packet, const mavlink_message_t &msg) +{ + if (packet.x == 1) { + periph.accel_cal_sysid = msg.sysid; + periph.accel_cal_compid = msg.compid; + periph.accel_cal_gcs = this; + periph.initialise_accel_cal = true; + } else { + mavlink_msg_command_ack_send(chan, packet.command, MAV_RESULT_UNSUPPORTED, + 0, 0, + msg.sysid, + msg.compid); + } +} + + +static MAV_PARAM_TYPE mav_param_type(enum ap_var_type t) +{ + if (t == AP_PARAM_INT8) { + return MAV_PARAM_TYPE_INT8; + } + if (t == AP_PARAM_INT16) { + return MAV_PARAM_TYPE_INT16; + } + if (t == AP_PARAM_INT32) { + return MAV_PARAM_TYPE_INT32; + } + // treat any others as float + return MAV_PARAM_TYPE_REAL32; +} + +uint16_t MAVLink_Periph::send_compass_params() +{ + uint16_t index = 0; + // find all compass parameters + AP_Param *vp; + ap_var_type type; + char name[16]; + do { + AP_Param::ParamToken token {}; + vp = AP_Param::find_by_index(index, &type, &token); + // check if we have a compass parameter + if (vp != nullptr) { + vp->copy_name_token(token, name, sizeof(name)); + if (strncmp(name, "COMPASS_", 8) == 0) { + mavlink_msg_param_value_send(chan, name, vp->cast_to_float(type), mav_param_type(vp->3,,,), vp->get_size(), index); + } + } + } while(vp != nullptr); + return index; +} + void MAVLink_Periph::handleMessage(const mavlink_message_t &msg) { Debug("MAVLink_Periph::handleMessage %d", msg.msgid); @@ -305,10 +449,16 @@ void MAVLink_Periph::handleMessage(const mavlink_message_t &msg) case MAVLINK_MSG_ID_HEARTBEAT: handle_odid_heartbeat(msg); break; + case MAVLINK_MSG_ID_COMMAND_ACK: + handle_command_ack(msg); + break; case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: + { + uint32_t index = send_compass_params(); #ifdef ENABLE_BASE_MODE - periph.gps_base.handle_param_request_list(msg); + periph.gps_base.handle_param_request_list(msg, index); #endif + } break; case MAVLINK_MSG_ID_PARAM_SET: #ifdef ENABLE_BASE_MODE @@ -323,6 +473,9 @@ void MAVLink_Periph::handleMessage(const mavlink_message_t &msg) case MAVLINK_MSG_ID_COMMAND_LONG: handle_command_long(msg); break; + case MAVLINK_MSG_ID_COMMAND_INT: + handle_command_int(msg); + break; case MAVLINK_MSG_ID_AUTOPILOT_VERSION_REQUEST: send_version(); break; @@ -331,6 +484,38 @@ void MAVLink_Periph::handleMessage(const mavlink_message_t &msg) } // end switch } +/* + send a text message to GCS + */ +void MAVLink_Periph::send_textv(MAV_SEVERITY severity, const char *fmt, va_list arg_list) +{ + static uint16_t msgid = 0; + char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN] = {}; + vsnprintf(buf, sizeof(buf), fmt, arg_list); + msgid++; + mavlink_msg_statustext_send(chan, severity, buf, msgid, 0); +} + +void MAVLink_Periph::send_text(MAV_SEVERITY severity, const char *fmt, ...) +{ + va_list arg_list; + va_start(arg_list, fmt); + send_textv(severity, fmt, arg_list); + va_end(arg_list); +} + +void MAVLink_Periph::send_accelcal_vehicle_position(uint32_t position) +{ + mavlink_msg_command_long_send( + chan, + 0, + 0, + MAV_CMD_ACCELCAL_VEHICLE_POS, + 0, + (float) position, + 0, 0, 0, 0, 0, 0); +} + bool gcs_alternative_active[MAVLINK_COMM_NUM_BUFFERS]; // per-channel lock @@ -411,8 +596,7 @@ void comm_send_unlock(mavlink_channel_t chan_m) } /* - return reference to GCS channel lock, allowing for - HAVE_PAYLOAD_SPACE() to be run with a locked channel + return reference to GCS channel lock */ HAL_Semaphore &comm_chan_lock(mavlink_channel_t chan) { diff --git a/AP_Periph/MAVLink.h b/AP_Periph/MAVLink.h index efe2460..917f8c8 100644 --- a/AP_Periph/MAVLink.h +++ b/AP_Periph/MAVLink.h @@ -17,6 +17,7 @@ #include #include +#include /* * GCS backend used for many examples and tools @@ -32,15 +33,26 @@ class MAVLink_Periph uint32_t write(const uint8_t *tbuf, uint32_t len); mavlink_message_t* channel_buffer() { return &chan_buffer; } mavlink_status_t* channel_status() { return &chan_status; } - mavlink_channel_t get_channel() const { return chan; } + mavlink_channel_t get_chan() const { return chan; } bool process_byte(const uint8_t c); void send_version() const; + + void send_text(MAV_SEVERITY severity, const char *fmt, ...) FMT_PRINTF(3, 4); + void send_textv(MAV_SEVERITY severity, const char *fmt, va_list arg_list); + void send_accelcal_vehicle_position(uint32_t position); + private: void handleMessage(const mavlink_message_t &msg); MAV_RESULT handle_preflight_reboot(const mavlink_command_long_t &packet); void handle_open_drone_id_arm_status(const mavlink_message_t &msg); void handle_command_long(const mavlink_message_t &msg); + void handle_command_int(const mavlink_message_t &msg); + void handle_command_preflight_calibration(const mavlink_command_int_t &packet, const mavlink_message_t &msg); + void handle_command_accelcal_vehicle_pos(const mavlink_command_int_t &packet, const mavlink_message_t &msg); + void handle_command_ack(const mavlink_message_t &msg); + uint16_t send_compass_params(); + uint8_t sysid_my_gcs() const; void handle_cubepilot_firmware_update_resp(const mavlink_message_t &msg); diff --git a/AP_Periph/Parameters.cpp b/AP_Periph/Parameters.cpp index 0878c5d..6a75954 100644 --- a/AP_Periph/Parameters.cpp +++ b/AP_Periph/Parameters.cpp @@ -218,6 +218,10 @@ const AP_Param::Info AP_Periph_FW::var_info[] = { GOBJECT(gps_rover, "ROVER", GPS_Rover), #endif + // @Group: AHRS + // @Path: ardupilot/libraries/AP_AHRS/AP_AHRS.cpp + GOBJECT(ahrs, "AHRS_", AP_AHRS), + AP_VAREND }; diff --git a/AP_Periph/Parameters.h b/AP_Periph/Parameters.h index 99dc90a..b776af5 100644 --- a/AP_Periph/Parameters.h +++ b/AP_Periph/Parameters.h @@ -59,7 +59,8 @@ class Parameters { k_param_imu, k_param_gps_safeboot, k_param_gps_rover, - k_param_gps_type + k_param_gps_type, + k_param_ahrs, }; AP_Int16 format_version; diff --git a/AP_Periph/sensor_imu.cpp b/AP_Periph/sensor_imu.cpp index ef37b77..c1b38a2 100644 --- a/AP_Periph/sensor_imu.cpp +++ b/AP_Periph/sensor_imu.cpp @@ -8,17 +8,31 @@ void AP_Periph_DroneCAN::can_imu_update(void) #if AP_INERTIALSENSOR_ENABLED auto &imu = periph.imu; while (true) { - imu.update(); + if (periph.accel_cal_gcs && periph.initialise_accel_cal) { + can_printf("Starting accel cal\n"); + // start with gyro calibration + if (!imu.calibrate_gyros()) { + periph.initialise_accel_cal = false; + } + // start accel cal + imu.acal_init(); + imu.get_acal()->start(periph.accel_cal_gcs); + periph.initialise_accel_cal = false; + mavlink_msg_command_ack_send(periph.accel_cal_gcs->get_chan(), MAV_CMD_PREFLIGHT_CALIBRATION, MAV_RESULT_ACCEPTED, + 0, 0, + periph.accel_cal_sysid, + periph.accel_cal_compid); + } + if (periph.accel_cal_gcs) { + imu.acal_update(); + } + periph.ahrs.update(); if (!imu.healthy()) { continue; } uavcan_equipment_ahrs_RawIMU pkt {}; - if (imu.get_last_update_usec() == periph.last_imu_update_usec) { - return; - } - Vector3f tmp; imu.get_delta_velocity(tmp, pkt.integration_interval); pkt.accelerometer_integral[0] = tmp.x; diff --git a/AP_Periph/wscript b/AP_Periph/wscript index bdc0c4e..73df475 100644 --- a/AP_Periph/wscript +++ b/AP_Periph/wscript @@ -71,6 +71,11 @@ def build(bld): 'AP_Logger', 'AC_PID', 'AP_CheckFirmware', + 'AP_AHRS', + 'AP_NavEKF3', + 'AP_NavEKF', + 'AP_DAL', + 'AP_Declination' ] bld.ap_stlib( name= 'AP_Periph_libs', diff --git a/Here4/hwdef.dat b/Here4/hwdef.dat index de0702a..20cca63 100644 --- a/Here4/hwdef.dat +++ b/Here4/hwdef.dat @@ -90,7 +90,6 @@ define HAL_USE_ADC FALSE define STM32_ADC_USE_ADC1 FALSE define HAL_DISABLE_ADC_DRIVER TRUE -define HAL_INS_ACCELCAL_ENABLED FALSE define HAL_PERIPH_ENABLE_BARO TRUE SPIDEV rm3100 SPI3 DEVID1 MAG_CS MODE0 1*MHZ 1*MHZ @@ -153,3 +152,10 @@ define AP_SERIALLED_ENABLED 1 #define AP_SCRIPTING_ENABLED 1 #define HAL_EFI_ENABLED 0 define HAL_GCS_ENABLED 0 + +define HAL_PERIPH_ENABLE_AHRS 1 +define AP_BEACON_ENABLED 0 +define AP_WHEELENCODER_ENABLED 0 +define HAL_MAVLINK_ENABLED 1 +define HAL_INS_ACCELCAL_ENABLED 1 +define COMPASS_CAL_ENABLED 1 diff --git a/HerePro/hwdef.dat b/HerePro/hwdef.dat index 30248f1..96e74e1 100644 --- a/HerePro/hwdef.dat +++ b/HerePro/hwdef.dat @@ -84,7 +84,6 @@ define NOTIFY_LED_LEN_DEFAULT 51 define CONFIGURE_PPS_PIN TRUE define HAL_PERIPH_ENABLE_NOTIFY define HAL_PERIPH_ENABLE_RC_OUT -define AP_INERTIALSENSOR_ENABLED 0 FLASH_RESERVE_START_KB 256 @@ -251,3 +250,14 @@ define HAL_UART_MIN_TX_SIZE 8192 define HAL_UART_MIN_RX_SIZE 8192 define HAL_UART_STATS_ENABLED 1 + +define HAL_PERIPH_ENABLE_AHRS 1 +define AP_BEACON_ENABLED 0 +define AP_WHEELENCODER_ENABLED 0 +define HAL_MAVLINK_ENABLED 1 +define HAL_INS_ACCELCAL_ENABLED 1 +define COMPASS_CAL_ENABLED 1 +define AP_AIRSPEED_ENABLED 0 +define AP_INERTIALSENSOR_ENABLED 1 +define AP_AHRS_DCM_ENABLED 1 +define AP_COMPASS_DIAGONALS_ENABLED 1