diff options
author | Anton Babushkin <anton.babushkin@me.com> | 2013-05-28 19:04:29 +0400 |
---|---|---|
committer | Anton Babushkin <anton.babushkin@me.com> | 2013-05-28 19:04:29 +0400 |
commit | 234b9c8f670d8794a73ea467f26376610347aa62 (patch) | |
tree | 76c59c5acae0928629542ad793112d6ccf6be20e | |
parent | f8900f002ca553fa6eaf04207623472267094f16 (diff) | |
parent | 27ee36b2049167a1272122548fe61aa2993d79c1 (diff) | |
download | px4-firmware-234b9c8f670d8794a73ea467f26376610347aa62.tar.gz px4-firmware-234b9c8f670d8794a73ea467f26376610347aa62.tar.bz2 px4-firmware-234b9c8f670d8794a73ea467f26376610347aa62.zip |
Merge branch 'master' into seatbelt_multirotor
-rw-r--r-- | makefiles/config_px4fmu_default.mk | 1 | ||||
-rw-r--r-- | makefiles/toolchain_gnu-arm-eabi.mk | 2 | ||||
-rw-r--r-- | src/drivers/hmc5883/hmc5883.cpp | 2 | ||||
-rw-r--r-- | src/drivers/mkblctrl/mkblctrl.cpp | 512 | ||||
-rw-r--r-- | src/examples/fixedwing_control/main.c | 129 | ||||
-rw-r--r-- | src/examples/fixedwing_control/params.c | 2 | ||||
-rw-r--r-- | src/modules/gpio_led/gpio_led.c | 191 | ||||
-rw-r--r-- | src/modules/gpio_led/module.mk | 39 |
8 files changed, 620 insertions, 258 deletions
diff --git a/makefiles/config_px4fmu_default.mk b/makefiles/config_px4fmu_default.mk index b1737ee6b..7a73d1a32 100644 --- a/makefiles/config_px4fmu_default.mk +++ b/makefiles/config_px4fmu_default.mk @@ -56,6 +56,7 @@ MODULES += systemcmds/tests MODULES += modules/commander MODULES += modules/mavlink MODULES += modules/mavlink_onboard +MODULES += modules/gpio_led # # Estimation modules (EKF / other filters) diff --git a/makefiles/toolchain_gnu-arm-eabi.mk b/makefiles/toolchain_gnu-arm-eabi.mk index eeba0ff2c..c75a08bd1 100644 --- a/makefiles/toolchain_gnu-arm-eabi.mk +++ b/makefiles/toolchain_gnu-arm-eabi.mk @@ -219,7 +219,7 @@ endef define PRELINK @$(ECHO) "PRELINK: $1" @$(MKDIR) -p $(dir $1) - $(Q) $(LD) -Ur -o $1 $2 && $(OBJCOPY) --localize-hidden $1 + $(Q) $(LD) -Ur -Map $1.map -o $1 $2 && $(OBJCOPY) --localize-hidden $1 endef # Update the archive $1 with the files in $2 diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 78eda327c..59e90d86c 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -329,7 +329,7 @@ HMC5883::HMC5883(int bus) : _calibrated(false) { // enable debug() calls - _debug_enabled = true; + _debug_enabled = false; // default scaling _scale.x_offset = 0; diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index 3a735e26f..c67276f8a 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -76,7 +76,6 @@ #include <uORB/topics/actuator_outputs.h> #include <systemlib/err.h> -#include <systemlib/ppm_decode.h> #define I2C_BUS_SPEED 400000 #define UPDATE_RATE 400 @@ -114,6 +113,7 @@ public: virtual int ioctl(file *filp, int cmd, unsigned long arg); virtual int init(unsigned motors); + virtual ssize_t write(file *filp, const char *buffer, size_t len); int set_mode(Mode mode); int set_pwm_rate(unsigned rate); @@ -177,9 +177,10 @@ private: int gpio_ioctl(file *filp, int cmd, unsigned long arg); int mk_servo_arm(bool status); - int mk_servo_set(unsigned int chan, float val); - int mk_servo_set_test(unsigned int chan, float val); + int mk_servo_set(unsigned int chan, short val); + int mk_servo_set_value(unsigned int chan, short val); int mk_servo_test(unsigned int chan); + short scaling(float val, float inMin, float inMax, float outMin, float outMax); }; @@ -207,20 +208,20 @@ const int blctrlAddr_octo_x[] = { 1, 4, 0, 1, -4, 1, 1, -4 }; // Addresstranslat const int blctrlAddr_px4[] = { 0, 0, 0, 0, 0, 0, 0, 0}; -int addrTranslator[] = {0,0,0,0,0,0,0,0}; +int addrTranslator[] = {0, 0, 0, 0, 0, 0, 0, 0}; -struct MotorData_t -{ +struct MotorData_t { unsigned int Version; // the version of the BL (0 = old) - unsigned int SetPoint; // written by attitude controller - unsigned int SetPointLowerBits; // for higher Resolution of new BLs - unsigned int State; // 7 bit for I2C error counter, highest bit indicates if motor is present - unsigned int ReadMode; // select data to read - // the following bytes must be exactly in that order! - unsigned int Current; // in 0.1 A steps, read back from BL - unsigned int MaxPWM; // read back from BL is less than 255 if BL is in current limit - unsigned int Temperature; // old BL-Ctrl will return a 255 here, the new version the temp. in - unsigned int RoundCount; + unsigned int SetPoint; // written by attitude controller + unsigned int SetPointLowerBits; // for higher Resolution of new BLs + unsigned int State; // 7 bit for I2C error counter, highest bit indicates if motor is present + unsigned int ReadMode; // select data to read + unsigned short RawPwmValue; // length of PWM pulse + // the following bytes must be exactly in that order! + unsigned int Current; // in 0.1 A steps, read back from BL + unsigned int MaxPWM; // read back from BL is less than 255 if BL is in current limit + unsigned int Temperature; // old BL-Ctrl will return a 255 here, the new version the temp. in + unsigned int RoundCount; }; MotorData_t Motor[MAX_MOTORS]; @@ -314,7 +315,7 @@ MK::init(unsigned motors) /* start the IO interface task */ _task = task_spawn("mkblctrl", SCHED_DEFAULT, - SCHED_PRIORITY_MAX -20, + SCHED_PRIORITY_MAX - 20, 2048, (main_t)&MK::task_main_trampoline, nullptr); @@ -346,27 +347,11 @@ MK::set_mode(Mode mode) */ switch (mode) { case MODE_2PWM: - if(_num_outputs == 4) { - //debug("MODE_QUAD"); - } else if(_num_outputs == 6) { - //debug("MODE_HEXA"); - } else if(_num_outputs == 8) { - //debug("MODE_OCTO"); - } - //up_pwm_servo_init(0x3); up_pwm_servo_deinit(); _update_rate = UPDATE_RATE; /* default output rate */ break; case MODE_4PWM: - if(_num_outputs == 4) { - //debug("MODE_QUADRO"); - } else if(_num_outputs == 6) { - //debug("MODE_HEXA"); - } else if(_num_outputs == 8) { - //debug("MODE_OCTO"); - } - //up_pwm_servo_init(0xf); up_pwm_servo_deinit(); _update_rate = UPDATE_RATE; /* default output rate */ break; @@ -412,45 +397,55 @@ MK::set_frametype(int frametype) int MK::set_motor_count(unsigned count) { - if(count > 0) { + if (count > 0) { _num_outputs = count; - if(_px4mode == MAPPING_MK) { - if(_frametype == FRAME_PLUS) { + if (_px4mode == MAPPING_MK) { + if (_frametype == FRAME_PLUS) { fprintf(stderr, "[mkblctrl] addresstanslator for Mikrokopter addressing used. Frametype: +\n"); - } else if(_frametype == FRAME_X) { + + } else if (_frametype == FRAME_X) { fprintf(stderr, "[mkblctrl] addresstanslator for Mikrokopter addressing used. Frametype: X\n"); } - if(_num_outputs == 4) { - if(_frametype == FRAME_PLUS) { + + if (_num_outputs == 4) { + if (_frametype == FRAME_PLUS) { memcpy(&addrTranslator, &blctrlAddr_quad_plus, sizeof(blctrlAddr_quad_plus)); - } else if(_frametype == FRAME_X) { + + } else if (_frametype == FRAME_X) { memcpy(&addrTranslator, &blctrlAddr_quad_x, sizeof(blctrlAddr_quad_x)); } - } else if(_num_outputs == 6) { - if(_frametype == FRAME_PLUS) { + + } else if (_num_outputs == 6) { + if (_frametype == FRAME_PLUS) { memcpy(&addrTranslator, &blctrlAddr_hexa_plus, sizeof(blctrlAddr_hexa_plus)); - } else if(_frametype == FRAME_X) { + + } else if (_frametype == FRAME_X) { memcpy(&addrTranslator, &blctrlAddr_hexa_x, sizeof(blctrlAddr_hexa_x)); } - } else if(_num_outputs == 8) { - if(_frametype == FRAME_PLUS) { + + } else if (_num_outputs == 8) { + if (_frametype == FRAME_PLUS) { memcpy(&addrTranslator, &blctrlAddr_octo_plus, sizeof(blctrlAddr_octo_plus)); - } else if(_frametype == FRAME_X) { + + } else if (_frametype == FRAME_X) { memcpy(&addrTranslator, &blctrlAddr_octo_x, sizeof(blctrlAddr_octo_x)); } } + } else { fprintf(stderr, "[mkblctrl] PX4 native addressing used.\n"); memcpy(&addrTranslator, &blctrlAddr_px4, sizeof(blctrlAddr_px4)); } - if(_num_outputs == 4) { + if (_num_outputs == 4) { fprintf(stderr, "[mkblctrl] Quadrocopter Mode (4)\n"); - } else if(_num_outputs == 6) { + + } else if (_num_outputs == 6) { fprintf(stderr, "[mkblctrl] Hexacopter Mode (6)\n"); - } else if(_num_outputs == 8) { + + } else if (_num_outputs == 8) { fprintf(stderr, "[mkblctrl] Octocopter Mode (8)\n"); } @@ -469,16 +464,35 @@ MK::set_motor_test(bool motortest) return OK; } +short +MK::scaling(float val, float inMin, float inMax, float outMin, float outMax) +{ + short retVal = 0; + + retVal = (val - inMin) / (inMax - inMin) * (outMax - outMin) + outMin; + + if (retVal < outMin) { + retVal = outMin; + + } else if (retVal > outMax) { + retVal = outMax; + } + + return retVal; +} void MK::task_main() { + long update_rate_in_us = 0; + float tmpVal = 0; + /* * Subscribe to the appropriate PWM output topic based on whether we are the * primary PWM output or not. */ - _t_actuators = orb_subscribe(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS : - ORB_ID(actuator_controls_1)); + _t_actuators = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); + /* force a reset of the update rate */ _current_update_rate = 0; @@ -492,6 +506,8 @@ MK::task_main() _t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), &outputs); + + /* advertise the effective control inputs */ actuator_controls_effective_s controls_effective; memset(&controls_effective, 0, sizeof(controls_effective)); @@ -499,21 +515,14 @@ MK::task_main() _t_actuators_effective = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1), &controls_effective); + pollfd fds[2]; fds[0].fd = _t_actuators; fds[0].events = POLLIN; fds[1].fd = _t_armed; fds[1].events = POLLIN; - // rc input, published to ORB - struct rc_input_values rc_in; - orb_advert_t to_input_rc = 0; - - memset(&rc_in, 0, sizeof(rc_in)); - rc_in.input_source = RC_INPUT_SOURCE_PX4FMU_PPM; - log("starting"); - long update_rate_in_us = 0; /* loop until killed */ while (!_task_should_exit) { @@ -528,6 +537,7 @@ MK::task_main() update_rate_in_ms = 2; _update_rate = 500; } + /* reject slower than 50 Hz updates */ if (update_rate_in_ms > 20) { update_rate_in_ms = 20; @@ -539,8 +549,8 @@ MK::task_main() _current_update_rate = _update_rate; } - /* sleep waiting for data, but no more than a second */ - int ret = ::poll(&fds[0], 2, 1000); + /* sleep waiting for data max 100ms */ + int ret = ::poll(&fds[0], 2, 100); /* this would be bad... */ if (ret < 0) { @@ -553,7 +563,7 @@ MK::task_main() if (fds[0].revents & POLLIN) { /* get controls - must always do this to avoid spinning */ - orb_copy(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS : ORB_ID(actuator_controls_1), _t_actuators, &_controls); + orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _t_actuators, &_controls); /* can we mix? */ if (_mixers != nullptr) { @@ -565,53 +575,52 @@ MK::task_main() // XXX output actual limited values memcpy(&controls_effective, &_controls, sizeof(controls_effective)); - orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1), _t_actuators_effective, &controls_effective); - /* iterate actuators */ for (unsigned int i = 0; i < _num_outputs; i++) { /* last resort: catch NaN, INF and out-of-band errors */ if (i < outputs.noutputs && - isfinite(outputs.output[i]) && - outputs.output[i] >= -1.0f && - outputs.output[i] <= 1.0f) { + isfinite(outputs.output[i]) && + outputs.output[i] >= -1.0f && + outputs.output[i] <= 1.0f) { /* scale for PWM output 900 - 2100us */ - //outputs.output[i] = 1500 + (600 * outputs.output[i]); - //outputs.output[i] = 127 + (127 * outputs.output[i]); + /* nothing to do here */ } else { /* * Value is NaN, INF or out of band - set to the minimum value. * This will be clearly visible on the servo status and will limit the risk of accidentally * spinning motors. It would be deadly in flight. */ - if(outputs.output[i] < -1.0f) { + if (outputs.output[i] < -1.0f) { outputs.output[i] = -1.0f; - } else if(outputs.output[i] > 1.0f) { + + } else if (outputs.output[i] > 1.0f) { outputs.output[i] = 1.0f; + } else { outputs.output[i] = -1.0f; } } /* don't go under BLCTRL_MIN_VALUE */ - if(outputs.output[i] < BLCTRL_MIN_VALUE) { + if (outputs.output[i] < BLCTRL_MIN_VALUE) { outputs.output[i] = BLCTRL_MIN_VALUE; } - //_motortest = true; + /* output to BLCtrl's */ - if(_motortest == true) { + if (_motortest == true) { mk_servo_test(i); + } else { - //mk_servo_set(i, outputs.output[i]); - mk_servo_set_test(i, outputs.output[i]); // 8Bit + mk_servo_set_value(i, scaling(outputs.output[i], -1.0f, 1.0f, 0, 1024)); // scale the output to 0 - 1024 and sent to output routine } - } - /* and publish for anyone that cares to see */ - orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), _t_outputs, &outputs); } + + + } /* how about an arming update? */ @@ -622,29 +631,9 @@ MK::task_main() orb_copy(ORB_ID(actuator_armed), _t_armed, &aa); /* update PWM servo armed status if armed and not locked down */ - ////up_pwm_servo_arm(aa.armed && !aa.lockdown); mk_servo_arm(aa.armed && !aa.lockdown); } - // see if we have new PPM input data - if (ppm_last_valid_decode != rc_in.timestamp) { - // we have a new PPM frame. Publish it. - rc_in.channel_count = ppm_decoded_channels; - if (rc_in.channel_count > RC_INPUT_MAX_CHANNELS) { - rc_in.channel_count = RC_INPUT_MAX_CHANNELS; - } - for (uint8_t i=0; i<rc_in.channel_count; i++) { - rc_in.values[i] = ppm_buffer[i]; - } - rc_in.timestamp = ppm_last_valid_decode; - - /* lazily advertise on first publication */ - if (to_input_rc == 0) { - to_input_rc = orb_advertise(ORB_ID(input_rc), &rc_in); - } else { - orb_publish(ORB_ID(input_rc), to_input_rc, &rc_in); - } - } } @@ -666,7 +655,7 @@ MK::task_main() } -int +int MK::mk_servo_arm(bool status) { _armed = status; @@ -680,12 +669,13 @@ MK::mk_check_for_blctrl(unsigned int count, bool showOutput) _retries = 50; uint8_t foundMotorCount = 0; - for(unsigned i=0; i<MAX_MOTORS; i++) { + for (unsigned i = 0; i < MAX_MOTORS; i++) { Motor[i].Version = 0; Motor[i].SetPoint = 0; Motor[i].SetPointLowerBits = 0; Motor[i].State = 0; Motor[i].ReadMode = 0; + Motor[i].RawPwmValue = 0; Motor[i].Current = 0; Motor[i].MaxPWM = 0; Motor[i].Temperature = 0; @@ -695,34 +685,37 @@ MK::mk_check_for_blctrl(unsigned int count, bool showOutput) uint8_t msg = 0; uint8_t result[3]; - for(unsigned i=0; i< count; i++) { + for (unsigned i = 0; i < count; i++) { result[0] = 0; result[1] = 0; result[2] = 0; - - set_address( BLCTRL_BASE_ADDR + i ); - + + set_address(BLCTRL_BASE_ADDR + i); + if (OK == transfer(&msg, 1, &result[0], 3)) { Motor[i].Current = result[0]; Motor[i].MaxPWM = result[1]; Motor[i].Temperature = result[2]; Motor[i].State |= MOTOR_STATE_PRESENT_MASK; // set present bit; foundMotorCount++; - if(Motor[i].MaxPWM == 250) { + + if (Motor[i].MaxPWM == 250) { Motor[i].Version = BLCTRL_NEW; + } else { Motor[i].Version = BLCTRL_OLD; } } } - if(showOutput) { - fprintf(stderr, "[mkblctrl] MotorsFound: %i\n",foundMotorCount); - for(unsigned i=0; i< foundMotorCount; i++) { - fprintf(stderr, "[mkblctrl] blctrl[%i] : found=%i\tversion=%i\tcurrent=%i\tmaxpwm=%i\ttemperature=%i\n", i,Motor[i].State, Motor[i].Version, Motor[i].Current, Motor[i].MaxPWM, Motor[i].Temperature); + if (showOutput) { + fprintf(stderr, "[mkblctrl] MotorsFound: %i\n", foundMotorCount); + + for (unsigned i = 0; i < foundMotorCount; i++) { + fprintf(stderr, "[mkblctrl] blctrl[%i] : found=%i\tversion=%i\tcurrent=%i\tmaxpwm=%i\ttemperature=%i\n", i, Motor[i].State, Motor[i].Version, Motor[i].Current, Motor[i].MaxPWM, Motor[i].Temperature); } - if(foundMotorCount != 4 && foundMotorCount != 6 && foundMotorCount != 8) { + if (foundMotorCount != 4 && foundMotorCount != 6 && foundMotorCount != 8) { _task_should_exit = true; } } @@ -734,122 +727,136 @@ MK::mk_check_for_blctrl(unsigned int count, bool showOutput) int -MK::mk_servo_set(unsigned int chan, float val) +MK::mk_servo_set(unsigned int chan, short val) { - float tmpVal = 0; + short tmpVal = 0; _retries = 0; - uint8_t result[3] = { 0,0,0 }; - uint8_t msg[2] = { 0,0 }; - uint8_t rod=0; + uint8_t result[3] = { 0, 0, 0 }; + uint8_t msg[2] = { 0, 0 }; + uint8_t rod = 0; uint8_t bytesToSendBL2 = 2; + tmpVal = val; - tmpVal = (1023 + (1023 * val)); - if(tmpVal > 2047) { - tmpVal = 2047; + if (tmpVal > 1024) { + tmpVal = 1024; + + } else if (tmpVal < 0) { + tmpVal = 0; } + Motor[chan].SetPoint = (uint8_t)(tmpVal / 4); + //Motor[chan].SetPointLowerBits = (uint8_t) tmpVal % 4; - Motor[chan].SetPoint = (uint8_t) tmpVal / 3; // divide 8 - Motor[chan].SetPointLowerBits = (uint8_t) tmpVal % 8; // rest of divide 8 - //rod = (uint8_t) tmpVal % 8; - //Motor[chan].SetPointLowerBits = rod<<1; // rest of divide 8 Motor[chan].SetPointLowerBits = 0; - if(_armed == false) { + if (_armed == false) { Motor[chan].SetPoint = 0; Motor[chan].SetPointLowerBits = 0; } //if(Motor[chan].State & MOTOR_STATE_PRESENT_MASK) { - set_address(BLCTRL_BASE_ADDR + (chan + addrTranslator[chan])); - - if(Motor[chan].Version == BLCTRL_OLD) { - /* - * Old BL-Ctrl 8Bit served. Version < 2.0 - */ - msg[0] = Motor[chan].SetPoint; - if(Motor[chan].RoundCount >= 16) { - // on each 16th cyle we read out the status messages from the blctrl - if (OK == transfer(&msg[0], 1, &result[0], 2)) { - Motor[chan].Current = result[0]; - Motor[chan].MaxPWM = result[1]; - Motor[chan].Temperature = 255;; - } else { - if((Motor[chan].State & MOTOR_STATE_ERROR_MASK) < MOTOR_STATE_ERROR_MASK) Motor[chan].State++; // error - } - Motor[chan].RoundCount = 0; + set_address(BLCTRL_BASE_ADDR + (chan + addrTranslator[chan])); + + if (Motor[chan].Version == BLCTRL_OLD) { + /* + * Old BL-Ctrl 8Bit served. Version < 2.0 + */ + msg[0] = Motor[chan].SetPoint; + + if (Motor[chan].RoundCount >= 16) { + // on each 16th cyle we read out the status messages from the blctrl + if (OK == transfer(&msg[0], 1, &result[0], 2)) { + Motor[chan].Current = result[0]; + Motor[chan].MaxPWM = result[1]; + Motor[chan].Temperature = 255;; + } else { - if (OK != transfer(&msg[0], 1, nullptr, 0)) { - if((Motor[chan].State & MOTOR_STATE_ERROR_MASK) < MOTOR_STATE_ERROR_MASK) Motor[chan].State++; // error - } + if ((Motor[chan].State & MOTOR_STATE_ERROR_MASK) < MOTOR_STATE_ERROR_MASK) Motor[chan].State++; // error } + Motor[chan].RoundCount = 0; + } else { - /* - * New BL-Ctrl 11Bit served. Version >= 2.0 - */ - msg[0] = Motor[chan].SetPoint; - msg[1] = Motor[chan].SetPointLowerBits; - - if(Motor[chan].SetPointLowerBits == 0) { - bytesToSendBL2 = 1; // if setpoint lower bits are zero, we send only the higher bits - this saves time + if (OK != transfer(&msg[0], 1, nullptr, 0)) { + if ((Motor[chan].State & MOTOR_STATE_ERROR_MASK) < MOTOR_STATE_ERROR_MASK) Motor[chan].State++; // error } + } + + } else { + /* + * New BL-Ctrl 11Bit served. Version >= 2.0 + */ + msg[0] = Motor[chan].SetPoint; + msg[1] = Motor[chan].SetPointLowerBits; + + if (Motor[chan].SetPointLowerBits == 0) { + bytesToSendBL2 = 1; // if setpoint lower bits are zero, we send only the higher bits - this saves time + } + + if (Motor[chan].RoundCount >= 16) { + // on each 16th cyle we read out the status messages from the blctrl + if (OK == transfer(&msg[0], bytesToSendBL2, &result[0], 3)) { + Motor[chan].Current = result[0]; + Motor[chan].MaxPWM = result[1]; + Motor[chan].Temperature = result[2]; - if(Motor[chan].RoundCount >= 16) { - // on each 16th cyle we read out the status messages from the blctrl - if (OK == transfer(&msg[0], bytesToSendBL2, &result[0], 3)) { - Motor[chan].Current = result[0]; - Motor[chan].MaxPWM = result[1]; - Motor[chan].Temperature = result[2]; - } else { - if((Motor[chan].State & MOTOR_STATE_ERROR_MASK) < MOTOR_STATE_ERROR_MASK) Motor[chan].State++; // error - } - Motor[chan].RoundCount = 0; } else { - if (OK != transfer(&msg[0], bytesToSendBL2, nullptr, 0)) { - if((Motor[chan].State & MOTOR_STATE_ERROR_MASK) < MOTOR_STATE_ERROR_MASK) Motor[chan].State++; // error - } + if ((Motor[chan].State & MOTOR_STATE_ERROR_MASK) < MOTOR_STATE_ERROR_MASK) Motor[chan].State++; // error } + Motor[chan].RoundCount = 0; + + } else { + if (OK != transfer(&msg[0], bytesToSendBL2, nullptr, 0)) { + if ((Motor[chan].State & MOTOR_STATE_ERROR_MASK) < MOTOR_STATE_ERROR_MASK) Motor[chan].State++; // error + } } - Motor[chan].RoundCount++; + } + + Motor[chan].RoundCount++; //} - if(showDebug == true) { + if (showDebug == true) { debugCounter++; - if(debugCounter == 2000) { + + if (debugCounter == 2000) { debugCounter = 0; - for(int i=0; i<_num_outputs; i++){ - if(Motor[i].State & MOTOR_STATE_PRESENT_MASK) { + + for (int i = 0; i < _num_outputs; i++) { + if (Motor[i].State & MOTOR_STATE_PRESENT_MASK) { fprintf(stderr, "[mkblctrl] #%i:\tVer: %i\tVal: %i\tCurr: %i\tMaxPWM: %i\tTemp: %i\tState: %i\n", i, Motor[i].Version, Motor[i].SetPoint, Motor[i].Current, Motor[i].MaxPWM, Motor[i].Temperature, Motor[i].State); } } + fprintf(stderr, "\n"); } } + return 0; } int -MK::mk_servo_set_test(unsigned int chan, float val) +MK::mk_servo_set_value(unsigned int chan, short val) { _retries = 0; int ret; + short tmpVal = 0; + uint8_t msg[2] = { 0, 0 }; - float tmpVal = 0; + tmpVal = val; - uint8_t msg[2] = { 0,0 }; + if (tmpVal > 1024) { + tmpVal = 1024; - tmpVal = (1023 + (1023 * val)); - if(tmpVal > 2048) { - tmpVal = 2048; + } else if (tmpVal < 0) { + tmpVal = 0; } - Motor[chan].SetPoint = (uint8_t) (tmpVal / 8); + Motor[chan].SetPoint = (uint8_t)(tmpVal / 4); - if(_armed == false) { + if (_armed == false) { Motor[chan].SetPoint = 0; Motor[chan].SetPointLowerBits = 0; } @@ -860,7 +867,6 @@ MK::mk_servo_set_test(unsigned int chan, float val) ret = transfer(&msg[0], 1, nullptr, 0); ret = OK; - return ret; } @@ -868,59 +874,61 @@ MK::mk_servo_set_test(unsigned int chan, float val) int MK::mk_servo_test(unsigned int chan) { - int ret=0; + int ret = 0; float tmpVal = 0; float val = -1; _retries = 0; - uint8_t msg[2] = { 0,0 }; + uint8_t msg[2] = { 0, 0 }; - if(debugCounter >= MOTOR_SPINUP_COUNTER) { + if (debugCounter >= MOTOR_SPINUP_COUNTER) { debugCounter = 0; _motor++; - if(_motor < _num_outputs) { + if (_motor < _num_outputs) { fprintf(stderr, "[mkblctrl] Motortest - #%i:\tspinup\n", _motor); } - if(_motor >= _num_outputs) { + if (_motor >= _num_outputs) { _motor = -1; _motortest = false; } - } + debugCounter++; - if(_motor == chan) { + if (_motor == chan) { val = BLCTRL_MIN_VALUE; + } else { val = -1; } - tmpVal = (1023 + (1023 * val)); - if(tmpVal > 2048) { - tmpVal = 2048; + tmpVal = (511 + (511 * val)); + + if (tmpVal > 1024) { + tmpVal = 1024; } - //Motor[chan].SetPoint = (uint8_t) (tmpVal / 8); - //Motor[chan].SetPointLowerBits = (uint8_t) (tmpVal % 8) & 0x07; - Motor[chan].SetPoint = (uint8_t) tmpVal>>3; - Motor[chan].SetPointLowerBits = (uint8_t) tmpVal & 0x07; + Motor[chan].SetPoint = (uint8_t)(tmpVal / 4); - if(_motor != chan) { + if (_motor != chan) { Motor[chan].SetPoint = 0; Motor[chan].SetPointLowerBits = 0; } - if(Motor[chan].Version == BLCTRL_OLD) { + if (Motor[chan].Version == BLCTRL_OLD) { msg[0] = Motor[chan].SetPoint; + } else { msg[0] = Motor[chan].SetPoint; msg[1] = Motor[chan].SetPointLowerBits; } set_address(BLCTRL_BASE_ADDR + (chan + addrTranslator[chan])); - if(Motor[chan].Version == BLCTRL_OLD) { + + if (Motor[chan].Version == BLCTRL_OLD) { ret = transfer(&msg[0], 1, nullptr, 0); + } else { ret = transfer(&msg[0], 2, nullptr, 0); } @@ -931,9 +939,9 @@ MK::mk_servo_test(unsigned int chan) int MK::control_callback(uintptr_t handle, - uint8_t control_group, - uint8_t control_index, - float &input) + uint8_t control_group, + uint8_t control_index, + float &input) { const actuator_controls_s *controls = (actuator_controls_s *)handle; @@ -947,7 +955,6 @@ MK::ioctl(file *filp, int cmd, unsigned long arg) int ret; // XXX disabled, confusing users - //debug("ioctl 0x%04x 0x%08x", cmd, arg); /* try it as a GPIO ioctl first */ ret = gpio_ioctl(filp, cmd, arg); @@ -978,32 +985,37 @@ int MK::pwm_ioctl(file *filp, int cmd, unsigned long arg) { int ret = OK; - int channel; lock(); switch (cmd) { case PWM_SERVO_ARM: - ////up_pwm_servo_arm(true); mk_servo_arm(true); break; + case PWM_SERVO_SET_ARM_OK: + case PWM_SERVO_CLEAR_ARM_OK: + // these are no-ops, as no safety switch + break; + case PWM_SERVO_DISARM: - ////up_pwm_servo_arm(false); mk_servo_arm(false); break; case PWM_SERVO_SET_UPDATE_RATE: - set_pwm_rate(arg); + ret = OK; + break; + + case PWM_SERVO_SELECT_UPDATE_RATE: + ret = OK; break; case PWM_SERVO_SET(0) ... PWM_SERVO_SET(_max_actuators - 1): + if (arg < 2150) { + Motor[cmd - PWM_SERVO_SET(0)].RawPwmValue = (unsigned short)arg; + mk_servo_set_value(cmd - PWM_SERVO_SET(0), scaling(arg, 1010, 2100, 0, 1024)); - /* fake an update to the selected 'servo' channel */ - if ((arg >= 0) && (arg <= 255)) { - channel = cmd - PWM_SERVO_SET(0); - //mk_servo_set(channel, arg); } else { ret = -EINVAL; } @@ -1012,20 +1024,20 @@ MK::pwm_ioctl(file *filp, int cmd, unsigned long arg) case PWM_SERVO_GET(0) ... PWM_SERVO_GET(_max_actuators - 1): /* copy the current output value from the channel */ - *(servo_position_t *)arg = cmd - PWM_SERVO_GET(0); + *(servo_position_t *)arg = Motor[cmd - PWM_SERVO_SET(0)].RawPwmValue; + break; - case MIXERIOCGETOUTPUTCOUNT: - /* - if (_mode == MODE_4PWM) { - *(unsigned *)arg = 4; - } else { - *(unsigned *)arg = 2; - } - */ + case PWM_SERVO_GET_RATEGROUP(0): + case PWM_SERVO_GET_RATEGROUP(1): + case PWM_SERVO_GET_RATEGROUP(2): + case PWM_SERVO_GET_RATEGROUP(3): + //*(uint32_t *)arg = up_pwm_servo_get_rate_group(cmd - PWM_SERVO_GET_RATEGROUP(0)); + break; + case PWM_SERVO_GET_COUNT: + case MIXERIOCGETOUTPUTCOUNT: *(unsigned *)arg = _num_outputs; - break; case MIXERIOCRESET: @@ -1078,6 +1090,7 @@ MK::pwm_ioctl(file *filp, int cmd, unsigned long arg) ret = -EINVAL; } } + break; } @@ -1091,6 +1104,38 @@ MK::pwm_ioctl(file *filp, int cmd, unsigned long arg) return ret; } +/* + this implements PWM output via a write() method, for compatibility + with px4io + */ +ssize_t +MK::write(file *filp, const char *buffer, size_t len) +{ + unsigned count = len / 2; + // loeschen uint16_t values[4]; + uint16_t values[8]; + + // loeschen if (count > 4) { + // loeschen // we only have 4 PWM outputs on the FMU + // loeschen count = 4; + // loeschen } + if (count > _num_outputs) { + // we only have 8 I2C outputs in the driver + count = _num_outputs; + } + + + // allow for misaligned values + memcpy(values, buffer, count * 2); + + for (uint8_t i = 0; i < count; i++) { + Motor[i].RawPwmValue = (unsigned short)values[i]; + mk_servo_set_value(i, scaling(values[i], 1010, 2100, 0, 1024)); + } + + return count * 2; +} + void MK::gpio_reset(void) { @@ -1229,10 +1274,10 @@ enum MappingMode { MAPPING_PX4, }; - enum FrameType { - FRAME_PLUS = 0, - FRAME_X, - }; +enum FrameType { + FRAME_PLUS = 0, + FRAME_X, +}; PortMode g_port_mode; @@ -1297,18 +1342,17 @@ mk_new_mode(PortMode new_mode, int update_rate, int motorcount, bool motortest, g_mk->set_motor_test(motortest); - /* (re)set count of used motors */ - ////g_mk->set_motor_count(motorcount); /* count used motors */ - do { - if(g_mk->mk_check_for_blctrl(8, false) != 0) { + if (g_mk->mk_check_for_blctrl(8, false) != 0) { shouldStop = 4; + } else { shouldStop++; } + sleep(1); - } while ( shouldStop < 3); + } while (shouldStop < 3); g_mk->set_motor_count(g_mk->mk_check_for_blctrl(8, true)); @@ -1375,7 +1419,8 @@ mkblctrl_main(int argc, char *argv[]) if (argc > i + 1) { bus = atoi(argv[i + 1]); newMode = true; - } else { + + } else { errx(1, "missing argument for i2c bus (-b)"); return 1; } @@ -1384,17 +1429,21 @@ mkblctrl_main(int argc, char *argv[]) /* look for the optional frame parameter */ if (strcmp(argv[i], "-mkmode") == 0 || strcmp(argv[i], "--mkmode") == 0) { if (argc > i + 1) { - if(strcmp(argv[i + 1], "+") == 0 || strcmp(argv[i + 1], "x") == 0 || strcmp(argv[i + 1], "X") == 0) { + if (strcmp(argv[i + 1], "+") == 0 || strcmp(argv[i + 1], "x") == 0 || strcmp(argv[i + 1], "X") == 0) { px4mode = MAPPING_MK; newMode = true; - if(strcmp(argv[i + 1], "+") == 0) { + + if (strcmp(argv[i + 1], "+") == 0) { frametype = FRAME_PLUS; + } else { frametype = FRAME_X; } + } else { errx(1, "only + or x for frametype supported !"); } + } else { errx(1, "missing argument for mkmode (-mkmode)"); return 1; @@ -1409,12 +1458,12 @@ mkblctrl_main(int argc, char *argv[]) /* look for the optional -h --help parameter */ if (strcmp(argv[i], "-h") == 0 || strcmp(argv[i], "--help") == 0) { - showHelp == true; + showHelp = true; } } - if(showHelp) { + if (showHelp) { fprintf(stderr, "mkblctrl: help:\n"); fprintf(stderr, " [-mkmode frame{+/x}] [-b i2c_bus_number] [-t motortest] [-h / --help]\n"); exit(1); @@ -1424,6 +1473,7 @@ mkblctrl_main(int argc, char *argv[]) if (g_mk == nullptr) { if (mk_start(bus, motorcount) != OK) { errx(1, "failed to start the MK-BLCtrl driver"); + } else { newMode = true; } diff --git a/src/examples/fixedwing_control/main.c b/src/examples/fixedwing_control/main.c index 6a9ad9e1d..89fbef020 100644 --- a/src/examples/fixedwing_control/main.c +++ b/src/examples/fixedwing_control/main.c @@ -33,10 +33,13 @@ ****************************************************************************/ /** * @file main.c - * Implementation of a fixed wing attitude controller. This file is a complete - * fixed wing controller flying manual attitude control or auto waypoint control. + * + * Example implementation of a fixed wing attitude controller. This file is a complete + * fixed wing controller for manual attitude control or auto waypoint control. * There is no need to touch any other system components to extend / modify the * complete control architecture. + * + * @author Lorenz Meier <lm@inf.ethz.ch> */ #include <nuttx/config.h> @@ -60,7 +63,6 @@ #include <uORB/topics/actuator_controls.h> #include <uORB/topics/vehicle_rates_setpoint.h> #include <uORB/topics/vehicle_global_position.h> -#include <uORB/topics/debug_key_value.h> #include <uORB/topics/parameter_update.h> #include <systemlib/param/param.h> #include <systemlib/pid/pid.h> @@ -73,8 +75,15 @@ #include "params.h" /* Prototypes */ + /** * Daemon management function. + * + * This function allows to start / stop the background task (daemon). + * The purpose of it is to be able to start the controller on the + * command line, query its status and stop it, without giving up + * the command line to one particular process or the need for bg/fg + * ^Z support by the shell. */ __EXPORT int ex_fixedwing_control_main(int argc, char *argv[]); @@ -88,10 +97,34 @@ int fixedwing_control_thread_main(int argc, char *argv[]); */ static void usage(const char *reason); +/** + * Control roll and pitch angle. + * + * This very simple roll and pitch controller takes the current roll angle + * of the system and compares it to a reference. Pitch is controlled to zero and yaw remains + * uncontrolled (tutorial code, not intended for flight). + * + * @param att_sp The current attitude setpoint - the values the system would like to reach. + * @param att The current attitude. The controller should make the attitude match the setpoint + * @param speed_body The velocity of the system. Currently unused. + * @param rates_sp The angular rate setpoint. This is the output of the controller. + */ void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att, - float speed_body[], float gyro[], struct vehicle_rates_setpoint_s *rates_sp, + float speed_body[], struct vehicle_rates_setpoint_s *rates_sp, struct actuator_controls_s *actuators); +/** + * Control heading. + * + * This very simple heading to roll angle controller outputs the desired roll angle based on + * the current position of the system, the desired position (the setpoint) and the current + * heading. + * + * @param pos The current position of the system + * @param sp The current position setpoint + * @param att The current attitude + * @param att_sp The attitude setpoint. This is the output of the controller + */ void control_heading(const struct vehicle_global_position_s *pos, const struct vehicle_global_position_setpoint_s *sp, const struct vehicle_attitude_s *att, struct vehicle_attitude_setpoint_s *att_sp); @@ -103,7 +136,7 @@ static struct params p; static struct param_handles ph; void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att, - float speed_body[], float gyro[], struct vehicle_rates_setpoint_s *rates_sp, + float speed_body[], struct vehicle_rates_setpoint_s *rates_sp, struct actuator_controls_s *actuators) { @@ -148,7 +181,10 @@ void control_heading(const struct vehicle_global_position_s *pos, const struct v * Calculate heading error of current position to desired position */ - /* PX4 uses 1e7 scaled integers to represent global coordinates for max resolution */ + /* + * PX4 uses 1e7 scaled integers to represent global coordinates for max resolution, + * so they need to be scaled by 1e7 and converted to IEEE double precision floating point. + */ float bearing = get_bearing_to_next_waypoint(pos->lat/1e7d, pos->lon/1e7d, sp->lat/1e7d, sp->lon/1e7d); /* calculate heading error */ @@ -157,10 +193,10 @@ void control_heading(const struct vehicle_global_position_s *pos, const struct v float roll_command = yaw_err * p.hdng_p; /* limit output, this commonly is a tuning parameter, too */ - if (att_sp->roll_body < -0.5f) { - att_sp->roll_body = -0.5f; - } else if (att_sp->roll_body > 0.5f) { - att_sp->roll_body = 0.5f; + if (att_sp->roll_body < -0.6f) { + att_sp->roll_body = -0.6f; + } else if (att_sp->roll_body > 0.6f) { + att_sp->roll_body = 0.6f; } } @@ -183,7 +219,32 @@ int fixedwing_control_thread_main(int argc, char *argv[]) parameters_init(&ph); parameters_update(&ph, &p); - /* declare and safely initialize all structs to zero */ + + /* + * PX4 uses a publish/subscribe design pattern to enable + * multi-threaded communication. + * + * The most elegant aspect of this is that controllers and + * other processes can either 'react' to new data, or run + * at their own pace. + * + * PX4 developer guide: + * https://pixhawk.ethz.ch/px4/dev/shared_object_communication + * + * Wikipedia description: + * http://en.wikipedia.org/wiki/Publish–subscribe_pattern + * + */ + + + + + /* + * Declare and safely initialize all structs to zero. + * + * These structs contain the system state and things + * like attitude, position, the current waypoint, etc. + */ struct vehicle_attitude_s att; memset(&att, 0, sizeof(att)); struct vehicle_attitude_setpoint_s att_sp; @@ -199,20 +260,24 @@ int fixedwing_control_thread_main(int argc, char *argv[]) struct vehicle_global_position_setpoint_s global_sp; memset(&global_sp, 0, sizeof(global_sp)); - /* output structs */ + /* output structs - this is what is sent to the mixer */ struct actuator_controls_s actuators; memset(&actuators, 0, sizeof(actuators)); - /* publish actuator controls */ + /* publish actuator controls with zero values */ for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) { actuators.control[i] = 0.0f; } + /* + * Advertise that this controller will publish actuator + * control values and the rate setpoint + */ orb_advert_t actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators); orb_advert_t rates_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp); - /* subscribe */ + /* subscribe to topics. */ int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); int att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); int global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); @@ -222,8 +287,9 @@ int fixedwing_control_thread_main(int argc, char *argv[]) int param_sub = orb_subscribe(ORB_ID(parameter_update)); /* Setup of loop */ - float gyro[3] = {0.0f, 0.0f, 0.0f}; float speed_body[3] = {0.0f, 0.0f, 0.0f}; + /* RC failsafe check */ + bool throttle_half_once = false; struct pollfd fds[2] = {{ .fd = param_sub, .events = POLLIN }, { .fd = att_sub, .events = POLLIN }}; @@ -242,7 +308,10 @@ int fixedwing_control_thread_main(int argc, char *argv[]) int ret = poll(fds, 2, 500); if (ret < 0) { - /* poll error, this will not really happen in practice */ + /* + * Poll error, this will not really happen in practice, + * but its good design practice to make output an error message. + */ warnx("poll error"); } else if (ret == 0) { @@ -268,6 +337,8 @@ int fixedwing_control_thread_main(int argc, char *argv[]) orb_check(global_pos_sub, &pos_updated); bool global_sp_updated; orb_check(global_sp_sub, &global_sp_updated); + bool manual_sp_updated; + orb_check(manual_sp_sub, &manual_sp_updated); /* get a local copy of attitude */ orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); @@ -275,6 +346,7 @@ int fixedwing_control_thread_main(int argc, char *argv[]) if (global_sp_updated) orb_copy(ORB_ID(vehicle_global_position_setpoint), global_sp_sub, &global_sp); + /* currently speed in body frame is not used, but here for reference */ if (pos_updated) { orb_copy(ORB_ID(vehicle_global_position), global_pos_sub, &global_pos); @@ -292,15 +364,23 @@ int fixedwing_control_thread_main(int argc, char *argv[]) } } - orb_copy(ORB_ID(manual_control_setpoint), manual_sp_sub, &manual_sp); - orb_copy(ORB_ID(vehicle_status), vstatus_sub, &vstatus); + if (manual_sp_updated) + /* get the RC (or otherwise user based) input */ + orb_copy(ORB_ID(manual_control_setpoint), manual_sp_sub, &manual_sp); - gyro[0] = att.rollspeed; - gyro[1] = att.pitchspeed; - gyro[2] = att.yawspeed; + /* check if the throttle was ever more than 50% - go later only to failsafe if yes */ + if (isfinite(manual_sp.throttle) && + (manual_sp.throttle >= 0.6f) && + (manual_sp.throttle <= 1.0f)) { + throttle_half_once = true; + } + + /* get the system status and the flight mode we're in */ + orb_copy(ORB_ID(vehicle_status), vstatus_sub, &vstatus); /* control */ + /* if in auto mode, fly global position setpoint */ if (vstatus.state_machine == SYSTEM_STATE_AUTO || vstatus.state_machine == SYSTEM_STATE_STABILIZED) { @@ -312,7 +392,7 @@ int fixedwing_control_thread_main(int argc, char *argv[]) actuators.control[2] = 0.0f; /* simple attitude control */ - control_attitude(&att_sp, &att, speed_body, gyro, &rates_sp, &actuators); + control_attitude(&att_sp, &att, speed_body, &rates_sp, &actuators); /* pass through throttle */ actuators.control[3] = att_sp.thrust; @@ -320,11 +400,12 @@ int fixedwing_control_thread_main(int argc, char *argv[]) /* set flaps to zero */ actuators.control[4] = 0.0f; + /* if in manual mode, decide between attitude stabilization (SAS) and full manual pass-through */ } else if (vstatus.state_machine == SYSTEM_STATE_MANUAL) { if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) { /* if the RC signal is lost, try to stay level and go slowly back down to ground */ - if (vstatus.rc_signal_lost) { + if (vstatus.rc_signal_lost && throttle_half_once) { /* put plane into loiter */ att_sp.roll_body = 0.3f; @@ -355,7 +436,7 @@ int fixedwing_control_thread_main(int argc, char *argv[]) att_sp.timestamp = hrt_absolute_time(); /* attitude control */ - control_attitude(&att_sp, &att, speed_body, gyro, &rates_sp, &actuators); + control_attitude(&att_sp, &att, speed_body, &rates_sp, &actuators); /* pass through throttle */ actuators.control[3] = att_sp.thrust; diff --git a/src/examples/fixedwing_control/params.c b/src/examples/fixedwing_control/params.c index 8042c74f5..c2e94ff3d 100644 --- a/src/examples/fixedwing_control/params.c +++ b/src/examples/fixedwing_control/params.c @@ -45,7 +45,7 @@ /** * */ -PARAM_DEFINE_FLOAT(EXFW_HDNG_P, 0.2f); +PARAM_DEFINE_FLOAT(EXFW_HDNG_P, 0.1f); /** * diff --git a/src/modules/gpio_led/gpio_led.c b/src/modules/gpio_led/gpio_led.c new file mode 100644 index 000000000..a80bf9cb8 --- /dev/null +++ b/src/modules/gpio_led/gpio_led.c @@ -0,0 +1,191 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Author: Anton Babushkin <anton.babushkin@me.com> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file gpio_led.c + * + * Status LED via GPIO driver. + * + * @author Anton Babushkin <anton.babushkin@me.com> + */ + +#include <stdio.h> +#include <stdlib.h> +#include <string.h> +#include <fcntl.h> +#include <stdbool.h> +#include <nuttx/wqueue.h> +#include <nuttx/clock.h> +#include <systemlib/systemlib.h> +#include <uORB/uORB.h> +#include <uORB/topics/vehicle_status.h> +#include <poll.h> +#include <drivers/drv_gpio.h> + +struct gpio_led_s { + struct work_s work; + int gpio_fd; + int pin; + struct vehicle_status_s status; + int vehicle_status_sub; + bool led_state; + int counter; +}; + +static struct gpio_led_s gpio_led_data; + +__EXPORT int gpio_led_main(int argc, char *argv[]); + +void gpio_led_start(FAR void *arg); + +void gpio_led_cycle(FAR void *arg); + +int gpio_led_main(int argc, char *argv[]) +{ + int pin = GPIO_EXT_1; + + if (argc > 1) { + if (!strcmp(argv[1], "-p")) { + if (!strcmp(argv[2], "1")) { + pin = GPIO_EXT_1; + + } else if (!strcmp(argv[2], "2")) { + pin = GPIO_EXT_2; + + } else { + printf("[gpio_led] Unsupported pin: %s\n", argv[2]); + exit(1); + } + } + } + + memset(&gpio_led_data, 0, sizeof(gpio_led_data)); + gpio_led_data.pin = pin; + int ret = work_queue(LPWORK, &gpio_led_data.work, gpio_led_start, &gpio_led_data, 0); + + if (ret != 0) { + printf("[gpio_led] Failed to queue work: %d\n", ret); + exit(1); + } + + exit(0); +} + +void gpio_led_start(FAR void *arg) +{ + FAR struct gpio_led_s *priv = (FAR struct gpio_led_s *)arg; + + /* open GPIO device */ + priv->gpio_fd = open(GPIO_DEVICE_PATH, 0); + + if (priv->gpio_fd < 0) { + printf("[gpio_led] GPIO: open fail\n"); + return; + } + + /* configure GPIO pin */ + ioctl(priv->gpio_fd, GPIO_SET_OUTPUT, priv->pin); + + /* subscribe to vehicle status topic */ + memset(&priv->status, 0, sizeof(priv->status)); + priv->vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); + + /* add worker to queue */ + int ret = work_queue(LPWORK, &priv->work, gpio_led_cycle, priv, 0); + + if (ret != 0) { + printf("[gpio_led] Failed to queue work: %d\n", ret); + return; + } + + printf("[gpio_led] Started, using pin GPIO_EXT%i\n", priv->pin); +} + +void gpio_led_cycle(FAR void *arg) +{ + FAR struct gpio_led_s *priv = (FAR struct gpio_led_s *)arg; + + /* check for status updates*/ + bool status_updated; + orb_check(priv->vehicle_status_sub, &status_updated); + + if (status_updated) + orb_copy(ORB_ID(vehicle_status), priv->vehicle_status_sub, &priv->status); + + /* select pattern for current status */ + int pattern = 0; + + if (priv->status.flag_system_armed) { + if (priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE) { + pattern = 0x3f; // ****** solid (armed) + + } else { + pattern = 0x2A; // *_*_*_ fast blink (armed, battery warning) + } + + } else { + if (priv->status.state_machine == SYSTEM_STATE_PREFLIGHT) { + pattern = 0x00; // ______ off (disarmed, preflight check) + + } else if (priv->status.state_machine == SYSTEM_STATE_STANDBY && + priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE) { + pattern = 0x38; // ***___ slow blink (disarmed, ready) + + } else { + pattern = 0x28; // *_*___ slow double blink (disarmed, not good to arm) + } + } + + /* blink pattern */ + bool led_state_new = (pattern & (1 << priv->counter)) != 0; + + if (led_state_new != priv->led_state) { + priv->led_state = led_state_new; + + if (led_state_new) { + ioctl(priv->gpio_fd, GPIO_SET, priv->pin); + + } else { + ioctl(priv->gpio_fd, GPIO_CLEAR, priv->pin); + } + } + + priv->counter++; + + if (priv->counter > 5) + priv->counter = 0; + + /* repeat cycle at 5 Hz*/ + work_queue(LPWORK, &priv->work, gpio_led_cycle, priv, USEC2TICK(200000)); +} diff --git a/src/modules/gpio_led/module.mk b/src/modules/gpio_led/module.mk new file mode 100644 index 000000000..3b8553489 --- /dev/null +++ b/src/modules/gpio_led/module.mk @@ -0,0 +1,39 @@ +############################################################################ +# +# Copyright (C) 2013 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# Status LED via GPIO driver +# + +MODULE_COMMAND = gpio_led +SRCS = gpio_led.c |