/**
* Set the gyro range and store it locally for scaling
*/
void PIOS_MPU6000_SetGyroRange(enum pios_mpu60x0_range gyro_range)
{
PIOS_MPU6000_SetReg(PIOS_MPU60X0_GYRO_CFG_REG, gyro_range);
switch(gyro_range) {
case PIOS_MPU60X0_SCALE_250_DEG:
PIOS_SENSORS_SetMaxGyro(250);
break;
case PIOS_MPU60X0_SCALE_500_DEG:
PIOS_SENSORS_SetMaxGyro(500);
break;
case PIOS_MPU60X0_SCALE_1000_DEG:
PIOS_SENSORS_SetMaxGyro(1000);
break;
case PIOS_MPU60X0_SCALE_2000_DEG:
PIOS_SENSORS_SetMaxGyro(2000);
break;
}
pios_mpu6000_dev->gyro_range = gyro_range;
}
/**
* @brief Set gyroscope range
* @returns 0 if successful
* @param range[in] gyroscope range
*/
int32_t PIOS_MPU9250_SetGyroRange(enum pios_mpu60x0_range range)
{
if (PIOS_MPU9250_WriteReg(PIOS_MPU60X0_GYRO_CFG_REG, range) != 0)
return -1;
switch (range) {
case PIOS_MPU60X0_SCALE_250_DEG:
PIOS_SENSORS_SetMaxGyro(250);
break;
case PIOS_MPU60X0_SCALE_500_DEG:
PIOS_SENSORS_SetMaxGyro(500);
break;
case PIOS_MPU60X0_SCALE_1000_DEG:
PIOS_SENSORS_SetMaxGyro(1000);
break;
case PIOS_MPU60X0_SCALE_2000_DEG:
PIOS_SENSORS_SetMaxGyro(2000);
break;
}
dev->gyro_range = range;
return 0;
}
/**
* @brief Sets the maximum range of the L3GD20
* @returns 0 for success, -1 for invalid device, -2 if unable to set register
*/
int32_t PIOS_L3GD20_SetRange(enum pios_l3gd20_range range)
{
if (PIOS_L3GD20_Validate(pios_l3gd20_dev) != 0)
return -1;
pios_l3gd20_dev->range = range;
if (PIOS_L3GD20_SetReg(PIOS_L3GD20_CTRL_REG4, pios_l3gd20_dev->range) != 0)
return -2;
switch(range) {
case PIOS_L3GD20_SCALE_250_DEG:
PIOS_SENSORS_SetMaxGyro(250);
break;
case PIOS_L3GD20_SCALE_500_DEG:
PIOS_SENSORS_SetMaxGyro(500);
break;
case PIOS_L3GD20_SCALE_2000_DEG:
PIOS_SENSORS_SetMaxGyro(2000);
break;
}
return 0;
}
static void SensorsTask(void *parameters)
{
AlarmsClear(SYSTEMALARMS_ALARM_SENSORS);
// HomeLocationData homeLocation;
// HomeLocationGet(&homeLocation);
// homeLocation.Latitude = 0;
// homeLocation.Longitude = 0;
// homeLocation.Altitude = 0;
// homeLocation.Be[0] = 26000;
// homeLocation.Be[1] = 400;
// homeLocation.Be[2] = 40000;
// homeLocation.Set = HOMELOCATION_SET_TRUE;
// HomeLocationSet(&homeLocation);
PIOS_SENSORS_SetMaxGyro(500);
// Main task loop
while (1) {
PIOS_WDG_UpdateFlag(PIOS_WDG_SENSORS);
SystemSettingsData systemSettings;
SystemSettingsGet(&systemSettings);
switch(systemSettings.AirframeType) {
case SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWING:
case SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWINGELEVON:
case SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWINGVTAIL:
sensor_sim_type = MODEL_AIRPLANE;
break;
case SYSTEMSETTINGS_AIRFRAMETYPE_QUADX:
case SYSTEMSETTINGS_AIRFRAMETYPE_QUADP:
case SYSTEMSETTINGS_AIRFRAMETYPE_VTOL:
case SYSTEMSETTINGS_AIRFRAMETYPE_HEXA:
case SYSTEMSETTINGS_AIRFRAMETYPE_OCTO:
sensor_sim_type = MODEL_QUADCOPTER;
break;
case SYSTEMSETTINGS_AIRFRAMETYPE_GROUNDVEHICLECAR:
sensor_sim_type = MODEL_CAR;
break;
default:
sensor_sim_type = MODEL_AGNOSTIC;
}
static int i;
i++;
if (i % 5000 == 0) {
//float dT = PIOS_DELAY_DiffuS(last_time) / 10.0e6;
//fprintf(stderr, "Sensor relative timing: %f\n", dT);
}
sensors_count++;
switch(sensor_sim_type) {
case CONSTANT:
simulateConstant();
break;
case MODEL_AGNOSTIC:
simulateModelAgnostic();
break;
case MODEL_QUADCOPTER:
simulateModelQuadcopter();
break;
case MODEL_AIRPLANE:
simulateModelAirplane();
break;
case MODEL_CAR:
simulateModelCar();
}
vTaskDelay(MS2TICKS(2));
}
}
