Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- Poznamky
- --------
- Python knihovna http://www.johannesbader.ch/2014/06/track-your-heartrate-on-raspberry-pi-with-ant/
- ANT protokol http://www.thisisant.com/ a http://www.thisisant.com/developer/resources/downloads#software_tab
- PWM http://abyz.co.uk/rpi/pigpio/
- video http://w.xuv.be/projects/raspi_video_loop
- video/audio soft /opt/vc/src/hello_pi/hello_video/
- regulátor /home/pi/programy/kolo/ a tam kolo-test.py
- musí běžet pigpiod
- musí existovat FIFO /tmp/video_speed
- všechno se spouští automaticky přes /etc/rc.local
- kolo-test.py
- ------------
- import sys
- import time
- import pigpio
- import serial
- from ant.core import driver
- from ant.core import event
- from ant.core.constants import *
- from ant.core.message import *
- NETKEY = '\xB9\xA5\x21\xFB\xBD\x72\xC3\x45'
- # Event callback
- class CallbackBlbaTrida(event.EventCallback):
- def __init__(self):
- self.speedFifo = open("/tmp/video_speed", "w", 0)
- self.old_rev_count = 0
- self.old_speed_event = 0
- self.circle = 6
- self.speed = 0.0
- self.otherSpeed = 0.0
- self.fan_min = 5.0
- self.fan_max = 40.0
- self.timeout = 0
- self.timesleep = 0
- self.nocount = 0
- self.pwm = 0
- self.pwm_min = 0.6
- def updateSpeeds(self):
- self.speedFifo.write(str(self.speed) + " " + str(self.otherSpeed) + "\n")
- def process(self, msg):
- if isinstance(msg, ChannelBroadcastDataMessage):
- new_speed_event = 0
- new_rev_count = 0
- speed_event = ord(msg.payload[5]) + ord(msg.payload[6]) * 256
- rev_count = ord(msg.payload[7]) + ord(msg.payload[8]) * 256
- # print "Bike speed event: ", speed_event
- # print "Rev count: ", rev_count
- new_speed_event = speed_event - self.old_speed_event
- if new_speed_event < 0:
- new_speed_event = 65536 + new_speed_event
- new_rev_count = rev_count - self.old_rev_count
- if new_rev_count < 0:
- new_rev_count = 65536 + new_rev_count
- self.old_rev_count = rev_count
- self.old_speed_event = speed_event
- if new_speed_event > 0:
- self.nocount = 0
- self.speed = (self.circle * float(new_rev_count) * 1024.0) / float(new_speed_event)
- if self.speed < 1:
- self.speed = 0
- # send to FIFO
- self.updateSpeeds()
- # print "Speed: ", self.speed, " pwm: ", self.pwm * 255.0
- else:
- # check N/A
- self.nocount = self.nocount + 1
- if self.nocount == 4:
- self.nocount = self.nocount - 1 # N/A report po jednom
- self.timeout = 1
- self.speed = 0
- self.updateSpeeds()
- # print "Speed: N/A"
- self.timesleep = 8
- # change PWM
- # meh
- callbackCosi = CallbackBlbaTrida()
- # serial
- serport = serial.Serial(port="/dev/ttyAMA0", baudrate=115200, parity=serial.PARITY_NONE, stopbits=serial.STOPBITS_ONE, bytesize=serial.EIGHTBITS)
- serport.open()
- serport.isOpen()
- serport.write("0\n")
- # GPIO PWM driver
- pi = pigpio.pi()
- pi.set_PWM_frequency(18, 100)
- pi.set_PWM_dutycycle(18, 0)
- # Initialize driver
- stick = driver.USB1Driver("/dev/ttyUSB0")
- stick.open()
- # Initialize event machine
- evm = event.EventMachine(stick)
- evm.registerCallback(callbackCosi)
- evm.start()
- # Reset
- msg = SystemResetMessage()
- stick.write(msg.encode())
- time.sleep(1)
- # Set network key
- msg = NetworkKeyMessage(key=NETKEY)
- stick.write(msg.encode())
- if evm.waitForAck(msg) != RESPONSE_NO_ERROR:
- sys.exit()
- # Initialize it as a receiving channel using our network key
- msg = ChannelAssignMessage()
- stick.write(msg.encode())
- if evm.waitForAck(msg) != RESPONSE_NO_ERROR:
- sys.exit()
- # Now set the channel id for pairing with an ANT+ bike cadence/speed sensor
- # device: 34946 nebo 56027
- msg = ChannelIDMessage(device_type=121, device_number=56027)
- stick.write(msg.encode())
- if evm.waitForAck(msg) != RESPONSE_NO_ERROR:
- sys.exit()
- # Listen forever and ever (not really, but for a long time)
- msg = ChannelSearchTimeoutMessage(timeout=255)
- stick.write(msg.encode())
- if evm.waitForAck(msg) != RESPONSE_NO_ERROR:
- sys.exit()
- # We want a ~4.05 Hz transmission period
- msg = ChannelPeriodMessage(period=8086)
- stick.write(msg.encode())
- if evm.waitForAck(msg) != RESPONSE_NO_ERROR:
- sys.exit()
- # And ANT frequency 57, of course
- msg = ChannelFrequencyMessage(frequency=57)
- stick.write(msg.encode())
- if evm.waitForAck(msg) != RESPONSE_NO_ERROR:
- sys.exit()
- # Time to go live
- msg = ChannelOpenMessage()
- stick.write(msg.encode())
- if evm.waitForAck(msg) != RESPONSE_NO_ERROR:
- sys.exit()
- # print "Listening for ANT events ..."
- sText = ""
- while True:
- try:
- # read serial
- while serport.inWaiting() > 0:
- serchar = serport.read(1)
- if serchar == '\n':
- if sText != "\n":
- callbackCosi.otherSpeed = float(sText)
- sText = ""
- # print "otherSpeed ", callbackCosi.otherSpeed
- else:
- sText += serchar
- # write serial
- serport.write(str(callbackCosi.speed) + "\n")
- # report timeout check
- if callbackCosi.timesleep > 0:
- callbackCosi.timesleep = callbackCosi.timesleep - 1
- else:
- callbackCosi.updateSpeeds()
- # PWM timeout check
- time.sleep(0.25)
- if callbackCosi.timeout > 0:
- callbackCosi.timeout = callbackCosi.timeout - 1
- if callbackCosi.timeout == 0:
- callbackCosi.speed = 0
- else:
- callbackCosi.updateSpeeds();
- callbackCosi.pwm = (((callbackCosi.speed + callbackCosi.otherSpeed) - callbackCosi.fan_min) / (callbackCosi.fan_max * 2)) + callbackCosi.pwm_min
- if callbackCosi.pwm > 1.0:
- callbackCosi.pwm = 1.0
- if callbackCosi.pwm < callbackCosi.pwm_min:
- callbackCosi.pwm = 0
- pi.set_PWM_dutycycle(18, callbackCosi.pwm * 255.0)
- except KeyboardInterrupt:
- print "... END"
- break
- except ValueError:
- callbackCosi.otherSpeed = 0
- # print "unknown otherSpeed '", sText, "'"
- sText = ""
- # Shutdown
- msg = SystemResetMessage()
- stick.write(msg.encode())
- time.sleep(1)
- evm.stop()
- stick.close()
- pi.set_PWM_dutycycle(18, 0)
- pi.set_PWM_frequency(18, 0)
- pi.stop()
- video.c
- -------
- /*
- Copyright (c) 2012, Broadcom Europe Ltd
- All rights reserved.
- Redistribution and use in source and binary forms, with or without
- modification, are permitted provided that the following conditions are met:
- * Redistributions of source code must retain the above copyright
- notice, this list of conditions and the following disclaimer.
- * 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.
- * Neither the name of the copyright holder 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 HOLDER 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.
- */
- // Video deocode demo using OpenMAX IL though the ilcient helper library
- #include <stdio.h>
- #include <stdlib.h>
- #include <string.h>
- #include <fcntl.h>
- #include <math.h>
- #include <pthread.h>
- #include <alsa/asoundlib.h>
- #include "bcm_host.h"
- #include "ilclient.h"
- #include "GLES/gl.h"
- #include "EGL/egl.h"
- #include "EGL/eglext.h"
- #define FIFO_NAME "/tmp/video_speed"
- #define VID_SPEED_MAX 22.0f
- #define AUD_SPEED_MAX 12.0f
- #define AUD_SPEED_MIN 2.0f
- // audio
- static char *device = "default";
- short buf1[8*1024];
- short buf2[8*1024];
- short bufmix[8*1024];
- // vlákna
- pthread_t speed_thread;
- // aktuální rychlost
- float actualSpeed = 0;
- float otherSpeed = 0;
- int speed_func(int pipe)
- {
- // toto je další vlákno
- // čte a "průměruje" rychlost kola
- // ukládá do 'actualSpeed'
- char temp;
- int pos;
- float readSpeed, readOther;
- char text[64];
- // čekat na změnu rychlosti
- pos = 0;
- while(1)
- {
- if(read(pipe, &temp, 1) == 1)
- {
- if(temp == '\n')
- {
- text[pos] = 0;
- readSpeed = 0;
- sscanf(text, "%f %f", &readSpeed, &readOther);
- if(readSpeed < 0)
- break;
- actualSpeed = (readSpeed + actualSpeed) / 3.0f;
- otherSpeed = (readOther + otherSpeed) / 3.0f;
- if(actualSpeed < 1.0f)
- actualSpeed = 0;
- if(otherSpeed < 1.0f)
- otherSpeed = 0;
- // printf("speed update %f -> %f; other %f\n", readSpeed, actualSpeed, otherSpeed);
- pos = 0;
- } else {
- text[pos] = temp;
- if(pos < sizeof(text) - 1)
- pos++;
- }
- }
- }
- actualSpeed = -1;
- return 0;
- }
- int sound_func(int nuffin)
- {
- // toto je další vlákno
- // stará se o audio
- // - hlasitost obou zvuků
- int i;
- int err;
- snd_pcm_t *handle;
- snd_pcm_sframes_t frames;
- FILE *f[2];
- short *bsrc1, *bsrc2, *btarg;
- float volume[2] = {0.4f, 0.0f};
- float volumeMod = 0.0f;
- float mix;
- f[0] = fopen("zvuk1.raw", "rb");
- if(!f[0])
- {
- printf("- failed to open 1st sound\n- audio disabled\n");
- return 1;
- }
- f[1] = fopen("zvuk2.raw", "rb");
- if(!f[1])
- {
- printf("- failed to open 2nd sound\n- audio disabled\n");
- fclose(f[0]);
- return 1;
- }
- if ((err = snd_pcm_open(&handle, device, SND_PCM_STREAM_PLAYBACK, 0)) < 0)
- {
- printf("Playback open error: %s\n", snd_strerror(err));
- return 1;
- }
- if ((err = snd_pcm_set_params(handle, SND_PCM_FORMAT_S16_LE, SND_PCM_ACCESS_RW_INTERLEAVED, 1, 48000, 1, 250000)) < 0)
- {
- printf("Playback open error: %s\n", snd_strerror(err));
- return 1;
- }
- while(actualSpeed >= 0)
- {
- // úprava hlasitosti
- /* if((actualSpeed + otherSpeed) > AUD_SPEED_MIN)
- volumeMod = -0.00001f; // postupně zeslabit
- else
- volumeMod = 0.00001f; // postupně zesílit
- */
- /* // druhý kanál - hlasitost dle rychlostí
- volume[1] = ((actualSpeed + otherSpeed) - 2.0f) / ((AUD_SPEED_MAX - 2.0f) * 2.0f);
- if(volume[1] < 0)
- volume[1] = 0;
- if(volume[1] > 1.0f)
- volume[1] = 1.0f;*/
- volume[0] = 10.0f - (actualSpeed + otherSpeed) * 0.55f;
- if(volume[0] < 0)
- volume[0] = 0;
- if(volume[0] > 1)
- volume[1] = 1;
- // load
- if(volume[0])
- {
- i = fread(buf1, 1, sizeof(buf1), f[0]);
- if(i < sizeof(buf1))
- {
- // konec souboru, načíst začátek
- fseek(f[0], 0, SEEK_SET);
- fread(buf1 + (i / sizeof(short)), 1, sizeof(buf1) - i, f[0]);
- }
- }
- /* if(volume[1])
- {
- i = fread(buf2, 1, sizeof(buf2), f[1]);
- if(i < sizeof(buf2))
- {
- // konec souboru, načíst začátek
- fseek(f[1], 0, SEEK_SET);
- fread(buf2 + (i / sizeof(short)), 1, sizeof(buf2) - i, f[1]);
- }
- }*/
- // MIX
- bsrc1 = buf1;
- bsrc2 = buf2;
- btarg = bufmix;
- for(i = 0; i < sizeof(bufmix)/sizeof(short); i++)
- {
- mix = (float)*bsrc1 * volume[0] + (float)*bsrc2 * volume[1];
- if(mix < -32768.0f)
- mix = -32768.0f;
- if(mix > 32767.0f)
- mix = 32767.0f;
- *btarg = (short)mix;
- volume[0] += volumeMod;
- if(volumeMod > 0)
- {
- if(volume[0] > 0.4f)
- volume[0] = 0.4f;
- } else {
- if(volume[0] < 0.0f)
- volume[0] = 0.0f;
- }
- bsrc1++;
- bsrc2++;
- btarg++;
- }
- // play
- frames = snd_pcm_writei(handle, bufmix, sizeof(bufmix)/sizeof(short));
- if (frames < 0)
- frames = snd_pcm_recover(handle, frames, 0);
- if (frames < 0)
- {
- printf("snd_pcm_writei failed: %s\n", snd_strerror(err));
- break;
- }
- }
- fclose(f[0]);
- fclose(f[1]);
- return 0;
- }
- static int video_decode_test(char *filename)
- {
- OMX_VIDEO_PARAM_PORTFORMATTYPE format;
- OMX_TIME_CONFIG_CLOCKSTATETYPE cstate;
- COMPONENT_T *video_decode = NULL, *video_scheduler = NULL, *video_render = NULL, *clock = NULL;
- COMPONENT_T *list[5];
- TUNNEL_T tunnel[4];
- ILCLIENT_T *client;
- FILE *in;
- int status = 0;
- unsigned int data_len = 0;
- OMX_TIME_CONFIG_SCALETYPE scaleType;
- int speed_pipe;
- float videoSpeed, oldSpeed;
- // vlákno rychlosti
- speed_pipe = open(FIFO_NAME, O_RDONLY);
- if(speed_pipe < 0)
- {
- printf("- failed to open named pipe %s\n", FIFO_NAME);
- actualSpeed = VID_SPEED_MAX / 2.0f;
- } else
- pthread_create(&speed_thread, NULL, (void*)speed_func, (void*)speed_pipe);
- // vlákno
- pthread_create(&speed_thread, NULL, (void*)sound_func, NULL);
- // video stuff
- memset(&scaleType, 0, sizeof(scaleType));
- scaleType.nSize = sizeof(scaleType);
- scaleType.nVersion.nVersion = OMX_VERSION;
- memset(list, 0, sizeof(list));
- memset(tunnel, 0, sizeof(tunnel));
- if((in = fopen(filename, "rb")) == NULL)
- return -2;
- if((client = ilclient_init()) == NULL)
- {
- fclose(in);
- return -3;
- }
- if(OMX_Init() != OMX_ErrorNone)
- {
- ilclient_destroy(client);
- fclose(in);
- return -4;
- }
- // create video_decode
- if(ilclient_create_component(client, &video_decode, "video_decode", ILCLIENT_DISABLE_ALL_PORTS | ILCLIENT_ENABLE_INPUT_BUFFERS) != 0)
- status = -14;
- list[0] = video_decode;
- // create video_render
- if(status == 0 && ilclient_create_component(client, &video_render, "video_render", ILCLIENT_DISABLE_ALL_PORTS) != 0)
- status = -14;
- list[1] = video_render;
- // create clock
- if(status == 0 && ilclient_create_component(client, &clock, "clock", ILCLIENT_DISABLE_ALL_PORTS) != 0)
- status = -14;
- list[2] = clock;
- memset(&cstate, 0, sizeof(cstate));
- cstate.nSize = sizeof(cstate);
- cstate.nVersion.nVersion = OMX_VERSION;
- cstate.eState = OMX_TIME_ClockStateWaitingForStartTime;
- cstate.nWaitMask = 1;
- if(clock != NULL && OMX_SetParameter(ILC_GET_HANDLE(clock), OMX_IndexConfigTimeClockState, &cstate) != OMX_ErrorNone)
- status = -13;
- // create video_scheduler
- if(status == 0 && ilclient_create_component(client, &video_scheduler, "video_scheduler", ILCLIENT_DISABLE_ALL_PORTS) != 0)
- status = -14;
- list[3] = video_scheduler;
- set_tunnel(tunnel, video_decode, 131, video_scheduler, 10);
- set_tunnel(tunnel+1, video_scheduler, 11, video_render, 90);
- set_tunnel(tunnel+2, clock, 80, video_scheduler, 12);
- // setup clock tunnel first
- if(status == 0 && ilclient_setup_tunnel(tunnel+2, 0, 0) != 0)
- status = -15;
- else
- ilclient_change_component_state(clock, OMX_StateExecuting);
- if(status == 0)
- ilclient_change_component_state(video_decode, OMX_StateIdle);
- memset(&format, 0, sizeof(OMX_VIDEO_PARAM_PORTFORMATTYPE));
- format.nSize = sizeof(OMX_VIDEO_PARAM_PORTFORMATTYPE);
- format.nVersion.nVersion = OMX_VERSION;
- format.nPortIndex = 130;
- format.eCompressionFormat = OMX_VIDEO_CodingAVC;
- if(status == 0 &&
- OMX_SetParameter(ILC_GET_HANDLE(video_decode), OMX_IndexParamVideoPortFormat, &format) == OMX_ErrorNone &&
- ilclient_enable_port_buffers(video_decode, 130, NULL, NULL, NULL) == 0)
- {
- OMX_BUFFERHEADERTYPE *buf;
- int port_settings_changed = 0;
- int first_packet = 1;
- ilclient_change_component_state(video_decode, OMX_StateExecuting);
- while((buf = ilclient_get_input_buffer(video_decode, 130, 1)) != NULL)
- {
- // feed data and wait until we get port settings changed
- unsigned char *dest = buf->pBuffer;
- data_len += fread(dest, 1, buf->nAllocLen-data_len, in);
- if(port_settings_changed == 0 &&
- ((data_len > 0 && ilclient_remove_event(video_decode, OMX_EventPortSettingsChanged, 131, 0, 0, 1) == 0) ||
- (data_len == 0 && ilclient_wait_for_event(video_decode, OMX_EventPortSettingsChanged, 131, 0, 0, 1, ILCLIENT_EVENT_ERROR | ILCLIENT_PARAMETER_CHANGED, 10000) == 0)))
- {
- port_settings_changed = 1;
- if(ilclient_setup_tunnel(tunnel, 0, 0) != 0)
- {
- status = -7;
- break;
- }
- ilclient_change_component_state(video_scheduler, OMX_StateExecuting);
- // now setup tunnel to video_render
- if(ilclient_setup_tunnel(tunnel+1, 0, 1000) != 0)
- {
- status = -12;
- break;
- }
- ilclient_change_component_state(video_render, OMX_StateExecuting);
- }
- if(!data_len)
- fseek(in, 0, SEEK_SET);
- // speed change
- if(actualSpeed >= 0)
- {
- videoSpeed = actualSpeed / (VID_SPEED_MAX/2.0f);
- if(videoSpeed > 2.0f)
- videoSpeed = 2.0f;
- if(videoSpeed < 0.22f)
- {
- // printf("- paused\n");
- // PAUZA
- scaleType.xScale = 0;
- OMX_SetParameter(ILC_GET_HANDLE(clock), OMX_IndexConfigTimeScale, &scaleType);
- while(1)
- {
- usleep(100*1000);
- videoSpeed = actualSpeed / (VID_SPEED_MAX/2.0f);
- if(videoSpeed > 0.25f)
- break;
- }
- oldSpeed = 0;
- if(videoSpeed > 2.0f)
- videoSpeed = 2.0f;
- // konec pauzy
- // printf("- unpaused\n");
- }
- videoSpeed = floor(videoSpeed * 100.0f) / 100.0f; // limit desetiných míst
- if(videoSpeed != oldSpeed)
- {
- // printf("- speed change to %f; video speed %f\n", actualSpeed, videoSpeed);
- scaleType.xScale = floor((videoSpeed * pow(2,16)));
- OMX_SetParameter(ILC_GET_HANDLE(clock), OMX_IndexConfigTimeScale, &scaleType);
- oldSpeed = videoSpeed;
- }
- } else
- // konec přehrávání videa
- break;
- buf->nFilledLen = data_len;
- data_len = 0;
- buf->nOffset = 0;
- if(first_packet)
- {
- buf->nFlags = OMX_BUFFERFLAG_STARTTIME;
- first_packet = 0;
- }
- else
- buf->nFlags = OMX_BUFFERFLAG_TIME_UNKNOWN;
- if(OMX_EmptyThisBuffer(ILC_GET_HANDLE(video_decode), buf) != OMX_ErrorNone)
- {
- status = -6;
- break;
- }
- }
- buf->nFilledLen = 0;
- buf->nFlags = OMX_BUFFERFLAG_TIME_UNKNOWN | OMX_BUFFERFLAG_EOS;
- if(OMX_EmptyThisBuffer(ILC_GET_HANDLE(video_decode), buf) != OMX_ErrorNone)
- status = -20;
- // wait for EOS from render
- ilclient_wait_for_event(video_render, OMX_EventBufferFlag, 90, 0, OMX_BUFFERFLAG_EOS, 0,
- ILCLIENT_BUFFER_FLAG_EOS, 10000);
- // need to flush the renderer to allow video_decode to disable its input port
- ilclient_flush_tunnels(tunnel, 0);
- ilclient_disable_port_buffers(video_decode, 130, NULL, NULL, NULL);
- }
- fclose(in);
- ilclient_disable_tunnel(tunnel);
- ilclient_disable_tunnel(tunnel+1);
- ilclient_disable_tunnel(tunnel+2);
- ilclient_teardown_tunnels(tunnel);
- ilclient_state_transition(list, OMX_StateIdle);
- ilclient_state_transition(list, OMX_StateLoaded);
- ilclient_cleanup_components(list);
- OMX_Deinit();
- ilclient_destroy(client);
- close(speed_pipe);
- sleep(1);
- return status;
- }
- int main (int argc, char **argv)
- {
- printf("Variable speed looping hello_video mod\n");
- if (argc < 2) {
- printf("Usage: %s <filename>\n", argv[0]);
- exit(1);
- }
- bcm_host_init();
- return video_decode_test(argv[1]);
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement