From 613a9d43325ae87f5b80c89e7ab8f81fe4205192 Mon Sep 17 00:00:00 2001 From: FabianG Date: Tue, 26 Sep 2017 22:37:44 +0200 Subject: [PATCH 1/2] WIP Setting powerdistribution directly from setpoint message --- src/modules/interface/power_distribution.h | 1 + src/modules/src/power_distribution_stock.c | 25 ++++++++++++++++++++++ src/modules/src/stabilizer.c | 10 ++++++--- 3 files changed, 33 insertions(+), 3 deletions(-) diff --git a/src/modules/interface/power_distribution.h b/src/modules/interface/power_distribution.h index 8167514d35..59583312d8 100644 --- a/src/modules/interface/power_distribution.h +++ b/src/modules/interface/power_distribution.h @@ -31,6 +31,7 @@ void powerDistributionInit(void); bool powerDistributionTest(void); void powerDistribution(const control_t *control); +void powerDistributionDirect(const setpoint_t *setpoint); void powerStop(); diff --git a/src/modules/src/power_distribution_stock.c b/src/modules/src/power_distribution_stock.c index 0e66c0b85e..f9ded0afa1 100644 --- a/src/modules/src/power_distribution_stock.c +++ b/src/modules/src/power_distribution_stock.c @@ -107,6 +107,31 @@ void powerDistribution(const control_t *control) } } + +void powerDistributionDirect(const setpoint_t *setpoint) +{ + // expects floats as thrust measure as percentage in the range 0 to 100.0 + motorPower.m1 = limitThrust((int) (setpoint->motorControl.topLeft/100.0f * 65536)); + motorPower.m2 = limitThrust((int) (setpoint->motorControl.topRight/100.0f * 65536)); + motorPower.m3 = limitThrust((int) (setpoint->motorControl.bottomLeft/100.0f * 65536)); + motorPower.m4 = limitThrust((int)(setpoint->motorControl.bottomRight/100.0f * 65536)); + // TODO unsure what this flag does + if (motorSetEnable) + { + motorsSetRatio(MOTOR_M1, motorPowerSet.m1); + motorsSetRatio(MOTOR_M2, motorPowerSet.m2); + motorsSetRatio(MOTOR_M3, motorPowerSet.m3); + motorsSetRatio(MOTOR_M4, motorPowerSet.m4); + } + else + { + motorsSetRatio(MOTOR_M1, motorPower.m1); + motorsSetRatio(MOTOR_M2, motorPower.m2); + motorsSetRatio(MOTOR_M3, motorPower.m3); + motorsSetRatio(MOTOR_M4, motorPower.m4); + } +} + PARAM_GROUP_START(motorPowerSet) PARAM_ADD(PARAM_UINT8, enable, &motorSetEnable) PARAM_ADD(PARAM_UINT16, m1, &motorPowerSet.m1) diff --git a/src/modules/src/stabilizer.c b/src/modules/src/stabilizer.c index 48f436f22d..16c82dc953 100644 --- a/src/modules/src/stabilizer.c +++ b/src/modules/src/stabilizer.c @@ -129,16 +129,20 @@ static void stabilizerTask(void* param) commanderGetSetpoint(&setpoint, &state); - sitAwUpdateSetpoint(&setpoint, &sensorData, &state); + // Update of setpoint according to current state seems unnecessary + //sitAwUpdateSetpoint(&setpoint, &sensorData, &state); - stateController(&control, &setpoint, &sensorData, &state, tick); + // Seems also unnecessary but we could include some ramping or of course an actual controller + //stateController(&control, &setpoint, &sensorData, &state, tick); checkEmergencyStopTimeout(); if (emergencyStop) { powerStop(); } else { - powerDistribution(&control); + //FIXME I would maybe overload the power distribution directly on the setpoint_t struct + //powerDistribution(&control); + powerDistributionDirect(&setpoint); } tick++; From 017a1fa0ce5e1e19b7a98e05eab0f7afe41b42fe Mon Sep 17 00:00:00 2001 From: Colin Jones Date: Wed, 27 Sep 2017 08:22:47 +0200 Subject: [PATCH 2/2] Removed the emergency stop Removed emergy stop if a message isn't seen for 0.5 s. We should probably put this back in, once the ground station is functioning! --- src/modules/src/stabilizer.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modules/src/stabilizer.c b/src/modules/src/stabilizer.c index 16c82dc953..7590b131b3 100644 --- a/src/modules/src/stabilizer.c +++ b/src/modules/src/stabilizer.c @@ -137,13 +137,13 @@ static void stabilizerTask(void* param) checkEmergencyStopTimeout(); - if (emergencyStop) { - powerStop(); - } else { + // if (emergencyStop) { + // powerStop(); + // } else { //FIXME I would maybe overload the power distribution directly on the setpoint_t struct //powerDistribution(&control); powerDistributionDirect(&setpoint); - } + // } tick++; }