Skip to content

Commit

Permalink
AP_Periph: initial add support for AHRS
Browse files Browse the repository at this point in the history
  • Loading branch information
@bugobliterator
bugobliterator committed Nov 27, 2024
1 parent d42d333 commit 6aa6637
Show file tree
Hide file tree
Showing 11 changed files with 272 additions and 21 deletions.
11 changes: 10 additions & 1 deletion AP_Periph/AP_Periph.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand Down Expand Up @@ -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
Expand Down
8 changes: 7 additions & 1 deletion AP_Periph/AP_Periph.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -35,7 +35,7 @@
#include "GPS_Base.h"
#include "GPS_Rover.h"
#include <AP_Common/ExpandingString.h>

#include <AP_AHRS/AP_AHRS.h>

#include "Parameters.h"

Expand Down Expand Up @@ -246,6 +246,12 @@ class AP_Periph_FW {
ObjectBuffer<uavcan_protocol_debug_LogMessage> 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;
};


Expand Down
12 changes: 6 additions & 6 deletions AP_Periph/GPS_Base.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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),
Expand All @@ -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),
Expand All @@ -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,
Expand Down Expand Up @@ -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),
Expand Down Expand Up @@ -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),
Expand Down
192 changes: 188 additions & 4 deletions AP_Periph/MAVLink.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand All @@ -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) {
Expand All @@ -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);
Expand All @@ -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
Expand All @@ -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;
Expand All @@ -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
Expand Down Expand Up @@ -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)
{
Expand Down
14 changes: 13 additions & 1 deletion AP_Periph/MAVLink.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,6 +17,7 @@
#include <AP_HAL/AP_HAL_Boards.h>

#include <GCS_MAVLink/GCS_MAVLink.h>
#include <AP_Param/AP_Param.h>

/*
* GCS backend used for many examples and tools
Expand All @@ -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);
Expand Down
Loading

0 comments on commit 6aa6637

Please sign in to comment.