mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-19 05:43:34 +08:00
Checkpoint: more work in progress on mixer load/save
This commit is contained in:
+53
-45
@@ -53,19 +53,19 @@ static int
|
||||
scale_check(struct MixScaler *scale)
|
||||
{
|
||||
if (scale->offset > 1.0f)
|
||||
return -1;
|
||||
return 1;
|
||||
|
||||
if (scale->offset > 1.0f)
|
||||
return -1;
|
||||
if (scale->offset < -1.0f)
|
||||
return 2;
|
||||
|
||||
if (scale->lower_limit > scale->upper_limit)
|
||||
return -1;
|
||||
return 3;
|
||||
|
||||
if (scale->lower_limit < -1.0f)
|
||||
return -1;
|
||||
return 4;
|
||||
|
||||
if (scale->upper_limit > 1.0f)
|
||||
return -1;
|
||||
return 5;
|
||||
|
||||
return 0;
|
||||
}
|
||||
@@ -73,21 +73,25 @@ scale_check(struct MixScaler *scale)
|
||||
int
|
||||
mixer_check(struct MixMixer *mixer, unsigned control_count)
|
||||
{
|
||||
int ret;
|
||||
|
||||
if (mixer->control_count < 1)
|
||||
return -1;
|
||||
|
||||
if (mixer->control_count > control_count)
|
||||
return -1;
|
||||
return -2;
|
||||
|
||||
if (!scale_check(&mixer->output_scaler))
|
||||
return -1;
|
||||
ret = scale_check(&mixer->output_scaler);
|
||||
if (ret != 0)
|
||||
return ret;
|
||||
|
||||
for (unsigned i = 0; i < mixer->control_count; i++) {
|
||||
if (mixer->control_scaler[i].control >= control_count)
|
||||
return -1;
|
||||
return -3;
|
||||
|
||||
if (!scale_check(&mixer->control_scaler[i]))
|
||||
return -1;
|
||||
ret = scale_check(&mixer->control_scaler[i]);
|
||||
if (ret != 0)
|
||||
return (10 * i + ret);
|
||||
}
|
||||
|
||||
return 0;
|
||||
@@ -148,6 +152,7 @@ mixer_getline(int fd, char *line, unsigned maxlen)
|
||||
*line++ = c;
|
||||
}
|
||||
/* line too long */
|
||||
puts("line too long");
|
||||
return -1;
|
||||
}
|
||||
|
||||
@@ -184,32 +189,33 @@ mixer_load(int fd, struct MixMixer **mp)
|
||||
if (sscanf(buf, "M: %u", &scalers) != 1)
|
||||
goto out;
|
||||
|
||||
/* must have at least one scaler */
|
||||
if (scalers < 1)
|
||||
goto out;
|
||||
/* if there are scalers, load them */
|
||||
if (scalers > 0) {
|
||||
|
||||
/* allocate mixer */
|
||||
scalers--;
|
||||
mixer = (struct MixMixer *)malloc(MIXER_SIZE(scalers));
|
||||
/* allocate mixer */
|
||||
scalers--;
|
||||
mixer = (struct MixMixer *)malloc(MIXER_SIZE(scalers));
|
||||
|
||||
if (mixer == NULL)
|
||||
goto out;
|
||||
|
||||
mixer->control_count = scalers;
|
||||
|
||||
ret = mixer_getline(fd, buf, sizeof(buf));
|
||||
|
||||
if (ret < 1)
|
||||
goto out;
|
||||
|
||||
if (mixer_load_scaler(buf, &mixer->output_scaler))
|
||||
goto out;
|
||||
|
||||
for (unsigned i = 0; i < scalers; i++) {
|
||||
if (mixer_getline(fd, buf, sizeof(buf)))
|
||||
if (mixer == NULL)
|
||||
goto out;
|
||||
if (mixer_load_scaler(buf, &mixer->control_scaler[i]))
|
||||
|
||||
mixer->control_count = scalers;
|
||||
|
||||
ret = mixer_getline(fd, buf, sizeof(buf));
|
||||
|
||||
if (ret < 1)
|
||||
goto out;
|
||||
|
||||
if (mixer_load_scaler(buf, &mixer->output_scaler))
|
||||
goto out;
|
||||
|
||||
for (unsigned i = 0; i < scalers; i++) {
|
||||
ret = mixer_getline(fd, buf, sizeof(buf));
|
||||
if (ret < 1)
|
||||
goto out;
|
||||
if (mixer_load_scaler(buf, &mixer->control_scaler[i]))
|
||||
goto out;
|
||||
}
|
||||
}
|
||||
|
||||
result = 1;
|
||||
@@ -237,23 +243,25 @@ mixer_save(int fd, struct MixMixer *mixer)
|
||||
int len, ret;
|
||||
|
||||
/* write the mixer header */
|
||||
len = sprintf(buf, "M: %u\n", mixer->control_count);
|
||||
len = sprintf(buf, "M: %u\n", (mixer != NULL) ? mixer->control_count : 0);
|
||||
ret = write(fd, buf, len);
|
||||
if (ret != len)
|
||||
return -1;
|
||||
|
||||
/* write the output scaler */
|
||||
len = mixer_save_scaler(buf, &mixer->output_scaler);
|
||||
write(fd, buf, len);
|
||||
if (ret != len)
|
||||
return -1;
|
||||
|
||||
/* write the control scalers */
|
||||
for (unsigned j = 0; j < mixer->control_count; j++) {
|
||||
len = mixer_save_scaler(buf, &mixer->control_scaler[j]);
|
||||
if (mixer != NULL) {
|
||||
/* write the output scaler */
|
||||
len = mixer_save_scaler(buf, &mixer->output_scaler);
|
||||
write(fd, buf, len);
|
||||
if (ret != len)
|
||||
return -1;
|
||||
|
||||
/* write the control scalers */
|
||||
for (unsigned j = 0; j < mixer->control_count; j++) {
|
||||
len = mixer_save_scaler(buf, &mixer->control_scaler[j]);
|
||||
write(fd, buf, len);
|
||||
if (ret != len)
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
@@ -161,8 +161,8 @@ __EXPORT float mixer_mix(struct MixMixer *mixer, float *controls);
|
||||
/**
|
||||
* Check a mixer configuration for sanity.
|
||||
*
|
||||
* @param mixer The mixer configuration to be checked.
|
||||
* @param control_count The number of controls in the system.
|
||||
* @param mixer The mixer configuration to be checked.
|
||||
* @param control_count The number of controls in the system.
|
||||
* @return Zero if the mixer configuration is sane,
|
||||
* nonzero otherwise.
|
||||
*/
|
||||
@@ -177,17 +177,18 @@ __EXPORT int mixer_check(struct MixMixer *mixer, unsigned control_count);
|
||||
* scaler in the file is always the output scaler, followed by the input
|
||||
* scalers.
|
||||
*
|
||||
* M: <control count + 1>
|
||||
* M: <scaler count>
|
||||
* S: <control> <negative_scale> <positive_scale> <offset> <lower_limit> <upper_limit>
|
||||
* S: ...
|
||||
*
|
||||
* The <control> value for the output scaler is ignored.
|
||||
* The <control> value for the output scaler is ignored by the mixer.
|
||||
*
|
||||
* Multiple mixer definitions may be stored in a single file; it is assumed that
|
||||
* the reader will know how many to expect and read accordingly. Mixers may be
|
||||
* 'skipped' in a file by setting indicating that the mixer has only one scaler
|
||||
* (the output scaler). This results in a mixer with zero controls, which will
|
||||
* always generate output corresponding to the output scaler offset.
|
||||
* the reader will know how many to expect and read accordingly.
|
||||
*
|
||||
* A mixer entry with a scaler count of zero indicates a disabled mixer. This
|
||||
* will return NULL for the mixer when processed by this function, and will be
|
||||
* generated by passing NULL as the mixer to mixer_save.
|
||||
*
|
||||
* @param fd The file to read the definitions from.
|
||||
* @param mixer Mixer is returned here.
|
||||
|
||||
Reference in New Issue
Block a user