corner_pin_node_t::corner_pin_node_t() : xform2d_node_t()
{
set_name("corner_pin");
param_set().param_changed.connect( boost::bind( &corner_pin_node_t::param_changed, this, _1, _2));
}
corner_pin_node_t::corner_pin_node_t( const corner_pin_node_t& other) : xform2d_node_t( other)
{
param_set().param_changed.connect( boost::bind( &corner_pin_node_t::param_changed, this, _1, _2));
}
int do_gyro_calibration(int mavlink_fd)
{
mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
mavlink_log_info(mavlink_fd, "don't move system");
struct gyro_scale gyro_scale = {
0.0f,
1.0f,
0.0f,
1.0f,
0.0f,
1.0f,
};
int res = OK;
/* reset all offsets to zero and all scales to one */
int fd = open(GYRO_DEVICE_PATH, 0);
res = ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale);
close(fd);
if (res != OK) {
mavlink_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG);
}
if (res == OK) {
/* determine gyro mean values */
const unsigned calibration_count = 5000;
unsigned calibration_counter = 0;
unsigned poll_errcount = 0;
/* subscribe to gyro sensor topic */
int sub_sensor_gyro = orb_subscribe(ORB_ID(sensor_gyro0));
struct gyro_report gyro_report;
while (calibration_counter < calibration_count) {
/* wait blocking for new data */
struct pollfd fds[1];
fds[0].fd = sub_sensor_gyro;
fds[0].events = POLLIN;
int poll_ret = poll(fds, 1, 1000);
if (poll_ret > 0) {
orb_copy(ORB_ID(sensor_gyro0), sub_sensor_gyro, &gyro_report);
gyro_scale.x_offset += gyro_report.x;
gyro_scale.y_offset += gyro_report.y;
gyro_scale.z_offset += gyro_report.z;
calibration_counter++;
if (calibration_counter % (calibration_count / 20) == 0) {
mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter * 100) / calibration_count);
}
} else {
poll_errcount++;
}
if (poll_errcount > 1000) {
mavlink_log_critical(mavlink_fd, CAL_FAILED_SENSOR_MSG);
res = ERROR;
break;
}
}
close(sub_sensor_gyro);
gyro_scale.x_offset /= calibration_count;
gyro_scale.y_offset /= calibration_count;
gyro_scale.z_offset /= calibration_count;
}
if (res == OK) {
/* check offsets */
if (!isfinite(gyro_scale.x_offset) || !isfinite(gyro_scale.y_offset) || !isfinite(gyro_scale.z_offset)) {
mavlink_log_critical(mavlink_fd, "ERROR: offset is NaN");
res = ERROR;
}
}
if (res == OK) {
/* set offset parameters to new values */
if (param_set(param_find("SENS_GYRO_XOFF"), &(gyro_scale.x_offset))
|| param_set(param_find("SENS_GYRO_YOFF"), &(gyro_scale.y_offset))
|| param_set(param_find("SENS_GYRO_ZOFF"), &(gyro_scale.z_offset))) {
mavlink_log_critical(mavlink_fd, "ERROR: failed to set offset params");
res = ERROR;
}
}
#if 0
/* beep on offset calibration end */
mavlink_log_info(mavlink_fd, "gyro offset calibration done");
tune_neutral();
/* scale calibration */
/* this was only a proof of concept and is currently not working. scaling will be set to 1.0 for now. */
mavlink_log_info(mavlink_fd, "offset done. Rotate for scale 30x or wait 5s to skip.");
warnx("offset calibration finished. Rotate for scale 30x, or do not rotate and wait for 5 seconds to skip.");
/* apply new offsets */
fd = open(GYRO_DEVICE_PATH, 0);
if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale)) {
warn("WARNING: failed to apply new offsets for gyro");
}
close(fd);
unsigned rotations_count = 30;
float gyro_integral = 0.0f;
float baseline_integral = 0.0f;
// XXX change to mag topic
orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw);
float mag_last = -atan2f(raw.magnetometer_ga[1], raw.magnetometer_ga[0]);
if (mag_last > M_PI_F) { mag_last -= 2 * M_PI_F; }
if (mag_last < -M_PI_F) { mag_last += 2 * M_PI_F; }
uint64_t last_time = hrt_absolute_time();
uint64_t start_time = hrt_absolute_time();
while ((int)fabsf(baseline_integral / (2.0f * M_PI_F)) < rotations_count) {
/* abort this loop if not rotated more than 180 degrees within 5 seconds */
if ((fabsf(baseline_integral / (2.0f * M_PI_F)) < 0.6f)
&& (hrt_absolute_time() - start_time > 5 * 1e6)) {
mavlink_log_info(mavlink_fd, "scale skipped, gyro calibration done");
close(sub_sensor_combined);
return OK;
}
/* wait blocking for new data */
struct pollfd fds[1];
fds[0].fd = sub_sensor_combined;
fds[0].events = POLLIN;
int poll_ret = poll(fds, 1, 1000);
if (poll_ret) {
float dt_ms = (hrt_absolute_time() - last_time) / 1e3f;
last_time = hrt_absolute_time();
orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw);
// XXX this is just a proof of concept and needs world / body
// transformation and more
//math::Vector2f magNav(raw.magnetometer_ga);
// calculate error between estimate and measurement
// apply declination correction for true heading as well.
//float mag = -atan2f(magNav(1),magNav(0));
float mag = -atan2f(raw.magnetometer_ga[1], raw.magnetometer_ga[0]);
if (mag > M_PI_F) { mag -= 2 * M_PI_F; }
if (mag < -M_PI_F) { mag += 2 * M_PI_F; }
float diff = mag - mag_last;
if (diff > M_PI_F) { diff -= 2 * M_PI_F; }
if (diff < -M_PI_F) { diff += 2 * M_PI_F; }
baseline_integral += diff;
mag_last = mag;
// Jump through some timing scale hoops to avoid
// operating near the 1e6/1e8 max sane resolution of float.
gyro_integral += (raw.gyro_rad_s[2] * dt_ms) / 1e3f;
// warnx("dbg: b: %6.4f, g: %6.4f", (double)baseline_integral, (double)gyro_integral);
// } else if (poll_ret == 0) {
// /* any poll failure for 1s is a reason to abort */
// mavlink_log_info(mavlink_fd, "gyro calibration aborted, retry");
// return;
}
}
float gyro_scale = baseline_integral / gyro_integral;
warnx("gyro scale: yaw (z): %6.4f", (double)gyro_scale);
mavlink_log_info(mavlink_fd, "gyro scale: yaw (z): %6.4f", (double)gyro_scale);
if (!isfinite(gyro_scale.x_scale) || !isfinite(gyro_scale.y_scale) || !isfinite(gyro_scale.z_scale)) {
mavlink_log_info(mavlink_fd, "gyro scale calibration FAILED (NaN)");
close(sub_sensor_gyro);
mavlink_log_critical(mavlink_fd, "gyro calibration failed");
return ERROR;
}
/* beep on calibration end */
mavlink_log_info(mavlink_fd, "gyro scale calibration done");
tune_neutral();
#endif
if (res == OK) {
/* set scale parameters to new values */
if (param_set(param_find("SENS_GYRO_XSCALE"), &(gyro_scale.x_scale))
|| param_set(param_find("SENS_GYRO_YSCALE"), &(gyro_scale.y_scale))
|| param_set(param_find("SENS_GYRO_ZSCALE"), &(gyro_scale.z_scale))) {
mavlink_log_critical(mavlink_fd, "ERROR: failed to set scale params");
res = ERROR;
}
}
if (res == OK) {
/* apply new scaling and offsets */
fd = open(GYRO_DEVICE_PATH, 0);
res = ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale);
close(fd);
if (res != OK) {
mavlink_log_critical(mavlink_fd, CAL_FAILED_APPLY_CAL_MSG);
}
}
if (res == OK) {
/* auto-save to EEPROM */
res = param_save_default();
if (res != OK) {
mavlink_log_critical(mavlink_fd, CAL_FAILED_SAVE_PARAMS_MSG);
}
}
if (res == OK) {
mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name);
} else {
mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name);
}
return res;
}
void Ekf2Replay::task_main()
{
// formats
const int _k_max_data_size = 1024; // 16x16 bytes
uint8_t data[_k_max_data_size] = {};
const char param_file[] = "./rootfs/replay_params.txt";
// Open log file from which we read data
// TODO Check if file exists
int fd = ::open(_file_name, O_RDONLY);
// create path to write a replay file
char *replay_log_name;
replay_log_name = strtok(_file_name, ".");
char tmp[] = "_replayed.px4log";
char *path_to_replay_log = (char *) malloc(1 + strlen(tmp) + strlen(replay_log_name));
strcpy(path_to_replay_log, ".");
strcat(path_to_replay_log, replay_log_name);
strcat(path_to_replay_log, tmp);
// create path which tells user location of replay file
char tmp2[] = "./build_posix_sitl_replay/src/firmware/posix";
char *replay_file_location = (char *) malloc(1 + strlen(tmp) + strlen(tmp2) + strlen(replay_log_name));
strcat(replay_file_location, tmp2);
strcat(replay_file_location, replay_log_name);
strcat(replay_file_location, tmp);
// open logfile to write
_write_fd = ::open(path_to_replay_log, O_WRONLY | O_CREAT, S_IRWXU);
std::ifstream tmp_file;
tmp_file.open(param_file);
std::string line;
bool set_default_params_in_file = false;
if (tmp_file.is_open() && ! tmp_file.eof()) {
getline(tmp_file, line);
if (line.empty()) {
// the parameter file is empty so write the default values to it
set_default_params_in_file = true;
}
}
tmp_file.close();
std::ofstream myfile(param_file, std::ios::app);
// subscribe to estimator topics
_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
_estimator_status_sub = orb_subscribe(ORB_ID(estimator_status));
_innov_sub = orb_subscribe(ORB_ID(ekf2_innovations));
_lpos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
_control_state_sub = orb_subscribe(ORB_ID(control_state));
// we use attitude updates from the estimator for synchronisation
_fds[0].fd = _att_sub;
_fds[0].events = POLLIN;
bool read_first_header = false;
bool set_user_params = false;
PX4_INFO("Replay in progress... \n");
PX4_INFO("Log data will be written to %s\n", replay_file_location);
while (!_task_should_exit) {
_message_counter++;
uint8_t header[3] = {};
if (::read(fd, header, 3) != 3) {
if (!read_first_header) {
PX4_WARN("error reading log file, is the path printed above correct?");
} else {
PX4_INFO("Done!");
}
_task_should_exit = true;
continue;
}
read_first_header = true;
if ((header[0] != HEAD_BYTE1 || header[1] != HEAD_BYTE2)) {
// we assume that the log file is finished here
PX4_WARN("Done!");
_task_should_exit = true;
continue;
}
// write header but only for messages which are not generated by the estimator
if (needToSaveMessage(header[2])) {
writeMessage(_write_fd, &header[0], 3);
}
if (header[2] == LOG_FORMAT_MSG) {
// format message
struct log_format_s f;
if (::read(fd, &f.type, sizeof(f)) != sizeof(f)) {
PRINT_READ_ERROR;
_task_should_exit = true;
continue;
}
writeMessage(_write_fd, &f.type, sizeof(log_format_s));
memcpy(&_formats[f.type], &f, sizeof(f));
} else if (header[2] == LOG_PARM_MSG) {
// parameter message
if (::read(fd, &data[0], sizeof(log_PARM_s)) != sizeof(log_PARM_s)) {
PRINT_READ_ERROR;
_task_should_exit = true;
continue;
}
writeMessage(_write_fd, &data[0], sizeof(log_PARM_s));
// apply the parameters
char param_name[16];
for (unsigned i = 0; i < 16; i++) {
param_name[i] = data[i];
if (data[i] == '\0') {
break;
}
}
float param_data = 0;
memcpy(¶m_data, &data[16], sizeof(float));
param_t handle = param_find(param_name);
param_type_t param_format = param_type(handle);
if (param_format == PARAM_TYPE_INT32) {
int32_t value = 0;
value = (int32_t)param_data;
param_set(handle, (const void *)&value);
} else if (param_format == PARAM_TYPE_FLOAT) {
param_set(handle, (const void *)¶m_data);
}
// if the user param file was empty then we fill it with the ekf2 parameter values from
// the log file
if (set_default_params_in_file) {
if (strncmp(param_name, "EKF2", 4) == 0) {
std::ostringstream os;
double value = (double)param_data;
os << std::string(param_name) << " ";
os << value << "\n";
myfile << os.str();
}
}
} else if (header[2] == LOG_VER_MSG) {
// version message
if (::read(fd, &data[0], sizeof(log_VER_s)) != sizeof(log_VER_s)) {
PRINT_READ_ERROR;
_task_should_exit = true;
continue;
}
writeMessage(_write_fd, &data[0], sizeof(log_VER_s));
} else if (header[2] == LOG_TIME_MSG) {
// time message
if (::read(fd, &data[0], sizeof(log_TIME_s)) != sizeof(log_TIME_s)) {
// assume that this is because we have reached the end of the file
PX4_INFO("Done!");
_task_should_exit = true;
continue;
}
writeMessage(_write_fd, &data[0], sizeof(log_TIME_s));
} else {
// the first time we arrive here we should apply the parameters specified in the user file
// this makes sure they are applied after the parameter values of the log file
if (!set_user_params) {
myfile.close();
setUserParams(param_file);
set_user_params = true;
}
// data message
if (::read(fd, &data[0], _formats[header[2]].length - 3) != _formats[header[2]].length - 3) {
PX4_INFO("Done!");
_task_should_exit = true;
continue;
}
// all messages which we are not getting from the estimator are written
// back into the replay log file
if (needToSaveMessage(header[2])) {
writeMessage(_write_fd, &data[0], _formats[header[2]].length - 3);
}
if (header[2] == LOG_RPL1_MSG && _part1_counter_ref > 0) {
// we have found another imu replay message while we still have one waiting to be published.
// so publish that now
publishAndWaitForEstimator();
}
// set estimator input data
setEstimatorInput(&data[0], header[2]);
// we have read the imu replay message (part 1) and have waited 3 more cycles for other replay message parts
// e.g. flow, gps or range. we know that in case they were written to the log file they should come right after
// the first replay message, therefore, we can kick the estimator now
if (_part1_counter_ref > 0 && _part1_counter_ref < _message_counter - 3) {
publishAndWaitForEstimator();
}
}
}
::close(_write_fd);
::close(fd);
delete ekf2_replay::instance;
ekf2_replay::instance = nullptr;
// Report sensor innovation RMS values to assist with time delay tuning
if (_numInnovSamples > 0) {
PX4_INFO("GPS vel innov RMS = %6.3f", (double)sqrtf(_velInnovSumSq / _numInnovSamples));
PX4_INFO("GPS pos innov RMS = %6.3f", (double)sqrtf(_posInnovSumSq / _numInnovSamples));
PX4_INFO("Hgt innov RMS = %6.3f", (double)sqrtf(_hgtInnovSumSq / _numInnovSamples));
PX4_INFO("Mag innov RMS = %6.4f", (double)sqrtf(_magInnovSumSq / _numInnovSamples));
PX4_INFO("TAS innov RMS = %6.3f", (double)sqrtf(_tasInnovSumSq / _numInnovSamples));
}
}
void
CameraTrigger::cycle_trampoline(void *arg)
{
CameraTrigger *trig = reinterpret_cast<CameraTrigger *>(arg);
// default loop polling interval
int poll_interval_usec = 5000;
if (trig->_command_sub < 0) {
trig->_command_sub = orb_subscribe(ORB_ID(vehicle_command));
}
bool updated = false;
orb_check(trig->_command_sub, &updated);
struct vehicle_command_s cmd;
unsigned cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
bool need_ack = false;
// this flag is set when the polling loop is slowed down to allow the camera to power on
trig->_turning_on = false;
// these flags are used to detect state changes in the command loop
bool main_state = trig->_trigger_enabled;
bool pause_state = trig->_trigger_paused;
// Command handling
if (updated) {
orb_copy(ORB_ID(vehicle_command), trig->_command_sub, &cmd);
if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_DIGICAM_CONTROL) {
need_ack = true;
if (commandParamToInt(cmd.param7) == 1) {
// test shots are not logged or forwarded to GCS for geotagging
trig->_test_shot = true;
}
if (commandParamToInt((float)cmd.param5) == 1) {
// Schedule shot
trig->_one_shot = true;
}
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_TRIGGER_CONTROL) {
need_ack = true;
if (commandParamToInt(cmd.param3) == 1) {
// pause triggger
trig->_trigger_paused = true;
} else if (commandParamToInt(cmd.param3) == 0) {
trig->_trigger_paused = false;
}
if (commandParamToInt(cmd.param2) == 1) {
// reset trigger sequence
trig->_trigger_seq = 0;
}
if (commandParamToInt(cmd.param1) == 1) {
trig->_trigger_enabled = true;
} else if (commandParamToInt(cmd.param1) == 0) {
trig->_trigger_enabled = false;
}
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_SET_CAM_TRIGG_DIST) {
need_ack = true;
/*
* TRANSITIONAL SUPPORT ADDED AS OF 11th MAY 2017 (v1.6 RELEASE)
*/
if (cmd.param1 > 0.0f) {
trig->_distance = cmd.param1;
param_set(trig->_p_distance, &(trig->_distance));
trig->_trigger_enabled = true;
trig->_trigger_paused = false;
} else if (commandParamToInt(cmd.param1) == 0) {
trig->_trigger_paused = true;
} else if (commandParamToInt(cmd.param1) == -1) {
trig->_trigger_enabled = false;
}
// We can only control the shutter integration time of the camera in GPIO mode (for now)
if (cmd.param2 > 0.0f) {
if (trig->_camera_interface_mode == CAMERA_INTERFACE_MODE_GPIO) {
trig->_activation_time = cmd.param2;
param_set(trig->_p_activation_time, &(trig->_activation_time));
}
}
// Trigger once immediately if param is set
if (cmd.param3 > 0.0f) {
// Schedule shot
trig->_one_shot = true;
}
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_SET_CAM_TRIGG_INTERVAL) {
need_ack = true;
if (cmd.param1 > 0.0f) {
trig->_interval = cmd.param1;
param_set(trig->_p_interval, &(trig->_interval));
}
// We can only control the shutter integration time of the camera in GPIO mode
if (cmd.param2 > 0.0f) {
if (trig->_camera_interface_mode == CAMERA_INTERFACE_MODE_GPIO) {
trig->_activation_time = cmd.param2;
param_set(trig->_p_activation_time, &(trig->_activation_time));
}
}
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
}
}
// State change handling
if ((main_state != trig->_trigger_enabled) ||
(pause_state != trig->_trigger_paused) ||
trig->_one_shot) {
if (trig->_trigger_enabled || trig->_one_shot) { // Just got enabled via a command
// If camera isn't already powered on, we enable power to it
if (!trig->_camera_interface->is_powered_on() &&
trig->_camera_interface->has_power_control()) {
trig->toggle_power();
trig->enable_keep_alive(true);
// Give the camera time to turn on before starting to send trigger signals
poll_interval_usec = 3000000;
trig->_turning_on = true;
}
} else if (!trig->_trigger_enabled || trig->_trigger_paused) { // Just got disabled/paused via a command
// Power off the camera if we are disabled
if (trig->_camera_interface->is_powered_on() &&
trig->_camera_interface->has_power_control() &&
!trig->_trigger_enabled) {
trig->enable_keep_alive(false);
trig->toggle_power();
}
// cancel all calls for both disabled and paused
hrt_cancel(&trig->_engagecall);
hrt_cancel(&trig->_disengagecall);
// ensure that the pin is off
hrt_call_after(&trig->_disengagecall, 0,
(hrt_callout)&CameraTrigger::disengage, trig);
// reset distance counter if needed
if (trig->_trigger_mode == TRIGGER_MODE_DISTANCE_ON_CMD ||
trig->_trigger_mode == TRIGGER_MODE_DISTANCE_ALWAYS_ON) {
// this will force distance counter reinit on getting enabled/unpaused
trig->_valid_position = false;
}
}
// only run on state changes, not every loop iteration
if (trig->_trigger_mode == TRIGGER_MODE_INTERVAL_ON_CMD) {
// update intervalometer state and reset calls
trig->update_intervalometer();
}
}
// run every loop iteration and trigger if needed
if (trig->_trigger_mode == TRIGGER_MODE_DISTANCE_ON_CMD ||
trig->_trigger_mode == TRIGGER_MODE_DISTANCE_ALWAYS_ON) {
// update distance counter and trigger
trig->update_distance();
}
// One shot command-based capture handling
if (trig->_one_shot && !trig->_turning_on) {
// One-shot trigger
trig->shoot_once();
trig->_one_shot = false;
if (trig->_test_shot) {
trig->_test_shot = false;
}
}
// Command ACK handling
if (updated && need_ack) {
vehicle_command_ack_s command_ack = {
.timestamp = 0,
.result_param2 = 0,
.command = cmd.command,
.result = (uint8_t)cmd_result,
.from_external = 0,
.result_param1 = 0,
.target_system = cmd.source_system,
.target_component = cmd.source_component
};
if (trig->_cmd_ack_pub == nullptr) {
trig->_cmd_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &command_ack,
vehicle_command_ack_s::ORB_QUEUE_LENGTH);
} else {
orb_publish(ORB_ID(vehicle_command_ack), trig->_cmd_ack_pub, &command_ack);
}
}
work_queue(LPWORK, &_work, (worker_t)&CameraTrigger::cycle_trampoline,
camera_trigger::g_camera_trigger, USEC2TICK(poll_interval_usec));
}
void bool_param_t::button_checked( int state)
{
param_set()->begin_edit();
set_value( state);
param_set()->end_edit();
}
std::auto_ptr<undo::command_t> static_param_t::do_create_command()
{
return std::auto_ptr<undo::command_t>( new static_param_command_t( *param_set(), id()));
}
void
MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
{
switch (msg->msgid) {
case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: {
/* request all parameters */
mavlink_param_request_list_t req_list;
mavlink_msg_param_request_list_decode(msg, &req_list);
if (req_list.target_system == mavlink_system.sysid &&
(req_list.target_component == mavlink_system.compid || req_list.target_component == MAV_COMP_ID_ALL)) {
_send_all_index = 0;
}
break;
}
case MAVLINK_MSG_ID_PARAM_SET: {
/* set parameter */
if (msg->msgid == MAVLINK_MSG_ID_PARAM_SET) {
mavlink_param_set_t set;
mavlink_msg_param_set_decode(msg, &set);
if (set.target_system == mavlink_system.sysid &&
(set.target_component == mavlink_system.compid || set.target_component == MAV_COMP_ID_ALL)) {
/* local name buffer to enforce null-terminated string */
char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1];
strncpy(name, set.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
/* enforce null termination */
name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0';
/* attempt to find parameter, set and send it */
param_t param = param_find_no_notification(name);
if (param == PARAM_INVALID) {
char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN];
sprintf(buf, "[pm] unknown param: %s", name);
_mavlink->send_statustext_info(buf);
} else {
/* set and send parameter */
param_set(param, &(set.param_value));
send_param(param);
}
}
}
break;
}
case MAVLINK_MSG_ID_PARAM_REQUEST_READ: {
/* request one parameter */
mavlink_param_request_read_t req_read;
mavlink_msg_param_request_read_decode(msg, &req_read);
if (req_read.target_system == mavlink_system.sysid &&
(req_read.target_component == mavlink_system.compid || req_read.target_component == MAV_COMP_ID_ALL)) {
/* when no index is given, loop through string ids and compare them */
if (req_read.param_index < 0) {
/* local name buffer to enforce null-terminated string */
char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1];
strncpy(name, req_read.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
/* enforce null termination */
name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0';
/* attempt to find parameter and send it */
send_param(param_find_no_notification(name));
} else {
/* when index is >= 0, send this parameter again */
send_param(param_for_used_index(req_read.param_index));
}
}
break;
}
case MAVLINK_MSG_ID_PARAM_MAP_RC: {
/* map a rc channel to a parameter */
mavlink_param_map_rc_t map_rc;
mavlink_msg_param_map_rc_decode(msg, &map_rc);
if (map_rc.target_system == mavlink_system.sysid &&
(map_rc.target_component == mavlink_system.compid ||
map_rc.target_component == MAV_COMP_ID_ALL)) {
/* Copy values from msg to uorb using the parameter_rc_channel_index as index */
size_t i = map_rc.parameter_rc_channel_index;
_rc_param_map.param_index[i] = map_rc.param_index;
strncpy(&(_rc_param_map.param_id[i][0]), map_rc.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
/* enforce null termination */
_rc_param_map.param_id[i][MAVLINK_MSG_PARAM_MAP_RC_FIELD_PARAM_ID_LEN] = '\0';
_rc_param_map.scale[i] = map_rc.scale;
_rc_param_map.value0[i] = map_rc.param_value0;
_rc_param_map.value_min[i] = map_rc.param_value_min;
_rc_param_map.value_max[i] = map_rc.param_value_max;
if (map_rc.param_index == -2) { // -2 means unset map
_rc_param_map.valid[i] = false;
} else {
_rc_param_map.valid[i] = true;
}
_rc_param_map.timestamp = hrt_absolute_time();
if (_rc_param_map_pub < 0) {
_rc_param_map_pub = orb_advertise(ORB_ID(rc_parameter_map), &_rc_param_map);
} else {
orb_publish(ORB_ID(rc_parameter_map), _rc_param_map_pub, &_rc_param_map);
}
}
break;
}
default:
break;
}
}
/* buf= pointer to beginning of uri (sip:[email protected]:5060;a=b?h=i) * len= len of uri * returns: fills uri & returns <0 on error or 0 if ok */ int parse_uri(char* buf, int len, struct sip_uri* uri) { enum states { URI_INIT, URI_USER, URI_PASSWORD, URI_PASSWORD_ALPHA, URI_HOST, URI_HOST_P, URI_HOST6_P, URI_HOST6_END, URI_PORT, URI_PARAM, URI_PARAM_P, URI_PARAM_VAL_P, URI_VAL_P, URI_HEADERS, /* param states */ /* transport */ PT_T, PT_R, PT_A, PT_N, PT_S, PT_P, PT_O, PT_R2, PT_T2, PT_eq, /* ttl */ PTTL_T2, PTTL_L, PTTL_eq, /* user */ PU_U, PU_S, PU_E, PU_R, PU_eq, /* method */ PM_M, PM_E, PM_T, PM_H, PM_O, PM_D, PM_eq, /* maddr */ PMA_A, PMA_D, PMA_D2, PMA_R, PMA_eq, /* lr */ PLR_L, PLR_R_FIN, PLR_eq, /* gr */ PG_G, PG_G_FIN, PG_eq, /* r2 */ PR2_R, PR2_2_FIN, PR2_eq, /* transport values */ /* udp */ VU_U, VU_D, VU_P_FIN, /* tcp */ VT_T, VT_C, VT_P_FIN, /* tls */ VTLS_L, VTLS_S_FIN, /* sctp */ VS_S, VS_C, VS_T, VS_P_FIN, /* ws */ VW_W, VW_S, VW_S_FIN, VWS_S_FIN }; register enum states state; char* s; char* b; /* param start */ char *v; /* value start */ str* param; /* current param */ str* param_val; /* current param val */ str user; str password; int port_no; register char* p; char* end; char* pass; int found_user; int error_headers; unsigned int scheme; uri_type backup; #ifdef EXTRA_DEBUG int i; #endif #define SIP_SCH 0x3a706973 #define SIPS_SCH 0x73706973 #define TEL_SCH 0x3a6c6574 #define URN_SERVICE_SCH 0x3a6e7275 #define URN_SERVICE_STR ":service:" #define URN_SERVICE_STR_LEN (sizeof(URN_SERVICE_STR) - 1) #define case_port( ch, var) \ case ch: \ (var)=(var)*10+ch-'0'; \ break #define still_at_user \ if (found_user==0){ \ user.s=uri->host.s; \ if (pass){\ user.len=pass-user.s; \ password.s=pass+1; \ password.len=p-password.s; \ }else{ \ user.len=p-user.s; \ }\ /* save the uri type/scheme */ \ backup=uri->type; \ /* everything else is 0 */ \ memset(uri, 0, sizeof(struct sip_uri)); \ /* restore the scheme, copy user & pass */ \ uri->type=backup; \ uri->user=user; \ if (pass) uri->passwd=password; \ s=p+1; \ found_user=1;\ error_headers=0; \ state=URI_HOST; \ }else goto error_bad_char #define check_host_end \ case ':': \ /* found the host */ \ uri->host.s=s; \ uri->host.len=p-s; \ state=URI_PORT; \ s=p+1; \ break; \ case ';': \ uri->host.s=s; \ uri->host.len=p-s; \ state=URI_PARAM; \ s=p+1; \ break; \ case '?': \ uri->host.s=s; \ uri->host.len=p-s; \ state=URI_HEADERS; \ s=p+1; \ break; \ case '&': \ case '@': \ goto error_bad_char #define param_set(t_start, v_start) \ param->s=(t_start);\ param->len=(p-(t_start));\ param_val->s=(v_start); \ param_val->len=(p-(v_start)) #define u_param_set(t_start, v_start) \ if (uri->u_params_no < URI_MAX_U_PARAMS){ \ if((v_start)>(t_start)){ \ uri->u_name[uri->u_params_no].s=(t_start); \ uri->u_name[uri->u_params_no].len=((v_start)-(t_start)-1); \ if(p>(v_start)) { \ uri->u_val[uri->u_params_no].s=(v_start); \ uri->u_val[uri->u_params_no].len=(p-(v_start)); \ } \ } else { \ uri->u_name[uri->u_params_no].s=(t_start); \ uri->u_name[uri->u_params_no].len=(p-(t_start)); \ } \ uri->u_params_no++; \ } else { \ LM_ERR("unknown URI param list excedeed\n"); \ } #define semicolon_case \ case';': \ if (pass){ \ found_user=1;/* no user, pass cannot contain ';'*/ \ pass=0; \ } \ state=URI_PARAM /* new param */ #define question_case \ case '?': \ uri->params.s=s; \ uri->params.len=p-s; \ state=URI_HEADERS; \ s=p+1; \ if (pass){ \ found_user=1;/* no user, pass cannot contain '?'*/ \ pass=0; \ } #define colon_case \ case ':': \ if (found_user==0){ \ /*might be pass but only if user not found yet*/ \ if (pass){ \ found_user=1; /* no user */ \ pass=0; \ }else{ \ pass=p; \ } \ } \ state=URI_PARAM_P /* generic param */ #define param_common_cases \ case '@': \ /* ughhh, this is still the user */ \ still_at_user; \ break; \ semicolon_case; \ break; \ question_case; \ break; \ colon_case; \ break #define u_param_common_cases \ case '@': \ /* ughhh, this is still the user */ \ still_at_user; \ break; \ semicolon_case; \ u_param_set(b, v); \ break; \ question_case; \ u_param_set(b, v); \ break; \ colon_case; \ break #define value_common_cases \ case '@': \ /* ughhh, this is still the user */ \ still_at_user; \ break; \ semicolon_case; \ param_set(b, v); \ break; \ question_case; \ param_set(b, v); \ break; \ colon_case; \ state=URI_VAL_P; \ break #define param_switch(old_state, c1, c2, new_state) \ case old_state: \ switch(*p){ \ case c1: \ case c2: \ state=(new_state); \ break; \ u_param_common_cases; \ default: \ state=URI_PARAM_P; \ } \ break #define param_switch1(old_state, c1, new_state) \ case old_state: \ switch(*p){ \ case c1: \ state=(new_state); \ break; \ param_common_cases; \ default: \ state=URI_PARAM_P; \ } \ break #define param_switch_big(old_state, c1, c2, d1, d2, new_state_c, new_state_d) \ case old_state : \ switch(*p){ \ case c1: \ case c2: \ state=(new_state_c); \ break; \ case d1: \ case d2: \ state=(new_state_d); \ break; \ u_param_common_cases; \ default: \ state=URI_PARAM_P; \ } \ break #define value_switch(old_state, c1, c2, new_state) \ case old_state: \ switch(*p){ \ case c1: \ case c2: \ state=(new_state); \ break; \ value_common_cases; \ default: \ state=URI_VAL_P; \ } \ break #define value_switch_big(old_state, c1, c2, d1, d2, new_state_c, new_state_d) \ case old_state: \ switch(*p){ \ case c1: \ case c2: \ state=(new_state_c); \ break; \ case d1: \ case d2: \ state=(new_state_d); \ break; \ value_common_cases; \ default: \ state=URI_VAL_P; \ } \ break #define transport_fin(c_state, proto_no) \ case c_state: \ switch(*p){ \ case '@': \ still_at_user; \ break; \ semicolon_case; \ param_set(b, v); \ uri->proto=(proto_no); \ break; \ question_case; \ param_set(b, v); \ uri->proto=(proto_no); \ break; \ colon_case; \ default: \ state=URI_VAL_P; \ break; \ } \ break /* init */ end=buf+len; p=buf+4; found_user=0; error_headers=0; b=v=0; param=param_val=0; pass=0; password.s = 0; password.len = 0; port_no=0; state=URI_INIT; memset(uri, 0, sizeof(struct sip_uri)); /* zero it all, just to be sure*/ /*look for sip:, sips: or tel:*/ if (len<5) goto error_too_short; scheme=buf[0]+(buf[1]<<8)+(buf[2]<<16)+(buf[3]<<24); scheme|=0x20202020; if (scheme==SIP_SCH){ uri->type=SIP_URI_T; }else if(scheme==SIPS_SCH){ if(buf[4]==':'){ p++; uri->type=SIPS_URI_T;} else goto error_bad_uri; }else if (scheme==TEL_SCH){ uri->type=TEL_URI_T; }else if (scheme==URN_SERVICE_SCH){ if (memcmp(buf+3,URN_SERVICE_STR,URN_SERVICE_STR_LEN) == 0) { p+= URN_SERVICE_STR_LEN-1; uri->type=URN_SERVICE_URI_T; } else goto error_bad_uri; }else goto error_bad_uri; s=p; for(;p<end; p++){ switch((unsigned char)state){ case URI_INIT: switch(*p){ case '[': /* uri = [ipv6address]... */ state=URI_HOST6_P; s=p; break; case ']': /* invalid, no uri can start with ']' */ case ':': /* the same as above for ':' */ goto error_bad_char; case '@': /* error no user part */ goto error_bad_char; default: state=URI_USER; } break; case URI_USER: switch(*p){ case '@': /* found the user*/ uri->user.s=s; uri->user.len=p-s; state=URI_HOST; found_user=1; s=p+1; /* skip '@' */ break; case ':': /* found the user, or the host? */ uri->user.s=s; uri->user.len=p-s; state=URI_PASSWORD; s=p+1; /* skip ':' */ break; case ';': /* this could be still the user or * params?*/ uri->host.s=s; uri->host.len=p-s; state=URI_PARAM; s=p+1; break; case '?': /* still user or headers? */ uri->host.s=s; uri->host.len=p-s; state=URI_HEADERS; s=p+1; break; /* almost anything permitted in the user part */ case '[': case ']': /* the user part cannot contain "[]" */ goto error_bad_char; } break; case URI_PASSWORD: /* this can also be the port (missing user)*/ switch(*p){ case '@': /* found the password*/ uri->passwd.s=s; uri->passwd.len=p-s; port_no=0; state=URI_HOST; found_user=1; s=p+1; /* skip '@' */ break; case ';': /* upps this is the port */ uri->port.s=s; uri->port.len=p-s; uri->port_no=port_no; /* user contains in fact the host */ uri->host.s=uri->user.s; uri->host.len=uri->user.len; uri->user.s=0; uri->user.len=0; state=URI_PARAM; found_user=1; /* there is no user part */ s=p+1; break; case '?': /* upps this is the port */ uri->port.s=s; uri->port.len=p-s; uri->port_no=port_no; /* user contains in fact the host */ uri->host.s=uri->user.s; uri->host.len=uri->user.len; uri->user.s=0; uri->user.len=0; state=URI_HEADERS; found_user=1; /* there is no user part */ s=p+1; break; case_port('0', port_no); case_port('1', port_no); case_port('2', port_no); case_port('3', port_no); case_port('4', port_no); case_port('5', port_no); case_port('6', port_no); case_port('7', port_no); case_port('8', port_no); case_port('9', port_no); case '[': case ']': case ':': goto error_bad_char; default: /* it can't be the port, non number found */ port_no=0; state=URI_PASSWORD_ALPHA; } break; case URI_PASSWORD_ALPHA: switch(*p){ case '@': /* found the password*/ uri->passwd.s=s; uri->passwd.len=p-s; state=URI_HOST; found_user=1; s=p+1; /* skip '@' */ break; case ';': /* contains non-numbers => cannot be port no*/ case '?': goto error_bad_port; case '[': case ']': case ':': goto error_bad_char; } break; case URI_HOST: switch(*p){ case '[': state=URI_HOST6_P; break; case ':': case ';': case '?': /* null host name ->invalid */ case '&': case '@': /*chars not allowed in hosts names */ goto error_bad_host; default: state=URI_HOST_P; } break; case URI_HOST_P: switch(*p){ check_host_end; } break; case URI_HOST6_END: switch(*p){ check_host_end; default: /*no chars allowed after [ipv6] */ goto error_bad_host; } break; case URI_HOST6_P: switch(*p){ case ']': state=URI_HOST6_END; break; case '[': case '&': case '@': case ';': case '?': goto error_bad_host; } break; case URI_PORT: switch(*p){ case ';': uri->port.s=s; uri->port.len=p-s; uri->port_no=port_no; state=URI_PARAM; s=p+1; break; case '?': uri->port.s=s; uri->port.len=p-s; uri->port_no=port_no; state=URI_HEADERS; s=p+1; break; case_port('0', port_no); case_port('1', port_no); case_port('2', port_no); case_port('3', port_no); case_port('4', port_no); case_port('5', port_no); case_port('6', port_no); case_port('7', port_no); case_port('8', port_no); case_port('9', port_no); case '&': case '@': case ':': default: goto error_bad_port; } break; case URI_PARAM: /* beginning of a new param */ switch(*p){ param_common_cases; /* recognized params */ case 't': case 'T': b=p; state=PT_T; break; case 'u': case 'U': b=p; state=PU_U; break; case 'm': case 'M': b=p; state=PM_M; break; case 'l': case 'L': b=p; state=PLR_L; break; case 'g': case 'G': b=p; state=PG_G; break; case 'r': case 'R': b=p; state=PR2_R; break; default: b=p; state=URI_PARAM_P; } break; case URI_PARAM_P: /* ignore current param */ /* supported params: * maddr, transport, ttl, lr, user, method, r2 */ switch(*p){ u_param_common_cases; case '=': v=p + 1; state=URI_PARAM_VAL_P; break; }; break; case URI_PARAM_VAL_P: /* value of the ignored current param */ switch(*p){ u_param_common_cases; }; break; /* ugly but fast param names parsing */ /*transport */ param_switch_big(PT_T, 'r', 'R', 't', 'T', PT_R, PTTL_T2); param_switch(PT_R, 'a', 'A', PT_A); param_switch(PT_A, 'n', 'N', PT_N); param_switch(PT_N, 's', 'S', PT_S); param_switch(PT_S, 'p', 'P', PT_P); param_switch(PT_P, 'o', 'O', PT_O); param_switch(PT_O, 'r', 'R', PT_R2); param_switch(PT_R2, 't', 'T', PT_T2); param_switch1(PT_T2, '=', PT_eq); /* value parsing */ case PT_eq: param=&uri->transport; param_val=&uri->transport_val; uri->proto = PROTO_OTHER; switch (*p){ param_common_cases; case 'u': case 'U': v=p; state=VU_U; break; case 't': case 'T': v=p; state=VT_T; break; case 's': case 'S': v=p; state=VS_S; break; case 'w': case 'W': v=p; state=VW_W; break; default: v=p; state=URI_VAL_P; } break; /* generic value */ case URI_VAL_P: switch(*p){ value_common_cases; } break; /* udp */ value_switch(VU_U, 'd', 'D', VU_D); value_switch(VU_D, 'p', 'P', VU_P_FIN); transport_fin(VU_P_FIN, PROTO_UDP); /* tcp */ value_switch_big(VT_T, 'c', 'C', 'l', 'L', VT_C, VTLS_L); value_switch(VT_C, 'p', 'P', VT_P_FIN); transport_fin(VT_P_FIN, PROTO_TCP); /* tls */ value_switch(VTLS_L, 's', 'S', VTLS_S_FIN); transport_fin(VTLS_S_FIN, PROTO_TLS); /* sctp */ value_switch(VS_S, 'c', 'C', VS_C); value_switch(VS_C, 't', 'T', VS_T); value_switch(VS_T, 'p', 'P', VS_P_FIN); transport_fin(VS_P_FIN, PROTO_SCTP); /* ws */ value_switch(VW_W, 's', 'S', VW_S); case VW_S: if (*p == 's' || *p == 'S') { state=(VWS_S_FIN); break; } /* if not a 's' transiting to VWS_S_FIN, fallback * to testing as existing VW_S_FIN (NOTE the missing break) */ state=(VW_S_FIN); transport_fin(VW_S_FIN, PROTO_WS); transport_fin(VWS_S_FIN, PROTO_WSS); /* ttl */ param_switch(PTTL_T2, 'l', 'L', PTTL_L); param_switch1(PTTL_L, '=', PTTL_eq); case PTTL_eq: param=&uri->ttl; param_val=&uri->ttl_val; switch(*p){ param_common_cases; default: v=p; state=URI_VAL_P; } break; /* user param */ param_switch(PU_U, 's', 'S', PU_S); param_switch(PU_S, 'e', 'E', PU_E); param_switch(PU_E, 'r', 'R', PU_R); param_switch1(PU_R, '=', PU_eq); case PU_eq: param=&uri->user_param; param_val=&uri->user_param_val; switch(*p){ param_common_cases; default: v=p; state=URI_VAL_P; } break; /* method*/ param_switch_big(PM_M, 'e', 'E', 'a', 'A', PM_E, PMA_A); param_switch(PM_E, 't', 'T', PM_T); param_switch(PM_T, 'h', 'H', PM_H); param_switch(PM_H, 'o', 'O', PM_O); param_switch(PM_O, 'd', 'D', PM_D); param_switch1(PM_D, '=', PM_eq); case PM_eq: param=&uri->method; param_val=&uri->method_val; switch(*p){ param_common_cases; default: v=p; state=URI_VAL_P; } break; /*maddr*/ param_switch(PMA_A, 'd', 'D', PMA_D); param_switch(PMA_D, 'd', 'D', PMA_D2); param_switch(PMA_D2, 'r', 'R', PMA_R); param_switch1(PMA_R, '=', PMA_eq); case PMA_eq: param=&uri->maddr; param_val=&uri->maddr_val; switch(*p){ param_common_cases; default: v=p; state=URI_VAL_P; } break; /* lr */ param_switch(PLR_L, 'r', 'R', PLR_R_FIN); case PLR_R_FIN: switch(*p){ case '@': still_at_user; break; case '=': state=PLR_eq; break; semicolon_case; uri->lr.s=b; uri->lr.len=(p-b); break; question_case; uri->lr.s=b; uri->lr.len=(p-b); break; colon_case; break; default: state=URI_PARAM_P; } break; /* handle lr=something case */ case PLR_eq: param=&uri->lr; param_val=&uri->lr_val; switch(*p){ param_common_cases; default: v=p; state=URI_VAL_P; } break; /* r2 */ param_switch1(PR2_R, '2', PR2_2_FIN); case PR2_2_FIN: switch(*p){ case '@': still_at_user; break; case '=': state=PR2_eq; break; semicolon_case; uri->r2.s=b; uri->r2.len=(p-b); break; question_case; uri->r2.s=b; uri->r2.len=(p-b); break; colon_case; break; default: state=URI_PARAM_P; } break; /* handle lr=something case */ case PR2_eq: param=&uri->r2; param_val=&uri->r2_val; switch(*p){ param_common_cases; default: v=p; state=URI_VAL_P; } break; /* gr */ param_switch(PG_G, 'r', 'R', PG_G_FIN); case PG_G_FIN: switch(*p){ case '@': still_at_user; break; case '=': state=PG_eq; break; semicolon_case; uri->gr.s=b; uri->gr.len=(p-b); break; question_case; uri->gr.s=b; uri->gr.len=(p-b); break; colon_case; break; default: state=URI_PARAM_P; } break; /* handle gr=something case */ case PG_eq: param=&uri->gr; param_val=&uri->gr_val; switch(*p){ param_common_cases; default: v=p; state=URI_VAL_P; } break; case URI_HEADERS: /* for now nobody needs them so we completely ignore the * headers (they are not allowed in request uri) --andrei */ switch(*p){ case '@': /* yak, we are still at user */ still_at_user; break; case ';': /* we might be still parsing user, try it */ if (found_user) goto error_bad_char; error_headers=1; /* if this is not the user we have an error */ /* if pass is set => it cannot be user:pass * => error (';') is illegal in a header */ if (pass) goto error_headers; break; case ':': if (found_user==0){ /*might be pass but only if user not found yet*/ if (pass){ found_user=1; /* no user */ pass=0; }else{ pass=p; } } break; case '?': if (pass){ found_user=1; /* no user, pass cannot contain '?'*/ pass=0; } break; } break; default: goto error_bug; } } /*end of uri */ switch (state){ case URI_INIT: /* error empty uri */ goto error_too_short; case URI_USER: /* this is the host, it can't be the user */ if (found_user) goto error_bad_uri; uri->host.s=s; uri->host.len=p-s; state=URI_HOST; break; case URI_PASSWORD: /* this is the port, it can't be the passwd */ if (found_user) goto error_bad_port; uri->port.s=s; uri->port.len=p-s; uri->port_no=port_no; uri->host=uri->user; uri->user.s=0; uri->user.len=0; break; case URI_PASSWORD_ALPHA: /* this is the port, it can't be the passwd */ goto error_bad_port; case URI_HOST_P: case URI_HOST6_END: uri->host.s=s; uri->host.len=p-s; break; case URI_HOST: /* error: null host */ case URI_HOST6_P: /* error: unterminated ipv6 reference*/ goto error_bad_host; case URI_PORT: uri->port.s=s; uri->port.len=p-s; uri->port_no=port_no; break; case URI_PARAM: case URI_PARAM_P: case URI_PARAM_VAL_P: u_param_set(b, v); /* intermediate param states */ case PT_T: /* transport */ case PT_R: case PT_A: case PT_N: case PT_S: case PT_P: case PT_O: case PT_R2: case PT_T2: case PT_eq: /* ignore empty transport params */ case PTTL_T2: /* ttl */ case PTTL_L: case PTTL_eq: case PU_U: /* user */ case PU_S: case PU_E: case PU_R: case PU_eq: case PM_M: /* method */ case PM_E: case PM_T: case PM_H: case PM_O: case PM_D: case PM_eq: case PLR_L: /* lr */ case PR2_R: /* r2 */ case PG_G: /* gr */ uri->params.s=s; uri->params.len=p-s; break; /* fin param states */ case PLR_R_FIN: case PLR_eq: uri->params.s=s; uri->params.len=p-s; uri->lr.s=b; uri->lr.len=p-b; break; case PR2_2_FIN: case PR2_eq: uri->params.s=s; uri->params.len=p-s; uri->r2.s=b; uri->r2.len=p-b; break; case PG_G_FIN: case PG_eq: uri->params.s=s; uri->params.len=p-s; uri->gr.s=b; uri->gr.len=p-b; break; case URI_VAL_P: /* intermediate value states */ case VU_U: case VU_D: case VT_T: case VT_C: case VTLS_L: case VS_S: case VS_C: case VW_W: case VS_T: uri->params.s=s; uri->params.len=p-s; param_set(b, v); break; /* fin value states */ case VU_P_FIN: uri->params.s=s; uri->params.len=p-s; param_set(b, v); uri->proto=PROTO_UDP; break; case VT_P_FIN: uri->params.s=s; uri->params.len=p-s; param_set(b, v); uri->proto=PROTO_TCP; break; case VTLS_S_FIN: uri->params.s=s; uri->params.len=p-s; param_set(b, v); uri->proto=PROTO_TLS; break; case VS_P_FIN: uri->params.s=s; uri->params.len=p-s; param_set(b, v); uri->proto=PROTO_SCTP; break; case VW_S: case VW_S_FIN: uri->params.s=s; uri->params.len=p-s; param_set(b, v); uri->proto=PROTO_WS; break; case VWS_S_FIN: uri->params.s=s; uri->params.len=p-s; param_set(b, v); uri->proto=PROTO_WSS; break; /* headers */ case URI_HEADERS: uri->headers.s=s; uri->headers.len=p-s; if (error_headers) goto error_headers; break; default: goto error_bug; } switch(uri->type){ case SIP_URI_T: if ((uri->user_param_val.len == 5) && (strncasecmp(uri->user_param_val.s, "phone", 5) == 0)) { uri->type = TEL_URI_T; /* move params from user into uri->params */ p=q_memchr(uri->user.s, ';', uri->user.len); if (p){ uri->params.s=p+1; uri->params.len=uri->user.s+uri->user.len-uri->params.s; uri->user.len=p-uri->user.s; } } break; case SIPS_URI_T: if ((uri->user_param_val.len == 5) && (strncasecmp(uri->user_param_val.s, "phone", 5) == 0)) { uri->type = TELS_URI_T; p=q_memchr(uri->user.s, ';', uri->user.len); if (p){ uri->params.s=p+1; uri->params.len=uri->user.s+uri->user.len-uri->params.s; uri->user.len=p-uri->user.s; } } break; case TEL_URI_T: case TELS_URI_T: /* fix tel uris, move the number in uri and empty the host */ uri->user=uri->host; uri->host.s=""; uri->host.len=0; break; case URN_SERVICE_URI_T: /* leave the actual service name in the URI domain part */ break; case ERROR_URI_T: LM_ERR("unexpected error (BUG?)\n"); goto error_bad_uri; break; /* do nothing, avoids a compilation warning */ } #ifdef EXTRA_DEBUG /* do stuff */ LM_DBG("parsed uri:\n type=%d user=<%.*s>(%d)\n passwd=<%.*s>(%d)\n" " host=<%.*s>(%d)\n port=<%.*s>(%d): %d\n params=<%.*s>(%d)\n" " headers=<%.*s>(%d)\n", uri->type, uri->user.len, ZSW(uri->user.s), uri->user.len, uri->passwd.len, ZSW(uri->passwd.s), uri->passwd.len, uri->host.len, ZSW(uri->host.s), uri->host.len, uri->port.len, ZSW(uri->port.s), uri->port.len, uri->port_no, uri->params.len, ZSW(uri->params.s), uri->params.len, uri->headers.len, ZSW(uri->headers.s), uri->headers.len ); LM_DBG(" uri params:\n transport=<%.*s>, val=<%.*s>, proto=%d\n", uri->transport.len, ZSW(uri->transport.s), uri->transport_val.len, ZSW(uri->transport_val.s), uri->proto); LM_DBG(" user-param=<%.*s>, val=<%.*s>\n", uri->user_param.len, ZSW(uri->user_param.s), uri->user_param_val.len, ZSW(uri->user_param_val.s)); LM_DBG(" method=<%.*s>, val=<%.*s>\n", uri->method.len, ZSW(uri->method.s), uri->method_val.len, ZSW(uri->method_val.s)); LM_DBG(" ttl=<%.*s>, val=<%.*s>\n", uri->ttl.len, ZSW(uri->ttl.s), uri->ttl_val.len, ZSW(uri->ttl_val.s)); LM_DBG(" maddr=<%.*s>, val=<%.*s>\n", uri->maddr.len, ZSW(uri->maddr.s), uri->maddr_val.len, ZSW(uri->maddr_val.s)); LM_DBG(" lr=<%.*s>, val=<%.*s>\n", uri->lr.len, ZSW(uri->lr.s), uri->lr_val.len, ZSW(uri->lr_val.s)); LM_DBG(" r2=<%.*s>, val=<%.*s>\n", uri->r2.len, ZSW(uri->r2.s), uri->r2_val.len, ZSW(uri->r2_val.s)); for(i=0; i<URI_MAX_U_PARAMS && uri->u_name[i].s; i++) LM_DBG("uname=[%p]-><%.*s> uval=[%p]-><%.*s>\n", uri->u_name[i].s, uri->u_name[i].len, uri->u_name[i].s, uri->u_val[i].s, uri->u_val[i].len, uri->u_val[i].s); if (i!=uri->u_params_no) LM_ERR("inconsisten # of u_name:[%d]!=[%d]\n", i, uri->u_params_no); #endif return 0; error_too_short: LM_ERR("uri too short: <%.*s> (%d)\n", len, ZSW(buf), len); goto error_exit; error_bad_char: LM_ERR("bad char '%c' in state %d" " parsed: <%.*s> (%d) / <%.*s> (%d)\n", *p, state, (int)(p-buf), ZSW(buf), (int)(p-buf), len, ZSW(buf), len); goto error_exit; error_bad_host: LM_ERR("bad host in uri (error at char %c in" " state %d) parsed: <%.*s>(%d) /<%.*s> (%d)\n", *p, state, (int)(p-buf), ZSW(buf), (int)(p-buf), len, ZSW(buf), len); goto error_exit; error_bad_port: LM_ERR("bad port in uri (error at char %c in" " state %d) parsed: <%.*s>(%d) /<%.*s> (%d)\n", *p, state, (int)(p-buf), ZSW(buf), (int)(p-buf), len, ZSW(buf), len); goto error_exit; error_bad_uri: LM_ERR("bad uri, state %d parsed: <%.*s> (%d) / <%.*s> (%d)\n", state, (int)(p-buf), ZSW(buf), (int)(p-buf), len, ZSW(buf), len); goto error_exit; error_headers: LM_ERR("bad uri headers: <%.*s>(%d) / <%.*s>(%d)\n", uri->headers.len, ZSW(uri->headers.s), uri->headers.len, len, ZSW(buf), len); goto error_exit; error_bug: LM_CRIT("bad state %d parsed: <%.*s> (%d) / <%.*s> (%d)\n", state, (int)(p-buf), ZSW(buf), (int)(p-buf), len, ZSW(buf), len); error_exit: ser_error=E_BAD_URI; uri->type=ERROR_URI_T; update_stat(bad_URIs, 1); return E_BAD_URI; }
void
CameraTrigger::poll(void *arg)
{
CameraTrigger *trig = reinterpret_cast<CameraTrigger *>(arg);
bool updated;
orb_check(trig->_vcommand_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_command), trig->_vcommand_sub, &trig->_command);
if(trig->_command.command == vehicle_command_s::VEHICLE_CMD_DO_TRIGGER_CONTROL)
{
if(trig->_command.param1 < 1)
{
if(trig->_trigger_enabled)
{
trig->_trigger_enabled = false ;
}
}
else if(trig->_command.param1 >= 1)
{
if(!trig->_trigger_enabled)
{
trig->_trigger_enabled = true ;
}
}
// Set trigger rate from command
if(trig->_command.param2 > 0)
{
trig->_integration_time = trig->_command.param2;
param_set(trig->integration_time, &(trig->_integration_time));
}
}
}
if(!trig->_trigger_enabled) {
hrt_call_after(&trig->_pollcall, 1e6, (hrt_callout)&CameraTrigger::poll, trig);
return;
}
else
{
engage(trig);
hrt_call_after(&trig->_firecall, trig->_activation_time*1000, (hrt_callout)&CameraTrigger::disengage, trig);
orb_copy(ORB_ID(sensor_combined), trig->_sensor_sub, &trig->_sensor);
trig->_trigger.timestamp = trig->_sensor.timestamp; // get IMU timestamp
trig->_trigger.seq = trig->_trigger_seq++;
if (trig->_trigger_pub != nullptr) {
orb_publish(ORB_ID(camera_trigger), trig->_trigger_pub, &trig->_trigger);
} else {
trig->_trigger_pub = orb_advertise(ORB_ID(camera_trigger), &trig->_trigger);
}
hrt_call_after(&trig->_pollcall, (trig->_transfer_time + trig->_integration_time)*1000, (hrt_callout)&CameraTrigger::poll, trig);
}
}
calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub)
{
calibrate_return result = calibrate_return_ok;
mag_worker_data_t worker_data;
worker_data.mavlink_log_pub = mavlink_log_pub;
worker_data.done_count = 0;
worker_data.calibration_points_perside = calibration_total_points / calibration_sides;
worker_data.calibration_interval_perside_seconds = calibraton_duration_seconds / calibration_sides;
worker_data.calibration_interval_perside_useconds = worker_data.calibration_interval_perside_seconds * 1000 * 1000;
// Collect: As defined by configuration
// start with a full mask, all six bits set
uint32_t cal_mask = (1 << 6) - 1;
param_get(param_find("CAL_MAG_SIDES"), &cal_mask);
calibration_sides = 0;
for (unsigned i = 0; i < (sizeof(worker_data.side_data_collected) /
sizeof(worker_data.side_data_collected[0])); i++) {
if ((cal_mask & (1 << i)) > 0) {
// mark as missing
worker_data.side_data_collected[i] = false;
calibration_sides++;
} else {
// mark as completed from the beginning
worker_data.side_data_collected[i] = true;
calibration_log_info(mavlink_log_pub,
"[cal] %s side done, rotate to a different side",
detect_orientation_str(static_cast<enum detect_orientation_return>(i)));
usleep(100000);
}
}
for (size_t cur_mag = 0; cur_mag<max_mags; cur_mag++) {
// Initialize to no subscription
worker_data.sub_mag[cur_mag] = -1;
// Initialize to no memory allocated
worker_data.x[cur_mag] = NULL;
worker_data.y[cur_mag] = NULL;
worker_data.z[cur_mag] = NULL;
worker_data.calibration_counter_total[cur_mag] = 0;
}
const unsigned int calibration_points_maxcount = calibration_sides * worker_data.calibration_points_perside;
char str[30];
for (size_t cur_mag=0; cur_mag<max_mags; cur_mag++) {
worker_data.x[cur_mag] = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_points_maxcount));
worker_data.y[cur_mag] = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_points_maxcount));
worker_data.z[cur_mag] = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_points_maxcount));
if (worker_data.x[cur_mag] == NULL || worker_data.y[cur_mag] == NULL || worker_data.z[cur_mag] == NULL) {
calibration_log_critical(mavlink_log_pub, "[cal] ERROR: out of memory");
result = calibrate_return_error;
}
}
// Setup subscriptions to mag sensors
if (result == calibrate_return_ok) {
// We should not try to subscribe if the topic doesn't actually exist and can be counted.
const unsigned mag_count = orb_group_count(ORB_ID(sensor_mag));
for (unsigned cur_mag = 0; cur_mag < mag_count; cur_mag++) {
// Mag in this slot is available
worker_data.sub_mag[cur_mag] = orb_subscribe_multi(ORB_ID(sensor_mag), cur_mag);
#if defined(__PX4_QURT) || defined(__PX4_POSIX_RPI)
// For QURT respectively the driver framework, we need to get the device ID by copying one report.
struct mag_report mag_report;
orb_copy(ORB_ID(sensor_mag), worker_data.sub_mag[cur_mag], &mag_report);
device_ids[cur_mag] = mag_report.device_id;
#endif
if (worker_data.sub_mag[cur_mag] < 0) {
calibration_log_critical(mavlink_log_pub, "[cal] Mag #%u not found, abort", cur_mag);
result = calibrate_return_error;
break;
}
if (device_ids[cur_mag] != 0) {
// Get priority
int32_t prio;
orb_priority(worker_data.sub_mag[cur_mag], &prio);
if (prio > device_prio_max) {
device_prio_max = prio;
device_id_primary = device_ids[cur_mag];
}
} else {
calibration_log_critical(mavlink_log_pub, "[cal] Mag #%u no device id, abort", cur_mag);
result = calibrate_return_error;
break;
}
}
}
// Limit update rate to get equally spaced measurements over time (in ms)
if (result == calibrate_return_ok) {
for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) {
if (device_ids[cur_mag] != 0) {
// Mag in this slot is available
unsigned int orb_interval_msecs = (worker_data.calibration_interval_perside_useconds / 1000) / worker_data.calibration_points_perside;
//calibration_log_info(mavlink_log_pub, "Orb interval %u msecs", orb_interval_msecs);
orb_set_interval(worker_data.sub_mag[cur_mag], orb_interval_msecs);
}
}
}
if (result == calibrate_return_ok) {
int cancel_sub = calibrate_cancel_subscribe();
result = calibrate_from_orientation(mavlink_log_pub, // uORB handle to write output
cancel_sub, // Subscription to vehicle_command for cancel support
worker_data.side_data_collected, // Sides to calibrate
mag_calibration_worker, // Calibration worker
&worker_data, // Opaque data for calibration worked
true); // true: lenient still detection
calibrate_cancel_unsubscribe(cancel_sub);
}
// Close subscriptions
for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) {
if (worker_data.sub_mag[cur_mag] >= 0) {
px4_close(worker_data.sub_mag[cur_mag]);
}
}
// Calculate calibration values for each mag
float sphere_x[max_mags];
float sphere_y[max_mags];
float sphere_z[max_mags];
float sphere_radius[max_mags];
// Sphere fit the data to get calibration values
if (result == calibrate_return_ok) {
for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) {
if (device_ids[cur_mag] != 0) {
// Mag in this slot is available and we should have values for it to calibrate
sphere_fit_least_squares(worker_data.x[cur_mag], worker_data.y[cur_mag], worker_data.z[cur_mag],
worker_data.calibration_counter_total[cur_mag],
100, 0.0f,
&sphere_x[cur_mag], &sphere_y[cur_mag], &sphere_z[cur_mag],
&sphere_radius[cur_mag]);
if (!PX4_ISFINITE(sphere_x[cur_mag]) || !PX4_ISFINITE(sphere_y[cur_mag]) || !PX4_ISFINITE(sphere_z[cur_mag])) {
calibration_log_emergency(mavlink_log_pub, "ERROR: Retry calibration (sphere NaN, #%u)", cur_mag);
result = calibrate_return_error;
}
if (fabsf(sphere_x[cur_mag]) > MAG_MAX_OFFSET_LEN ||
fabsf(sphere_y[cur_mag]) > MAG_MAX_OFFSET_LEN ||
fabsf(sphere_z[cur_mag]) > MAG_MAX_OFFSET_LEN) {
calibration_log_critical(mavlink_log_pub, "Warning: %s mag with large offsets", (internal[cur_mag]) ? "autopilot, internal" : "GPS unit, external");
calibration_log_info(mavlink_log_pub, "Offsets: x: %8.4f, y: %8.4f, z: %8.4f, #%u", (double)sphere_x[cur_mag],
(double)sphere_y[cur_mag], (double)sphere_z[cur_mag], cur_mag);
result = calibrate_return_ok;
}
}
}
}
// Print uncalibrated data points
if (result == calibrate_return_ok) {
// DO NOT REMOVE! Critical validation data!
// printf("RAW DATA:\n--------------------\n");
// for (size_t cur_mag = 0; cur_mag < max_mags; cur_mag++) {
// if (worker_data.calibration_counter_total[cur_mag] == 0) {
// continue;
// }
// printf("RAW: MAG %u with %u samples:\n", (unsigned)cur_mag, (unsigned)worker_data.calibration_counter_total[cur_mag]);
// for (size_t i = 0; i < worker_data.calibration_counter_total[cur_mag]; i++) {
// float x = worker_data.x[cur_mag][i];
// float y = worker_data.y[cur_mag][i];
// float z = worker_data.z[cur_mag][i];
// printf("%8.4f, %8.4f, %8.4f\n", (double)x, (double)y, (double)z);
// }
// printf(">>>>>>>\n");
// }
// printf("CALIBRATED DATA:\n--------------------\n");
// for (size_t cur_mag = 0; cur_mag < max_mags; cur_mag++) {
// if (worker_data.calibration_counter_total[cur_mag] == 0) {
// continue;
// }
// printf("Calibrated: MAG %u with %u samples:\n", (unsigned)cur_mag, (unsigned)worker_data.calibration_counter_total[cur_mag]);
// for (size_t i = 0; i < worker_data.calibration_counter_total[cur_mag]; i++) {
// float x = worker_data.x[cur_mag][i] - sphere_x[cur_mag];
// float y = worker_data.y[cur_mag][i] - sphere_y[cur_mag];
// float z = worker_data.z[cur_mag][i] - sphere_z[cur_mag];
// printf("%8.4f, %8.4f, %8.4f\n", (double)x, (double)y, (double)z);
// }
// printf("SPHERE RADIUS: %8.4f\n", (double)sphere_radius[cur_mag]);
// printf(">>>>>>>\n");
// }
}
// Data points are no longer needed
for (size_t cur_mag=0; cur_mag<max_mags; cur_mag++) {
free(worker_data.x[cur_mag]);
free(worker_data.y[cur_mag]);
free(worker_data.z[cur_mag]);
}
if (result == calibrate_return_ok) {
for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) {
if (device_ids[cur_mag] != 0) {
struct mag_calibration_s mscale;
#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_RPI)
int fd_mag = -1;
// Set new scale
(void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, cur_mag);
fd_mag = px4_open(str, 0);
if (fd_mag < 0) {
calibration_log_critical(mavlink_log_pub, "[cal] ERROR: unable to open mag device #%u", cur_mag);
result = calibrate_return_error;
}
if (result == calibrate_return_ok) {
if (px4_ioctl(fd_mag, MAGIOCGSCALE, (long unsigned int)&mscale) != OK) {
calibration_log_critical(mavlink_log_pub, "[cal] ERROR: failed to get current calibration #%u", cur_mag);
result = calibrate_return_error;
}
}
#endif
if (result == calibrate_return_ok) {
mscale.x_offset = sphere_x[cur_mag];
mscale.y_offset = sphere_y[cur_mag];
mscale.z_offset = sphere_z[cur_mag];
#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_RPI)
if (px4_ioctl(fd_mag, MAGIOCSSCALE, (long unsigned int)&mscale) != OK) {
calibration_log_critical(mavlink_log_pub, CAL_ERROR_APPLY_CAL_MSG, cur_mag);
result = calibrate_return_error;
}
#endif
}
#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_RPI)
// Mag device no longer needed
if (fd_mag >= 0) {
px4_close(fd_mag);
}
#endif
if (result == calibrate_return_ok) {
bool failed = false;
/* set parameters */
(void)sprintf(str, "CAL_MAG%u_ID", cur_mag);
failed |= (OK != param_set_no_notification(param_find(str), &(device_ids[cur_mag])));
(void)sprintf(str, "CAL_MAG%u_XOFF", cur_mag);
failed |= (OK != param_set_no_notification(param_find(str), &(mscale.x_offset)));
(void)sprintf(str, "CAL_MAG%u_YOFF", cur_mag);
failed |= (OK != param_set_no_notification(param_find(str), &(mscale.y_offset)));
(void)sprintf(str, "CAL_MAG%u_ZOFF", cur_mag);
failed |= (OK != param_set_no_notification(param_find(str), &(mscale.z_offset)));
// FIXME: scaling is not used right now on QURT
#ifndef __PX4_QURT
(void)sprintf(str, "CAL_MAG%u_XSCALE", cur_mag);
failed |= (OK != param_set_no_notification(param_find(str), &(mscale.x_scale)));
(void)sprintf(str, "CAL_MAG%u_YSCALE", cur_mag);
failed |= (OK != param_set_no_notification(param_find(str), &(mscale.y_scale)));
(void)sprintf(str, "CAL_MAG%u_ZSCALE", cur_mag);
failed |= (OK != param_set_no_notification(param_find(str), &(mscale.z_scale)));
#endif
if (failed) {
calibration_log_critical(mavlink_log_pub, CAL_ERROR_SET_PARAMS_MSG, cur_mag);
result = calibrate_return_error;
} else {
calibration_log_info(mavlink_log_pub, "[cal] mag #%u off: x:%.2f y:%.2f z:%.2f Ga",
cur_mag,
(double)mscale.x_offset, (double)mscale.y_offset, (double)mscale.z_offset);
#ifndef __PX4_QURT
calibration_log_info(mavlink_log_pub, "[cal] mag #%u scale: x:%.2f y:%.2f z:%.2f",
cur_mag,
(double)mscale.x_scale, (double)mscale.y_scale, (double)mscale.z_scale);
#endif
usleep(200000);
}
}
}
}
// Trigger a param set on the last step so the whole
// system updates
(void)param_set(param_find("CAL_MAG_PRIME"), &(device_id_primary));
}
return result;
}
void
MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
{
switch (msg->msgid) {
case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: {
/* request all parameters */
mavlink_param_request_list_t req_list;
mavlink_msg_param_request_list_decode(msg, &req_list);
if (req_list.target_system == mavlink_system.sysid &&
(req_list.target_component == mavlink_system.compid || req_list.target_component == MAV_COMP_ID_ALL)) {
if (_send_all_index < 0) {
_send_all_index = PARAM_HASH;
} else {
/* a restart should skip the hash check on the ground */
_send_all_index = 0;
}
}
if (req_list.target_system == mavlink_system.sysid && req_list.target_component < 127 &&
(req_list.target_component != mavlink_system.compid || req_list.target_component == MAV_COMP_ID_ALL)) {
// publish list request to UAVCAN driver via uORB.
uavcan_parameter_request_s req;
req.message_type = msg->msgid;
req.node_id = req_list.target_component;
req.param_index = 0;
if (_uavcan_parameter_request_pub == nullptr) {
_uavcan_parameter_request_pub = orb_advertise(ORB_ID(uavcan_parameter_request), &req);
} else {
orb_publish(ORB_ID(uavcan_parameter_request), _uavcan_parameter_request_pub, &req);
}
}
break;
}
case MAVLINK_MSG_ID_PARAM_SET: {
/* set parameter */
mavlink_param_set_t set;
mavlink_msg_param_set_decode(msg, &set);
if (set.target_system == mavlink_system.sysid &&
(set.target_component == mavlink_system.compid || set.target_component == MAV_COMP_ID_ALL)) {
/* local name buffer to enforce null-terminated string */
char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1];
strncpy(name, set.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
/* enforce null termination */
name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0';
/* Whatever the value is, we're being told to stop sending */
if (strncmp(name, "_HASH_CHECK", sizeof(name)) == 0) {
_send_all_index = -1;
/* No other action taken, return */
return;
}
/* attempt to find parameter, set and send it */
param_t param = param_find_no_notification(name);
if (param == PARAM_INVALID) {
char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN];
sprintf(buf, "[pm] unknown param: %s", name);
_mavlink->send_statustext_info(buf);
} else {
// Load current value before setting it
float curr_val;
param_get(param, &curr_val);
param_set(param, &(set.param_value));
// Check if the parameter changed. If it didn't change, send current value back
if (!(fabsf(curr_val - set.param_value) > 0.0f)) {
send_param(param);
}
}
}
if (set.target_system == mavlink_system.sysid && set.target_component < 127 &&
(set.target_component != mavlink_system.compid || set.target_component == MAV_COMP_ID_ALL)) {
// publish set request to UAVCAN driver via uORB.
uavcan_parameter_request_s req;
req.message_type = msg->msgid;
req.node_id = set.target_component;
req.param_index = -1;
strncpy(req.param_id, set.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1);
req.param_id[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0';
if (set.param_type == MAV_PARAM_TYPE_REAL32) {
req.param_type = MAV_PARAM_TYPE_REAL32;
req.real_value = set.param_value;
} else {
int32_t val;
memcpy(&val, &set.param_value, sizeof(int32_t));
req.param_type = MAV_PARAM_TYPE_INT64;
req.int_value = val;
}
if (_uavcan_parameter_request_pub == nullptr) {
_uavcan_parameter_request_pub = orb_advertise(ORB_ID(uavcan_parameter_request), &req);
} else {
orb_publish(ORB_ID(uavcan_parameter_request), _uavcan_parameter_request_pub, &req);
}
}
break;
}
case MAVLINK_MSG_ID_PARAM_REQUEST_READ: {
/* request one parameter */
mavlink_param_request_read_t req_read;
mavlink_msg_param_request_read_decode(msg, &req_read);
if (req_read.target_system == mavlink_system.sysid &&
(req_read.target_component == mavlink_system.compid || req_read.target_component == MAV_COMP_ID_ALL)) {
/* when no index is given, loop through string ids and compare them */
if (req_read.param_index < 0) {
/* XXX: I left this in so older versions of QGC wouldn't break */
if (strncmp(req_read.param_id, HASH_PARAM, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN) == 0) {
/* return hash check for cached params */
uint32_t hash = param_hash_check();
/* build the one-off response message */
mavlink_param_value_t param_value;
param_value.param_count = param_count_used();
param_value.param_index = -1;
strncpy(param_value.param_id, HASH_PARAM, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
param_value.param_type = MAV_PARAM_TYPE_UINT32;
memcpy(¶m_value.param_value, &hash, sizeof(hash));
mavlink_msg_param_value_send_struct(_mavlink->get_channel(), ¶m_value);
} else {
/* local name buffer to enforce null-terminated string */
char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1];
strncpy(name, req_read.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
/* enforce null termination */
name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0';
/* attempt to find parameter and send it */
send_param(param_find_no_notification(name));
}
} else {
/* when index is >= 0, send this parameter again */
int ret = send_param(param_for_used_index(req_read.param_index));
if (ret == 1) {
char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN];
sprintf(buf, "[pm] unknown param ID: %u", req_read.param_index);
_mavlink->send_statustext_info(buf);
} else if (ret == 2) {
char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN];
sprintf(buf, "[pm] failed loading param from storage ID: %u", req_read.param_index);
_mavlink->send_statustext_info(buf);
}
}
}
if (req_read.target_system == mavlink_system.sysid && req_read.target_component < 127 &&
(req_read.target_component != mavlink_system.compid || req_read.target_component == MAV_COMP_ID_ALL)) {
// publish set request to UAVCAN driver via uORB.
uavcan_parameter_request_s req = {};
req.timestamp = hrt_absolute_time();
req.message_type = msg->msgid;
req.node_id = req_read.target_component;
req.param_index = req_read.param_index;
strncpy(req.param_id, req_read.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1);
req.param_id[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0';
// Enque the request and forward the first to the uavcan node
enque_uavcan_request(&req);
request_next_uavcan_parameter();
}
break;
}
case MAVLINK_MSG_ID_PARAM_MAP_RC: {
/* map a rc channel to a parameter */
mavlink_param_map_rc_t map_rc;
mavlink_msg_param_map_rc_decode(msg, &map_rc);
if (map_rc.target_system == mavlink_system.sysid &&
(map_rc.target_component == mavlink_system.compid ||
map_rc.target_component == MAV_COMP_ID_ALL)) {
/* Copy values from msg to uorb using the parameter_rc_channel_index as index */
size_t i = map_rc.parameter_rc_channel_index;
_rc_param_map.param_index[i] = map_rc.param_index;
strncpy(&(_rc_param_map.param_id[i * (rc_parameter_map_s::PARAM_ID_LEN + 1)]), map_rc.param_id,
MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
/* enforce null termination */
_rc_param_map.param_id[i * (rc_parameter_map_s::PARAM_ID_LEN + 1) + rc_parameter_map_s::PARAM_ID_LEN] = '\0';
_rc_param_map.scale[i] = map_rc.scale;
_rc_param_map.value0[i] = map_rc.param_value0;
_rc_param_map.value_min[i] = map_rc.param_value_min;
_rc_param_map.value_max[i] = map_rc.param_value_max;
if (map_rc.param_index == -2) { // -2 means unset map
_rc_param_map.valid[i] = false;
} else {
_rc_param_map.valid[i] = true;
}
_rc_param_map.timestamp = hrt_absolute_time();
if (_rc_param_map_pub == nullptr) {
_rc_param_map_pub = orb_advertise(ORB_ID(rc_parameter_map), &_rc_param_map);
} else {
orb_publish(ORB_ID(rc_parameter_map), _rc_param_map_pub, &_rc_param_map);
}
}
break;
}
default:
break;
}
}
void add_param( std::auto_ptr<T> p) { param_set().add_param( p);}
void do_accel_calibration(int status_pub, struct vehicle_status_s *status, int mavlink_fd) {
/* announce change */
mavlink_log_info(mavlink_fd, "accel calibration started");
/* set to accel calibration mode */
status->flag_preflight_accel_calibration = true;
state_machine_publish(status_pub, status, mavlink_fd);
/* measure and calculate offsets & scales */
float accel_offs[3];
float accel_scale[3];
int res = do_accel_calibration_mesurements(mavlink_fd, accel_offs, accel_scale);
if (res == OK) {
/* measurements complete successfully, set parameters */
if (param_set(param_find("SENS_ACC_XOFF"), &(accel_offs[0]))
|| param_set(param_find("SENS_ACC_YOFF"), &(accel_offs[1]))
|| param_set(param_find("SENS_ACC_ZOFF"), &(accel_offs[2]))
|| param_set(param_find("SENS_ACC_XSCALE"), &(accel_scale[0]))
|| param_set(param_find("SENS_ACC_YSCALE"), &(accel_scale[1]))
|| param_set(param_find("SENS_ACC_ZSCALE"), &(accel_scale[2]))) {
mavlink_log_critical(mavlink_fd, "ERROR: setting offs or scale failed");
}
int fd = open(ACCEL_DEVICE_PATH, 0);
struct accel_scale ascale = {
accel_offs[0],
accel_scale[0],
accel_offs[1],
accel_scale[1],
accel_offs[2],
accel_scale[2],
};
if (OK != ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale))
warn("WARNING: failed to set scale / offsets for accel");
close(fd);
/* auto-save to EEPROM */
int save_ret = param_save_default();
if (save_ret != 0) {
warn("WARNING: auto-save of params to storage failed");
}
mavlink_log_info(mavlink_fd, "accel calibration done");
tune_confirm();
sleep(2);
tune_confirm();
sleep(2);
/* third beep by cal end routine */
} else {
/* measurements error */
mavlink_log_info(mavlink_fd, "accel calibration aborted");
tune_error();
sleep(2);
}
/* exit accel calibration mode */
status->flag_preflight_accel_calibration = false;
state_machine_publish(status_pub, status, mavlink_fd);
}
int do_airspeed_calibration(int mavlink_fd)
{
/* give directions */
mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
const unsigned calibration_count = 2000;
int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
struct differential_pressure_s diff_pres;
float diff_pres_offset = 0.0f;
/* Reset sensor parameters */
struct airspeed_scale airscale = {
diff_pres_offset,
1.0f,
};
bool paramreset_successful = false;
int fd = open(AIRSPEED_DEVICE_PATH, 0);
if (fd > 0) {
if (OK == ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) {
paramreset_successful = true;
} else {
mavlink_log_critical(mavlink_fd, "airspeed offset zero failed");
}
close(fd);
}
if (!paramreset_successful) {
/* only warn if analog scaling is zero */
float analog_scaling = 0.0f;
param_get(param_find("SENS_DPRES_ANSC"), &(analog_scaling));
if (fabsf(analog_scaling) < 0.1f) {
mavlink_log_critical(mavlink_fd, "If analog sens, retry with [SENS_DPRES_ANSC=1000]");
close(diff_pres_sub);
return ERROR;
}
/* set scaling offset parameter */
if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG);
close(diff_pres_sub);
return ERROR;
}
}
unsigned calibration_counter = 0;
mavlink_log_critical(mavlink_fd, "Ensure sensor is not measuring wind");
usleep(500 * 1000);
while (calibration_counter < calibration_count) {
/* wait blocking for new data */
struct pollfd fds[1];
fds[0].fd = diff_pres_sub;
fds[0].events = POLLIN;
int poll_ret = poll(fds, 1, 1000);
if (poll_ret) {
orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres);
diff_pres_offset += diff_pres.differential_pressure_raw_pa;
calibration_counter++;
if (calibration_counter % (calibration_count / 20) == 0) {
mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter * 80) / calibration_count);
}
} else if (poll_ret == 0) {
/* any poll failure for 1s is a reason to abort */
mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name);
close(diff_pres_sub);
return ERROR;
}
}
diff_pres_offset = diff_pres_offset / calibration_count;
if (isfinite(diff_pres_offset)) {
int fd_scale = open(AIRSPEED_DEVICE_PATH, 0);
airscale.offset_pa = diff_pres_offset;
if (fd_scale > 0) {
if (OK != ioctl(fd_scale, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) {
mavlink_log_critical(mavlink_fd, "airspeed offset update failed");
}
close(fd_scale);
}
if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG);
close(diff_pres_sub);
return ERROR;
}
/* auto-save to EEPROM */
int save_ret = param_save_default();
if (save_ret != 0) {
warn("WARNING: auto-save of params to storage failed");
mavlink_log_critical(mavlink_fd, CAL_FAILED_SAVE_PARAMS_MSG);
close(diff_pres_sub);
return ERROR;
}
} else {
mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name);
close(diff_pres_sub);
return ERROR;
}
mavlink_log_critical(mavlink_fd, "Offset of %d Pa, create airflow now!", (int)diff_pres_offset);
/* wait 500 ms to ensure parameter propagated through the system */
usleep(500 * 1000);
calibration_counter = 0;
const int maxcount = 3000;
/* just take a few samples and make sure pitot tubes are not reversed, timeout after ~30 seconds */
while (calibration_counter < maxcount) {
/* wait blocking for new data */
struct pollfd fds[1];
fds[0].fd = diff_pres_sub;
fds[0].events = POLLIN;
int poll_ret = poll(fds, 1, 1000);
if (poll_ret) {
orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres);
calibration_counter++;
if (fabsf(diff_pres.differential_pressure_raw_pa) < 50.0f) {
if (calibration_counter % 100 == 0) {
mavlink_log_critical(mavlink_fd, "Missing airflow! (%d, wanted: 50 Pa, #h101)",
(int)diff_pres.differential_pressure_raw_pa);
}
continue;
}
/* do not allow negative values */
if (diff_pres.differential_pressure_raw_pa < 0.0f) {
mavlink_log_info(mavlink_fd, "negative pressure: ERROR (%d Pa)",
(int)diff_pres.differential_pressure_raw_pa);
mavlink_log_critical(mavlink_fd, "%d Pa: swap static and dynamic ports!", (int)diff_pres.differential_pressure_raw_pa);
close(diff_pres_sub);
/* the user setup is wrong, wipe the calibration to force a proper re-calibration */
diff_pres_offset = 0.0f;
if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG);
close(diff_pres_sub);
return ERROR;
}
/* save */
mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 0);
(void)param_save_default();
close(diff_pres_sub);
mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name);
return ERROR;
} else {
mavlink_log_info(mavlink_fd, "positive pressure: OK (%d Pa)",
(int)diff_pres.differential_pressure_raw_pa);
break;
}
} else if (poll_ret == 0) {
/* any poll failure for 1s is a reason to abort */
mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name);
close(diff_pres_sub);
return ERROR;
}
}
if (calibration_counter == maxcount) {
mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name);
close(diff_pres_sub);
return ERROR;
}
mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 100);
mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name);
tune_neutral(true);
close(diff_pres_sub);
return OK;
}
void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg)
{
switch (msg->msgid) {
case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: {
mavlink_param_request_list_t req;
mavlink_msg_param_request_list_decode(msg, &req);
if (req.target_system == mavlink_system.sysid &&
(req.target_component == mavlink_system.compid || req.target_component == MAV_COMP_ID_ALL)) {
/* Start sending parameters */
mavlink_pm_start_queued_send();
send_statustext_info("[pm] sending list");
}
} break;
case MAVLINK_MSG_ID_PARAM_SET: {
/* Handle parameter setting */
if (msg->msgid == MAVLINK_MSG_ID_PARAM_SET) {
mavlink_param_set_t mavlink_param_set;
mavlink_msg_param_set_decode(msg, &mavlink_param_set);
if (mavlink_param_set.target_system == mavlink_system.sysid
&& ((mavlink_param_set.target_component == mavlink_system.compid)
|| (mavlink_param_set.target_component == MAV_COMP_ID_ALL))) {
/* local name buffer to enforce null-terminated string */
char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1];
strncpy(name, mavlink_param_set.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
/* enforce null termination */
name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0';
/* attempt to find parameter, set and send it */
param_t param = param_find(name);
if (param == PARAM_INVALID) {
char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN];
sprintf(buf, "[pm] unknown: %s", name);
send_statustext_info(buf);
} else {
/* set and send parameter */
param_set(param, &(mavlink_param_set.param_value));
mavlink_pm_send_param(param);
}
}
}
} break;
case MAVLINK_MSG_ID_PARAM_REQUEST_READ: {
mavlink_param_request_read_t mavlink_param_request_read;
mavlink_msg_param_request_read_decode(msg, &mavlink_param_request_read);
if (mavlink_param_request_read.target_system == mavlink_system.sysid
&& ((mavlink_param_request_read.target_component == mavlink_system.compid)
|| (mavlink_param_request_read.target_component == MAV_COMP_ID_ALL))) {
/* when no index is given, loop through string ids and compare them */
if (mavlink_param_request_read.param_index == -1) {
/* local name buffer to enforce null-terminated string */
char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1];
strncpy(name, mavlink_param_request_read.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
/* enforce null termination */
name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0';
/* attempt to find parameter and send it */
mavlink_pm_send_param_for_name(name);
} else {
/* when index is >= 0, send this parameter again */
mavlink_pm_send_param_for_index(mavlink_param_request_read.param_index);
}
}
} break;
}
}
int do_mag_calibration(int mavlink_fd)
{
mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
mavlink_log_info(mavlink_fd, "don't move system");
/* 45 seconds */
uint64_t calibration_interval = 45 * 1000 * 1000;
/* maximum 500 values */
const unsigned int calibration_maxcount = 500;
unsigned int calibration_counter;
struct mag_scale mscale_null = {
0.0f,
1.0f,
0.0f,
1.0f,
0.0f,
1.0f,
};
int res = OK;
/* erase old calibration */
int fd = open(MAG_DEVICE_PATH, O_RDONLY);
res = ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null);
if (res != OK) {
mavlink_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG);
}
if (res == OK) {
/* calibrate range */
res = ioctl(fd, MAGIOCCALIBRATE, fd);
if (res != OK) {
mavlink_log_critical(mavlink_fd, "Skipped scale calibration");
/* this is non-fatal - mark it accordingly */
res = OK;
}
}
close(fd);
float *x = NULL;
float *y = NULL;
float *z = NULL;
if (res == OK) {
/* allocate memory */
mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 20);
x = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_maxcount));
y = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_maxcount));
z = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_maxcount));
if (x == NULL || y == NULL || z == NULL) {
mavlink_log_critical(mavlink_fd, "ERROR: out of memory");
res = ERROR;
return res;
}
} else {
/* exit */
return ERROR;
}
if (res == OK) {
int sub_mag = orb_subscribe(ORB_ID(sensor_mag));
struct mag_report mag;
/* limit update rate to get equally spaced measurements over time (in ms) */
orb_set_interval(sub_mag, (calibration_interval / 1000) / calibration_maxcount);
/* calibrate offsets */
uint64_t calibration_deadline = hrt_absolute_time() + calibration_interval;
unsigned poll_errcount = 0;
mavlink_log_info(mavlink_fd, "rotate in a figure 8 around all axis");
calibration_counter = 0;
while (hrt_absolute_time() < calibration_deadline &&
calibration_counter < calibration_maxcount) {
/* wait blocking for new data */
struct pollfd fds[1];
fds[0].fd = sub_mag;
fds[0].events = POLLIN;
int poll_ret = poll(fds, 1, 1000);
if (poll_ret > 0) {
orb_copy(ORB_ID(sensor_mag), sub_mag, &mag);
x[calibration_counter] = mag.x;
y[calibration_counter] = mag.y;
z[calibration_counter] = mag.z;
calibration_counter++;
if (calibration_counter % (calibration_maxcount / 20) == 0)
mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 20 + (calibration_counter * 50) / calibration_maxcount);
} else {
poll_errcount++;
}
if (poll_errcount > 1000) {
mavlink_log_critical(mavlink_fd, CAL_FAILED_SENSOR_MSG);
res = ERROR;
break;
}
}
close(sub_mag);
}
float sphere_x;
float sphere_y;
float sphere_z;
float sphere_radius;
if (res == OK) {
/* sphere fit */
mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 70);
sphere_fit_least_squares(x, y, z, calibration_counter, 100, 0.0f, &sphere_x, &sphere_y, &sphere_z, &sphere_radius);
mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 80);
if (!isfinite(sphere_x) || !isfinite(sphere_y) || !isfinite(sphere_z)) {
mavlink_log_critical(mavlink_fd, "ERROR: NaN in sphere fit");
res = ERROR;
}
}
if (x != NULL)
free(x);
if (y != NULL)
free(y);
if (z != NULL)
free(z);
if (res == OK) {
/* apply calibration and set parameters */
struct mag_scale mscale;
fd = open(MAG_DEVICE_PATH, 0);
res = ioctl(fd, MAGIOCGSCALE, (long unsigned int)&mscale);
if (res != OK) {
mavlink_log_critical(mavlink_fd, "ERROR: failed to get current calibration");
}
if (res == OK) {
mscale.x_offset = sphere_x;
mscale.y_offset = sphere_y;
mscale.z_offset = sphere_z;
res = ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale);
if (res != OK) {
mavlink_log_critical(mavlink_fd, CAL_FAILED_APPLY_CAL_MSG);
}
}
close(fd);
if (res == OK) {
/* set parameters */
if (param_set(param_find("SENS_MAG_XOFF"), &(mscale.x_offset)))
res = ERROR;
if (param_set(param_find("SENS_MAG_YOFF"), &(mscale.y_offset)))
res = ERROR;
if (param_set(param_find("SENS_MAG_ZOFF"), &(mscale.z_offset)))
res = ERROR;
if (param_set(param_find("SENS_MAG_XSCALE"), &(mscale.x_scale)))
res = ERROR;
if (param_set(param_find("SENS_MAG_YSCALE"), &(mscale.y_scale)))
res = ERROR;
if (param_set(param_find("SENS_MAG_ZSCALE"), &(mscale.z_scale)))
res = ERROR;
if (res != OK) {
mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG);
}
mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 90);
}
if (res == OK) {
/* auto-save to EEPROM */
res = param_save_default();
if (res != OK) {
mavlink_log_critical(mavlink_fd, CAL_FAILED_SAVE_PARAMS_MSG);
}
}
mavlink_log_info(mavlink_fd, "mag off: x:%.2f y:%.2f z:%.2f Ga", (double)mscale.x_offset,
(double)mscale.y_offset, (double)mscale.z_offset);
mavlink_log_info(mavlink_fd, "mag scale: x:%.2f y:%.2f z:%.2f", (double)mscale.x_scale,
(double)mscale.y_scale, (double)mscale.z_scale);
if (res == OK) {
mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name);
} else {
mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name);
}
}
return res;
}
CameraTrigger::CameraTrigger() :
_engagecall {},
_disengagecall {},
_engage_turn_on_off_call {},
_disengage_turn_on_off_call {},
_keepalivecall_up {},
_keepalivecall_down {},
_activation_time(0.5f /* ms */),
_interval(100.0f /* ms */),
_distance(25.0f /* m */),
_trigger_seq(0),
_trigger_enabled(false),
_trigger_paused(false),
_one_shot(false),
_test_shot(false),
_turning_on(false),
_last_shoot_position(0.0f, 0.0f),
_valid_position(false),
_command_sub(-1),
_lpos_sub(-1),
_trigger_pub(nullptr),
_cmd_ack_pub(nullptr),
_trigger_mode(TRIGGER_MODE_NONE),
_camera_interface_mode(CAMERA_INTERFACE_MODE_GPIO),
_camera_interface(nullptr)
{
// Initiate camera interface based on camera_interface_mode
if (_camera_interface != nullptr) {
delete (_camera_interface);
// set to zero to ensure parser is not used while not instantiated
_camera_interface = nullptr;
}
memset(&_work, 0, sizeof(_work));
// Parameters
_p_interval = param_find("TRIG_INTERVAL");
_p_distance = param_find("TRIG_DISTANCE");
_p_activation_time = param_find("TRIG_ACT_TIME");
_p_mode = param_find("TRIG_MODE");
_p_interface = param_find("TRIG_INTERFACE");
param_get(_p_activation_time, &_activation_time);
param_get(_p_interval, &_interval);
param_get(_p_distance, &_distance);
param_get(_p_mode, &_trigger_mode);
param_get(_p_interface, &_camera_interface_mode);
switch (_camera_interface_mode) {
#ifdef __PX4_NUTTX
case CAMERA_INTERFACE_MODE_GPIO:
_camera_interface = new CameraInterfaceGPIO();
break;
case CAMERA_INTERFACE_MODE_GENERIC_PWM:
_camera_interface = new CameraInterfacePWM();
break;
case CAMERA_INTERFACE_MODE_SEAGULL_MAP2_PWM:
_camera_interface = new CameraInterfaceSeagull();
break;
#endif
case CAMERA_INTERFACE_MODE_MAVLINK:
// start an interface that does nothing. Instead mavlink will listen to the camera_trigger uORB message
_camera_interface = new CameraInterface();
break;
default:
PX4_ERR("unknown camera interface mode: %i", (int)_camera_interface_mode);
break;
}
// Enforce a lower bound on the activation interval in PWM modes to not miss
// engage calls in-between 50Hz PWM pulses. (see PX4 PR #6973)
if ((_activation_time < 40.0f) &&
(_camera_interface_mode == CAMERA_INTERFACE_MODE_GENERIC_PWM ||
_camera_interface_mode == CAMERA_INTERFACE_MODE_SEAGULL_MAP2_PWM)) {
_activation_time = 40.0f;
PX4_WARN("Trigger interval too low for PWM interface, setting to 40 ms");
param_set(_p_activation_time, &(_activation_time));
}
// Advertise critical publishers here, because we cannot advertise in interrupt context
struct camera_trigger_s trigger = {};
_trigger_pub = orb_advertise(ORB_ID(camera_trigger), &trigger);
}
int do_accel_calibration(int mavlink_fd)
{
int fd;
mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
struct accel_scale accel_scale = {
0.0f,
1.0f,
0.0f,
1.0f,
0.0f,
1.0f,
};
int res = OK;
/* reset all offsets to zero and all scales to one */
fd = open(ACCEL_DEVICE_PATH, 0);
res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale);
close(fd);
if (res != OK) {
mavlink_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG);
}
float accel_offs[3];
float accel_T[3][3];
if (res == OK) {
/* measure and calculate offsets & scales */
res = do_accel_calibration_measurements(mavlink_fd, accel_offs, accel_T);
}
if (res == OK) {
/* measurements completed successfully, rotate calibration values */
param_t board_rotation_h = param_find("SENS_BOARD_ROT");
int32_t board_rotation_int;
param_get(board_rotation_h, &(board_rotation_int));
enum Rotation board_rotation_id = (enum Rotation)board_rotation_int;
math::Matrix<3, 3> board_rotation;
get_rot_matrix(board_rotation_id, &board_rotation);
math::Matrix<3, 3> board_rotation_t = board_rotation.transposed();
math::Vector<3> accel_offs_vec(&accel_offs[0]);
math::Vector<3> accel_offs_rotated = board_rotation_t *accel_offs_vec;
math::Matrix<3, 3> accel_T_mat(&accel_T[0][0]);
math::Matrix<3, 3> accel_T_rotated = board_rotation_t *accel_T_mat * board_rotation;
accel_scale.x_offset = accel_offs_rotated(0);
accel_scale.x_scale = accel_T_rotated(0, 0);
accel_scale.y_offset = accel_offs_rotated(1);
accel_scale.y_scale = accel_T_rotated(1, 1);
accel_scale.z_offset = accel_offs_rotated(2);
accel_scale.z_scale = accel_T_rotated(2, 2);
/* set parameters */
if (param_set(param_find("SENS_ACC_XOFF"), &(accel_scale.x_offset))
|| param_set(param_find("SENS_ACC_YOFF"), &(accel_scale.y_offset))
|| param_set(param_find("SENS_ACC_ZOFF"), &(accel_scale.z_offset))
|| param_set(param_find("SENS_ACC_XSCALE"), &(accel_scale.x_scale))
|| param_set(param_find("SENS_ACC_YSCALE"), &(accel_scale.y_scale))
|| param_set(param_find("SENS_ACC_ZSCALE"), &(accel_scale.z_scale))) {
mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG);
res = ERROR;
}
}
if (res == OK) {
/* apply new scaling and offsets */
fd = open(ACCEL_DEVICE_PATH, 0);
res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale);
close(fd);
if (res != OK) {
mavlink_log_critical(mavlink_fd, CAL_FAILED_APPLY_CAL_MSG);
}
}
if (res == OK) {
/* auto-save to EEPROM */
res = param_save_default();
if (res != OK) {
mavlink_log_critical(mavlink_fd, CAL_FAILED_SAVE_PARAMS_MSG);
}
}
if (res == OK) {
mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name);
} else {
mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name);
}
return res;
}
int do_mag_calibration(orb_advert_t *mavlink_log_pub)
{
calibration_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, sensor_name);
struct mag_calibration_s mscale_null;
mscale_null.x_offset = 0.0f;
mscale_null.x_scale = 1.0f;
mscale_null.y_offset = 0.0f;
mscale_null.y_scale = 1.0f;
mscale_null.z_offset = 0.0f;
mscale_null.z_scale = 1.0f;
int result = OK;
// Determine which mags are available and reset each
char str[30];
for (size_t i=0; i<max_mags; i++) {
device_ids[i] = 0; // signals no mag
}
for (unsigned cur_mag = 0; cur_mag < max_mags; cur_mag++) {
#ifndef __PX4_QURT
// Reset mag id to mag not available
(void)sprintf(str, "CAL_MAG%u_ID", cur_mag);
result = param_set_no_notification(param_find(str), &(device_ids[cur_mag]));
if (result != OK) {
calibration_log_info(mavlink_log_pub, "[cal] Unable to reset CAL_MAG%u_ID", cur_mag);
break;
}
#else
(void)sprintf(str, "CAL_MAG%u_XOFF", cur_mag);
result = param_set(param_find(str), &mscale_null.x_offset);
if (result != OK) {
PX4_ERR("unable to reset %s", str);
}
(void)sprintf(str, "CAL_MAG%u_YOFF", cur_mag);
result = param_set(param_find(str), &mscale_null.y_offset);
if (result != OK) {
PX4_ERR("unable to reset %s", str);
}
(void)sprintf(str, "CAL_MAG%u_ZOFF", cur_mag);
result = param_set(param_find(str), &mscale_null.z_offset);
if (result != OK) {
PX4_ERR("unable to reset %s", str);
}
(void)sprintf(str, "CAL_MAG%u_XSCALE", cur_mag);
result = param_set(param_find(str), &mscale_null.x_scale);
if (result != OK) {
PX4_ERR("unable to reset %s", str);
}
(void)sprintf(str, "CAL_MAG%u_YSCALE", cur_mag);
result = param_set(param_find(str), &mscale_null.y_scale);
if (result != OK) {
PX4_ERR("unable to reset %s", str);
}
(void)sprintf(str, "CAL_MAG%u_ZSCALE", cur_mag);
result = param_set(param_find(str), &mscale_null.z_scale);
if (result != OK) {
PX4_ERR("unable to reset %s", str);
}
#endif
/* for calibration, commander will run on apps, so orb messages are used to get info from dsp */
#ifndef __PX4_QURT
// Attempt to open mag
(void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, cur_mag);
int fd = px4_open(str, O_RDONLY);
if (fd < 0) {
continue;
}
// Get device id for this mag
device_ids[cur_mag] = px4_ioctl(fd, DEVIOCGDEVICEID, 0);
internal[cur_mag] = (px4_ioctl(fd, MAGIOCGEXTERNAL, 0) <= 0);
// Reset mag scale
result = px4_ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null);
if (result != OK) {
calibration_log_critical(mavlink_log_pub, CAL_ERROR_RESET_CAL_MSG, cur_mag);
}
/* calibrate range */
if (result == OK) {
result = px4_ioctl(fd, MAGIOCCALIBRATE, fd);
if (result != OK) {
calibration_log_info(mavlink_log_pub, "[cal] Skipped scale calibration, sensor %u", cur_mag);
/* this is non-fatal - mark it accordingly */
result = OK;
}
}
px4_close(fd);
#endif
}
// Calibrate all mags at the same time
if (result == OK) {
switch (mag_calibrate_all(mavlink_log_pub, device_ids)) {
case calibrate_return_cancelled:
// Cancel message already displayed, we're done here
result = ERROR;
break;
case calibrate_return_ok:
/* auto-save to EEPROM */
result = param_save_default();
/* if there is a any preflight-check system response, let the barrage of messages through */
usleep(200000);
if (result == OK) {
calibration_log_info(mavlink_log_pub, CAL_QGC_PROGRESS_MSG, 100);
calibration_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, sensor_name);
break;
} else {
calibration_log_critical(mavlink_log_pub, CAL_ERROR_SAVE_PARAMS_MSG);
}
// Fall through
default:
calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, sensor_name);
break;
}
}
/* give this message enough time to propagate */
usleep(600000);
return result;
}
int do_airspeed_calibration(int mavlink_fd)
{
int result = OK;
unsigned calibration_counter = 0;
const unsigned maxcount = 3000;
/* give directions */
mavlink_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name);
const unsigned calibration_count = 2000;
int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
struct differential_pressure_s diff_pres;
float diff_pres_offset = 0.0f;
/* Reset sensor parameters */
struct airspeed_scale airscale = {
diff_pres_offset,
1.0f,
};
bool paramreset_successful = false;
int fd = px4_open(AIRSPEED0_DEVICE_PATH, 0);
if (fd > 0) {
if (OK == px4_ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) {
paramreset_successful = true;
} else {
mavlink_log_critical(mavlink_fd, "[cal] airspeed offset zero failed");
}
px4_close(fd);
}
int cancel_sub = calibrate_cancel_subscribe();
if (!paramreset_successful) {
/* only warn if analog scaling is zero */
float analog_scaling = 0.0f;
param_get(param_find("SENS_DPRES_ANSC"), &(analog_scaling));
if (fabsf(analog_scaling) < 0.1f) {
mavlink_log_critical(mavlink_fd, "[cal] No airspeed sensor, see http://px4.io/help/aspd");
goto error_return;
}
/* set scaling offset parameter */
if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
mavlink_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG);
goto error_return;
}
}
mavlink_log_critical(mavlink_fd, "[cal] Ensure sensor is not measuring wind");
usleep(500 * 1000);
while (calibration_counter < calibration_count) {
if (calibrate_cancel_check(mavlink_fd, cancel_sub)) {
goto error_return;
}
/* wait blocking for new data */
px4_pollfd_struct_t fds[1];
fds[0].fd = diff_pres_sub;
fds[0].events = POLLIN;
int poll_ret = px4_poll(fds, 1, 1000);
if (poll_ret) {
orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres);
diff_pres_offset += diff_pres.differential_pressure_raw_pa;
calibration_counter++;
if (calibration_counter % (calibration_count / 20) == 0) {
mavlink_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, (calibration_counter * 80) / calibration_count);
}
} else if (poll_ret == 0) {
/* any poll failure for 1s is a reason to abort */
feedback_calibration_failed(mavlink_fd);
goto error_return;
}
}
diff_pres_offset = diff_pres_offset / calibration_count;
if (PX4_ISFINITE(diff_pres_offset)) {
int fd_scale = px4_open(AIRSPEED0_DEVICE_PATH, 0);
airscale.offset_pa = diff_pres_offset;
if (fd_scale > 0) {
if (OK != px4_ioctl(fd_scale, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) {
mavlink_log_critical(mavlink_fd, "[cal] airspeed offset update failed");
}
px4_close(fd_scale);
}
if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
mavlink_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG);
goto error_return;
}
/* auto-save to EEPROM */
int save_ret = param_save_default();
if (save_ret != 0) {
warn("WARNING: auto-save of params to storage failed");
mavlink_log_critical(mavlink_fd, CAL_ERROR_SAVE_PARAMS_MSG);
goto error_return;
}
} else {
feedback_calibration_failed(mavlink_fd);
goto error_return;
}
mavlink_log_critical(mavlink_fd, "[cal] Offset of %d Pascal", (int)diff_pres_offset);
/* wait 500 ms to ensure parameter propagated through the system */
usleep(500 * 1000);
mavlink_log_critical(mavlink_fd, "[cal] Create airflow now");
calibration_counter = 0;
/* just take a few samples and make sure pitot tubes are not reversed, timeout after ~30 seconds */
while (calibration_counter < maxcount) {
if (calibrate_cancel_check(mavlink_fd, cancel_sub)) {
goto error_return;
}
/* wait blocking for new data */
px4_pollfd_struct_t fds[1];
fds[0].fd = diff_pres_sub;
fds[0].events = POLLIN;
int poll_ret = px4_poll(fds, 1, 1000);
if (poll_ret) {
orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres);
calibration_counter++;
if (fabsf(diff_pres.differential_pressure_raw_pa) < 50.0f) {
if (calibration_counter % 500 == 0) {
mavlink_log_info(mavlink_fd, "[cal] Create air pressure! (got %d, wanted: 50 Pa)",
(int)diff_pres.differential_pressure_raw_pa);
}
continue;
}
/* do not allow negative values */
if (diff_pres.differential_pressure_raw_pa < 0.0f) {
mavlink_log_info(mavlink_fd, "[cal] Negative pressure difference detected (%d Pa)",
(int)diff_pres.differential_pressure_raw_pa);
mavlink_log_info(mavlink_fd, "[cal] Swap static and dynamic ports!");
/* the user setup is wrong, wipe the calibration to force a proper re-calibration */
diff_pres_offset = 0.0f;
if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
mavlink_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG);
goto error_return;
}
/* save */
mavlink_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, 0);
(void)param_save_default();
feedback_calibration_failed(mavlink_fd);
goto error_return;
} else {
mavlink_log_info(mavlink_fd, "[cal] Positive pressure: OK (%d Pa)",
(int)diff_pres.differential_pressure_raw_pa);
break;
}
} else if (poll_ret == 0) {
/* any poll failure for 1s is a reason to abort */
feedback_calibration_failed(mavlink_fd);
goto error_return;
}
}
if (calibration_counter == maxcount) {
feedback_calibration_failed(mavlink_fd);
goto error_return;
}
mavlink_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, 100);
mavlink_log_info(mavlink_fd, CAL_QGC_DONE_MSG, sensor_name);
tune_neutral(true);
normal_return:
calibrate_cancel_unsubscribe(cancel_sub);
px4_close(diff_pres_sub);
// This give a chance for the log messages to go out of the queue before someone else stomps on then
sleep(1);
return result;
error_return:
result = ERROR;
goto normal_return;
}
void popup_param_t::item_picked( int index)
{
param_set()->begin_edit();
set_value( index);
param_set()->end_edit();
}
static void
do_set(const char *name, const char *val, bool fail_on_not_found)
{
int32_t i;
float f;
param_t param = param_find(name);
/* set nothing if parameter cannot be found */
if (param == PARAM_INVALID) {
/* param not found - fail silenty in scripts as it prevents booting */
errx(((fail_on_not_found) ? 1 : 0), "Error: Parameter %s not found.", name);
}
printf("%c %s: ",
param_value_unsaved(param) ? '*' : (param_value_is_default(param) ? ' ' : '+'),
param_name(param));
/*
* Set parameter if type is known and conversion from string to value turns out fine
*/
switch (param_type(param)) {
case PARAM_TYPE_INT32:
if (!param_get(param, &i)) {
/* convert string */
char *end;
int32_t newval = strtol(val, &end, 10);
if (i == newval) {
printf("unchanged\n");
} else {
printf("curr: %d", i);
param_set(param, &newval);
printf(" -> new: %d\n", newval);
}
}
break;
case PARAM_TYPE_FLOAT:
if (!param_get(param, &f)) {
/* convert string */
char *end;
float newval = strtod(val, &end);
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wfloat-equal"
if (f == newval) {
#pragma GCC diagnostic pop
printf("unchanged\n");
} else {
printf("curr: %4.4f", (double)f);
param_set(param, &newval);
printf(" -> new: %4.4f\n", (double)newval);
}
}
break;
default:
errx(1, "<unknown / unsupported type %d>\n", 0 + param_type(param));
}
exit(0);
}
static void
radio_init(void)
{
__pdata uint32_t freq_min, freq_max;
__pdata uint32_t channel_spacing;
__pdata uint8_t txpower;
// Do generic PHY initialisation
if (!radio_initialise()) {
panic("radio_initialise failed");
}
switch (g_board_frequency) {
case FREQ_433:
freq_min = 433050000UL;
freq_max = 434790000UL;
txpower = 10;
num_fh_channels = 10;
break;
case FREQ_470:
freq_min = 470000000UL;
freq_max = 471000000UL;
txpower = 10;
num_fh_channels = 10;
break;
case FREQ_868:
freq_min = 868000000UL;
freq_max = 869000000UL;
txpower = 10;
num_fh_channels = 10;
break;
case FREQ_915:
freq_min = 915000000UL;
freq_max = 928000000UL;
txpower = 20;
num_fh_channels = MAX_FREQ_CHANNELS;
break;
default:
freq_min = 0;
freq_max = 0;
txpower = 0;
panic("bad board frequency %d", g_board_frequency);
break;
}
if (param_get(PARAM_NUM_CHANNELS) != 0) {
num_fh_channels = param_get(PARAM_NUM_CHANNELS);
}
if (param_get(PARAM_MIN_FREQ) != 0) {
freq_min = param_get(PARAM_MIN_FREQ) * 1000UL;
}
if (param_get(PARAM_MAX_FREQ) != 0) {
freq_max = param_get(PARAM_MAX_FREQ) * 1000UL;
}
if (param_get(PARAM_TXPOWER) != 0) {
txpower = param_get(PARAM_TXPOWER);
}
// constrain power and channels
txpower = constrain(txpower, BOARD_MINTXPOWER, BOARD_MAXTXPOWER);
num_fh_channels = constrain(num_fh_channels, 1, MAX_FREQ_CHANNELS);
// double check ranges the board can do
switch (g_board_frequency) {
case FREQ_433:
freq_min = constrain(freq_min, 414000000UL, 460000000UL);
freq_max = constrain(freq_max, 414000000UL, 460000000UL);
break;
case FREQ_470:
freq_min = constrain(freq_min, 450000000UL, 490000000UL);
freq_max = constrain(freq_max, 450000000UL, 490000000UL);
break;
case FREQ_868:
freq_min = constrain(freq_min, 849000000UL, 889000000UL);
freq_max = constrain(freq_max, 849000000UL, 889000000UL);
break;
case FREQ_915:
freq_min = constrain(freq_min, 868000000UL, 935000000UL);
freq_max = constrain(freq_max, 868000000UL, 935000000UL);
break;
default:
panic("bad board frequency %d", g_board_frequency);
break;
}
if (freq_max == freq_min) {
freq_max = freq_min + 1000000UL;
}
// get the duty cycle we will use
duty_cycle = param_get(PARAM_DUTY_CYCLE);
duty_cycle = constrain(duty_cycle, 0, 100);
param_set(PARAM_DUTY_CYCLE, duty_cycle);
// get the LBT threshold we will use
lbt_rssi = param_get(PARAM_LBT_RSSI);
if (lbt_rssi != 0) {
// limit to the RSSI valid range
lbt_rssi = constrain(lbt_rssi, 25, 220);
}
param_set(PARAM_LBT_RSSI, lbt_rssi);
// sanity checks
param_set(PARAM_MIN_FREQ, freq_min/1000);
param_set(PARAM_MAX_FREQ, freq_max/1000);
param_set(PARAM_NUM_CHANNELS, num_fh_channels);
channel_spacing = (freq_max - freq_min) / (num_fh_channels+2);
// add half of the channel spacing, to ensure that we are well
// away from the edges of the allowed range
freq_min += channel_spacing/2;
// add another offset based on network ID. This means that
// with different network IDs we will have much lower
// interference
srand(param_get(PARAM_NETID));
if (num_fh_channels > 5) {
freq_min += ((unsigned long)(rand()*625)) % channel_spacing;
}
debug("freq low=%lu high=%lu spacing=%lu\n",
freq_min, freq_min+(num_fh_channels*channel_spacing),
channel_spacing);
// set the frequency and channel spacing
// change base freq based on netid
radio_set_frequency(freq_min);
// set channel spacing
radio_set_channel_spacing(channel_spacing);
// start on a channel chosen by network ID
radio_set_channel(param_get(PARAM_NETID) % num_fh_channels);
// And intilise the radio with them.
if (!radio_configure(param_get(PARAM_AIR_SPEED)) &&
!radio_configure(param_get(PARAM_AIR_SPEED)) &&
!radio_configure(param_get(PARAM_AIR_SPEED))) {
panic("radio_configure failed");
}
// report the real air data rate in parameters
param_set(PARAM_AIR_SPEED, radio_air_rate());
// setup network ID
radio_set_network_id(param_get(PARAM_NETID));
// setup transmit power
radio_set_transmit_power(txpower);
// report the real transmit power in settings
param_set(PARAM_TXPOWER, radio_get_transmit_power());
#ifdef USE_RTC
// initialise real time clock
rtc_init();
#endif
// initialise frequency hopping system
fhop_init(param_get(PARAM_NETID));
// initialise TDM system
tdm_init();
}
void HttpHandler::handle_update(const char* method, const char* path, const http_options* options, int fd)
{
const char *action = NULL;
const char *fast_level = NULL;
const char *fast_enabled = NULL;
const char *suppression = NULL;
const char *http_thread = NULL;
const char *resolution = NULL;
syslog(LOG_INFO, "Received HTTP Request: %s", path);
if (!(action = net_http_option(options, "action"))) {
goto bad_request;
}
syslog(LOG_INFO, "action=%s", action);
if (string(action) == "set")
{
if ((fast_level = net_http_option(options, "level")))
{
if (param_set(PARAM_LEVEL, fast_level, 1) < 0)
{
syslog(LOG_CRIT, "Failed to set parameter: %s to: %s", PARAM_LEVEL, fast_level);
goto server_error;
}
}
else if ((suppression = net_http_option(options, "suppression")))
{
if (param_set(PARAM_SUPPRESSION, suppression, 1) < 0)
{
syslog(LOG_CRIT, "Failed to set parameter: %s to: %s", PARAM_SUPPRESSION, suppression);
goto server_error;
}
}
else if ((http_thread = net_http_option(options, "http_thread")))
{
if(threadHandler->is_running())
threadHandler->stop_thread_request();
}
else if ((resolution = net_http_option(options, "res")))
{
if (param_set(PARAM_RES, resolution, 1) < 0)
{
syslog(LOG_CRIT, "Failed to set parameter: %s to: %s", PARAM_RES, resolution);
goto server_error;
}
resolution_changed = true;
}
if (net_http_send_headers(fd,
HTTP_TIMEOUT,
txt_HTTP_HEADER_200,
txt_Content_Type_text_html_utf8,
txt_CRLF,
NULL) < 0) {
goto response_failure;
}
syslog(LOG_INFO, "Response sent!");
close(fd);
return;
}
bad_request:
syslog(LOG_WARNING, "Bad Request!");
if (net_http_send_string_utf8(fd, HTTP_TIMEOUT,
txt_HTTP_RESPONSE_400) < 0) {
goto response_failure;
}
close(fd);
return;
response_failure:
syslog(LOG_WARNING, "Failed to send HTTP response!");
close(fd);
return;
server_error:
if (net_http_send_string_utf8(fd, HTTP_TIMEOUT,
txt_HTTP_RESPONSE_500) < 0) {
syslog(LOG_WARNING, "Failed to send HTTP response!");
}
close(fd);
net_http_cleanup();
exit(EXIT_FAILURE);
}
