PGM_P MNMClass::getModelParamName(uint8_t model, uint8_t param) {
if (param >= 8) {
return get_param_name(mnm_generic_param_names, param);
} else {
return get_param_name(get_model_param_names(model), param);
}
}
PGM_P ot_model_param_name(uint8_t model, uint8_t param) {
if (param >= OT_AMP_ATTACK) {
return get_param_name(ot_generic_names, param);
} else {
return get_param_name(get_model_param_names(model), param);
}
}
void AP_Gimbal_Parameters::update()
{
uint32_t tnow_ms = hal.scheduler->millis();
// retry initial param retrieval
if(!received_all()){
if (tnow_ms-_last_request_ms > _retry_period) {
_last_request_ms = tnow_ms;
mavlink_msg_param_request_list_send(_chan, 0, MAV_COMP_ID_GIMBAL);
for(uint8_t i=0; i<MAVLINK_GIMBAL_NUM_TRACKED_PARAMS; i++) {
if (!_params[i].seen) {
_params[i].fetch_attempts++;
}
}
}
}
// retry param_set
for(uint8_t i=0; i<MAVLINK_GIMBAL_NUM_TRACKED_PARAMS; i++) {
if (_params[i].state == GMB_PARAMSTATE_ATTEMPTING_TO_SET && tnow_ms - _last_request_ms > _retry_period) {
mavlink_msg_param_set_send(_chan, 0, MAV_COMP_ID_GIMBAL, get_param_name((gmb_param_t)i), _params[i].value, MAV_PARAM_TYPE_REAL32);
if (!_params[i].seen) {
_params[i].fetch_attempts++;
}
}
}
// check for nonexistant parameters
for(uint8_t i=0; i<MAVLINK_GIMBAL_NUM_TRACKED_PARAMS; i++) {
if (!_params[i].seen && _params[i].fetch_attempts > _max_fetch_attempts) {
_params[i].state = GMB_PARAMSTATE_NONEXISTANT;
hal.console->printf("Gimbal parameter %s timed out\n", get_param_name((gmb_param_t)i));
}
}
if(_flashing_step == GMB_PARAM_FLASHING_WAITING_FOR_SET) {
bool done = true;
for(uint8_t i=0; i<MAVLINK_GIMBAL_NUM_TRACKED_PARAMS; i++) {
if (_params[i].state == GMB_PARAMSTATE_ATTEMPTING_TO_SET) {
done = false;
break;
}
}
if (done) {
_flashing_step = GMB_PARAM_FLASHING_WAITING_FOR_ACK;
set_param(GMB_PARAM_GMB_FLASH,69.0f);
}
}
}
void AP_Gimbal_Parameters::set_param(gmb_param_t param, float value) {
if ((_params[param].state == GMB_PARAMSTATE_CONSISTENT && param != GMB_PARAM_GMB_FLASH && _params[param].value == value) || _params[param].state == GMB_PARAMSTATE_NONEXISTANT) {
return;
}
_params[param].state = GMB_PARAMSTATE_ATTEMPTING_TO_SET;
_params[param].value = value;
mavlink_msg_param_set_send(_chan, 0, MAV_COMP_ID_GIMBAL, get_param_name(param), _params[param].value, MAV_PARAM_TYPE_REAL32);
_last_request_ms = hal.scheduler->millis();
}
void
multi_fit_engine_print_fit_results(struct multi_fit_engine *fit,
str_t text)
{
str_t pname;
size_t k, j, kp;
str_set_null(text);
str_init(pname, 16);
for(j = 0, kp = 0; j < fit->common_parameters->number; j++, kp++) {
fit_param_t *fp = fit->common_parameters->values + j;
get_param_name(fp, pname);
str_printf_add(text, "COMMON / %9s : %.6g\n", CSTR(pname),
gsl_vector_get(fit->results, j));
}
for(k = 0; k < fit->samples_number; k++) {
for(j = 0; j < fit->private_parameters->number; j++, kp++) {
fit_param_t *fp = fit->private_parameters->values + j;
get_param_name(fp, pname);
str_printf_add(text, "SAMPLE(%02i)/ %9s : %.6g\n", k, CSTR(pname),
gsl_vector_get(fit->results, kp));
}
}
str_append_c(text, "Residual Chi Square by sample:\n", '\n');
for(k = 0; k < fit->samples_number; k++) {
double chisq = gsl_vector_get(fit->chisq, k);
str_printf_add(text, "ChiSq(%02i): %g\n", k, chisq);
}
str_free(pname);
}
void fit_panel::setup()
{
param_matrix = new FXMatrix(scroll_window, 2, LAYOUT_SIDE_BOTTOM|LAYOUT_FILL_Y|MATRIX_BY_COLUMNS, 0, 0, 0, 0, DEFAULT_SPACING, DEFAULT_SPACING, DEFAULT_SPACING, DEFAULT_SPACING, 1, 1);
new FXLabel(param_matrix, "Range");
m_wl_entry = new FXTextField(param_matrix, 10, this, ID_SPECTR_RANGE, FRAME_SUNKEN|FRAME_THICK|LAYOUT_FILL_ROW);
config_spectral_range();
m_parameters.resize(m_fit->parameters_number());
Str pname;
FXString label_text;
int current_layer = 0;
for(unsigned k = 0; k < m_parameters.size(); k++) {
param_info* p = this->get_parameter_pointer(k);
m_fit->get_parameter(k, &p->fp);
if(p->fp.id == PID_LAYER_N && p->fp.layer_nb != current_layer) {
current_layer = p->fp.layer_nb;
label_text.format("Layer %i", current_layer);
FXLabel *lab = new FXLabel(param_matrix, label_text);
lab->setFont(®ressProApp()->bold_font);
new FXLabel(param_matrix, "");
}
get_param_name(&p->fp, pname.str());
FXString fxpname((const FXchar *) pname.cstr());
FXCheckButton* bt = new FXCheckButton(param_matrix, fxpname, this, ID_PARAM_SELECT);
FXTextField* tf = new fx_numeric_field(param_matrix, 10, this, ID_PARAM_VALUE, FRAME_SUNKEN|FRAME_THICK|TEXTFIELD_REAL|LAYOUT_FILL_ROW);
tf->setUserData(p);
bt->setUserData(p);
p->text_field = tf;
p->selected = false;
p->is_dirty = true;
}
m_undo_manager.clear();
m_canvas = new plot_canvas(this, NULL, 0, LAYOUT_FILL_X|LAYOUT_FILL_Y);
m_fit->config_plot(m_canvas);
}
void AP_Gimbal_Parameters::handle_param_value(DataFlash_Class *dataflash, mavlink_message_t *msg)
{
mavlink_param_value_t packet;
mavlink_msg_param_value_decode(msg, &packet);
dataflash->Log_Write_Parameter(packet.param_id, packet.param_value);
for(uint8_t i=0; i<MAVLINK_GIMBAL_NUM_TRACKED_PARAMS; i++) {
if (!strcmp(packet.param_id, get_param_name((gmb_param_t)i))) {
_params[i].seen = true;
switch(_params[i].state) {
case GMB_PARAMSTATE_NONEXISTANT:
case GMB_PARAMSTATE_NOT_YET_READ:
case GMB_PARAMSTATE_FETCH_AGAIN:
_params[i].value = packet.param_value;
_params[i].state = GMB_PARAMSTATE_CONSISTENT;
break;
case GMB_PARAMSTATE_CONSISTENT:
_params[i].value = packet.param_value;
break;
case GMB_PARAMSTATE_ATTEMPTING_TO_SET:
if (i == GMB_PARAM_GMB_FLASH) {
if (_flashing_step == GMB_PARAM_FLASHING_WAITING_FOR_ACK && packet.param_value == 1) {
_flashing_step = GMB_PARAM_NOT_FLASHING;
}
_params[i].value = 0;
_params[i].state = GMB_PARAMSTATE_CONSISTENT;
} else if (packet.param_value == _params[i].value) {
_params[i].state = GMB_PARAMSTATE_CONSISTENT;
}
break;
}
break;
}
}
}
void AP_Gimbal_Parameters::handle_param_value(DataFlash_Class *dataflash, mavlink_message_t *msg)
{
mavlink_param_value_t packet;
mavlink_msg_param_value_decode(msg, &packet);
if (flashing() && packet.param_value == 1 && !strcmp(packet.param_id, "GMB_FLASH")) {
mavlink_msg_command_long_send(_chan, 0, MAV_COMP_ID_GIMBAL, 42501, 0, 0, 0, 0, 0, 0, 0, 0);
reset();
return;
}
dataflash->Log_Write_Parameter(packet.param_id, packet.param_value);
for(uint8_t i=0; i<MAVLINK_GIMBAL_NUM_TRACKED_PARAMS; i++) {
if (!strcmp(packet.param_id, get_param_name((gmb_param_t)i))) {
_params[i].seen = true;
switch(_params[i].state) {
case GMB_PARAMSTATE_NONEXISTANT:
case GMB_PARAMSTATE_NOT_YET_READ:
case GMB_PARAMSTATE_FETCH_AGAIN:
_params[i].value = packet.param_value;
_params[i].state = GMB_PARAMSTATE_CONSISTENT;
break;
case GMB_PARAMSTATE_CONSISTENT:
_params[i].value = packet.param_value;
break;
case GMB_PARAMSTATE_ATTEMPTING_TO_SET:
if (i == GMB_PARAM_GMB_FLASH || packet.param_value == _params[i].value) {
_params[i].state = GMB_PARAMSTATE_CONSISTENT;
}
break;
}
break;
}
}
}
fit_window::fit_window(fit_manager* fit, FXApp* a,const FXString& name,FXIcon *ic,FXIcon *mi,FXuint opts,FXint x,FXint y,FXint w,FXint h,FXint pl,FXint pr,FXint pt,FXint pb,FXint hs,FXint vs)
: FXMainWindow(a, "Dispersion Fit", ic, mi, opts, x, y, w, h, pl, pr, pt, pb, hs, vs),
m_canvas(0), m_fit(fit)
{
// Menubar
menubar = new FXMenuBar(this, LAYOUT_SIDE_TOP|LAYOUT_FILL_X);
statusbar = new FXStatusBar(this, LAYOUT_SIDE_BOTTOM|LAYOUT_FILL_X|FRAME_RAISED|STATUSBAR_WITH_DRAGCORNER);
// fit menu
fitmenu = new FXMenuPane(this);
new FXMenuCommand(fitmenu, "&Run", NULL, this, ID_RUN_FIT);
new FXMenuTitle(menubar, "&Fit", NULL, fitmenu);
// plot menu
plotmenu = new FXMenuPane(this);
new FXMenuCommand(plotmenu, "&Auto Scale", NULL, this, ID_PLOT_SCALE);
new FXMenuTitle(menubar, "&Plot", NULL, plotmenu);
FXHorizontalFrame *mf = new FXHorizontalFrame(this, LAYOUT_FILL_X|LAYOUT_FILL_Y);
FXScrollWindow *iw = new FXScrollWindow(mf, VSCROLLER_ALWAYS | HSCROLLING_OFF | LAYOUT_FILL_Y);
FXMatrix *matrix = new FXMatrix(iw, 2, LAYOUT_SIDE_BOTTOM|LAYOUT_FILL_Y|MATRIX_BY_COLUMNS, 0, 0, 0, 0, DEFAULT_SPACING, DEFAULT_SPACING, DEFAULT_SPACING, DEFAULT_SPACING, 1, 1);
new FXLabel(matrix, "Range");
m_wl_entry = new FXTextField(matrix, 10, this, ID_SPECTR_RANGE, FRAME_SUNKEN|FRAME_THICK|LAYOUT_FILL_ROW);
{
double wls, wle, wld;
m_fit->get_sampling(wls, wle, wld);
if (wld == 0.0)
m_wl_entry->setText(FXStringFormat("%.3g-%.3g", wls, wle));
else
m_wl_entry->setText(FXStringFormat("%.3g-%.3g,%g", wls, wle, wld));
}
m_parameters.resize(m_fit->parameters_number());
m_bold_font = new FXFont(getApp(), "helvetica", 9, FXFont::Bold, FXFont::Italic);
Str pname;
FXString label_text;
int current_layer = 0;
for (unsigned k = 0; k < m_parameters.size(); k++)
{
param_info* p = this->get_parameter_pointer(k);
m_fit->get_parameter(k, &p->fp);
if (p->fp.id == PID_LAYER_N && p->fp.layer_nb != current_layer)
{
current_layer = p->fp.layer_nb;
label_text.format("Layer %i", current_layer);
FXLabel *lab = new FXLabel(matrix, label_text);
lab->setFont(m_bold_font);
new FXLabel(matrix, "");
}
get_param_name(&p->fp, pname.str());
FXString fxpname((const FXchar *) pname.cstr());
FXCheckButton* bt = new FXCheckButton(matrix, fxpname, this, ID_PARAM_SELECT);
FXTextField* tf = new fx_numeric_field(matrix, 10, this, ID_PARAM_VALUE, FRAME_SUNKEN|FRAME_THICK|TEXTFIELD_REAL|LAYOUT_FILL_ROW);
tf->setUserData(p);
bt->setUserData(p);
p->text_field = tf;
p->selected = false;
p->is_dirty = true;
}
m_canvas = new plot_canvas(mf, NULL, 0, LAYOUT_FILL_X|LAYOUT_FILL_Y);
m_fit->config_plot(m_canvas);
}
PGM_P ot_model_fx_name(uint8_t fx, uint8_t param) {
return get_param_name(get_fx_param_names(fx), GET_OT_FX_PARAM(param));
}
